diff --git a/MIGRATION.md b/MIGRATION.md index 3aa22d5f05..b2d6977bc7 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,7 @@ API changes in MoveIt releases ## ROS Rolling +- [11/2024] All MoveIt 2 headers have been updated to use the .hpp extension. .h headers are now autogenerated with a deprecation warning, and may be removed in future releases. Please update your imports to use the .hpp headers. - [11/2024] Added flags to control padding to CollisionRequest. This change deprecates PlanningScene::checkCollisionUnpadded(..) functions. Please use PlanningScene::checkCollision(..) with a req.pad_environment_collisions = false; - [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse. diff --git a/moveit/scripts/create_deprecated_headers.py b/moveit/scripts/create_deprecated_headers.py new file mode 100755 index 0000000000..0680a081d9 --- /dev/null +++ b/moveit/scripts/create_deprecated_headers.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2024 Tom Noble. +# +# 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 HOLDER 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: Tom Noble + +import sys +import argparse +import logging +from typing import List, Tuple +from pathlib import Path + + +DISCLAIMER = """ +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via {}, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +""" + + +class NoIncludeGuard(Exception): + ERROR = "No include guard found in {}.hpp. Unable to generate pretext." + + def __init__(self, file: Path): + super().__init__(self.ERROR.format(file)) + + +class NoIncludeDirectory(Exception): + ERROR = "No include directory found for {}.hpp. Unable to generate relative .hpp include" + + def __init__(self, file: Path): + super().__init__(self.ERROR.format(file)) + + +class HppFile: + def __init__(self, path: Path): + self.path = path + self.guard = "#pragma once" + self.pretext = self.pretext() + self.include = self.include() + + def drop_data_after(self, data: str, match: str): + return data[: data.find(match) + len(match)] + + def read(self) -> str: + data = open(self.path, "r").read() + contains_guard = self.guard in data + if not contains_guard: + raise NoIncludeGuard(self.path) + return data + + def pretext(self) -> str: + data = self.read() + return self.drop_data_after(data, self.guard) + + def include(self) -> str: + ends_with_include = lambda p: str(p).endswith("include") + include_paths = [p for p in self.path.parents if ends_with_include(p)] + if not include_paths: + raise NoIncludeDirectory(self.path) + relative_import = self.path.relative_to(include_paths[0]) + return f"#include <{relative_import}>" + + +class DeprecatedHeader: + def __init__(self, hpp: HppFile): + self.hpp = hpp + self.path = hpp.path.with_suffix(".h") + self.warn = '#pragma message(".h header is obsolete. Please use the .hpp header instead.")' + self.contents = self.contents() + + def contents(self) -> str: + disclaimer = DISCLAIMER.format(Path(__file__).name).rstrip("\n") + items = [disclaimer, self.hpp.pretext, self.warn, self.hpp.include] + return "\n".join(items) + "\n" + + +class HeaderSummary: + def __init__(self, n_processed_hpps: int, bad_hpps: List[str]): + self.n_processed_hpps = n_processed_hpps + self.bad_hpps = bad_hpps + + def were_all_hpps_processed(self) -> bool: + return len(self.bad_hpps) == 0 + + def __repr__(self) -> str: + summary = f"Can generate {self.n_processed_hpps} .h files." + if self.bad_hpps: + summary += f" Cannot generate {len(self.bad_hpps)} .h files:\n\n" + summary += "\n".join([f"❌ {hpp}" for hpp in self.bad_hpps]) + summary += "\n" + return summary + + +class DeprecatedHeaderGenerator: + def __init__(self, hpp_paths: List[str]): + self.hpp_paths = hpp_paths + self.processed_hpps = [] + self.bad_hpps = [] + + def __process_hpp(self, hpp: str) -> None: + try: + self.processed_hpps.append(HppFile(hpp)) + except (NoIncludeDirectory, NoIncludeGuard) as e: + self.bad_hpps.append(str(hpp)) + + def process_all_hpps(self) -> HeaderSummary: + print(f"\nProcessing {len(self.hpp_paths)} .hpp files...") + _ = [self.__process_hpp(hpp) for hpp in self.hpp_paths] + return HeaderSummary(len(self.processed_hpps), self.bad_hpps) + + def create_h_files(self) -> None: + print(f"Proceeding to generate {len(self.processed_hpps)} .h files...") + h_files = [DeprecatedHeader(hpp) for hpp in self.processed_hpps] + _ = [open(h.path, "w").write(h.contents) for h in h_files] + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + # TODO: Add argument for skipping private headers + parser.add_argument("--apply", action="store_true", help="Generates the .h files") + args = parser.parse_args() + generator = DeprecatedHeaderGenerator(list(Path.cwd().rglob("*.hpp"))) + summary = generator.process_all_hpps() + print(summary) + if args.apply and not summary.were_all_hpps_processed(): + args.apply = input("Continue? (y/n): ").lower() == "y" + if args.apply: + generator.create_h_files() + else: + print("Skipping file generation...") + print("Done.\n") 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 316497e9cf..105a3d5d38 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,19 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan */ #pragma once - -#include -#include - -#include - -namespace collision_detection -{ -/** \brief An allocator for AllValid collision detectors */ -class MOVEIT_COLLISION_DETECTION_EXPORT CollisionDetectorAllocatorAllValid - : public CollisionDetectorAllocatorTemplate -{ -public: - static const std::string NAME; // defined in collision_env_allvalid.cpp -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.hpp new file mode 100644 index 0000000000..e0c2013d43 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Acorn Pooley, Ioan Sucan */ + +#pragma once + +#include +#include + +#include + +namespace collision_detection +{ +/** \brief An allocator for AllValid collision detectors */ +class MOVEIT_COLLISION_DETECTION_EXPORT CollisionDetectorAllocatorAllValid + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_env_allvalid.cpp +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h index 50b543d837..727a3fa794 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,42 +47,5 @@ /* Author: Ioan Sucan, Jia Pan, Jens Petit */ #pragma once - -#include - -namespace collision_detection -{ -/** \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: - CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); - CollisionEnvAllValid(const moveit::core::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 moveit::core::RobotState& state) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - virtual double distanceRobot(const moveit::core::RobotState& state) const; - virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; - void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.hpp new file mode 100644 index 0000000000..7615ca55ba --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * 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, Jens Petit */ + +#pragma once + +#include + +namespace collision_detection +{ +/** \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: + CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0); + CollisionEnvAllValid(const moveit::core::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 moveit::core::RobotState& state) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + virtual double distanceRobot(const moveit::core::RobotState& state) const; + virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; +}; +} // namespace collision_detection 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 0fcd4f9808..92a6034878 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,337 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc - -/** \brief The types of bodies that are considered for collision */ -namespace BodyTypes -{ -/** \brief The types of bodies that are considered for collision */ -enum Type -{ - /** \brief A link on the robot */ - ROBOT_LINK, - - /** \brief A body attached to a robot link */ - ROBOT_ATTACHED, - - /** \brief A body in the environment */ - WORLD_OBJECT -}; -} // namespace BodyTypes - -/** \brief The types of bodies that are considered for collision */ -using BodyType = BodyTypes::Type; - -/** \brief Definition of a contact point */ -struct Contact -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief contact position */ - Eigen::Vector3d pos; - - /** \brief normal unit vector at contact */ - Eigen::Vector3d normal; - - /** \brief depth (penetration between bodies) */ - double depth = 0.0; - - /** \brief The id of the first body involved in the contact */ - std::string body_name_1; - - /** \brief The type of the first body involved in the contact */ - BodyType body_type_1; - - /** \brief The id of the second body involved in the contact */ - std::string body_name_2; - - /** \brief The type of the second body involved in the contact */ - BodyType body_type_2; - - /** \brief The distance percentage between casted poses until collision. - * - * If the value is 0, then the collision occurred in the start pose. If the value is 1, then the collision occurred - * in the end pose. */ - double percent_interpolation = 0.0; - - /** \brief The two nearest points connecting the two bodies */ - Eigen::Vector3d nearest_points[2]; -}; - -/** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a - * particular volume */ -struct CostSource -{ - /// The minimum bound of the AABB defining the volume responsible for this partial cost - std::array aabb_min; - - /// The maximum bound of the AABB defining the volume responsible for this partial cost - std::array aabb_max; - - /// The partial cost (the probability of existence for the object there is a collision with) - double cost = 0.0; - - /// Get the volume of the AABB around the cost source - double getVolume() const - { - return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); - } - - /// Order cost sources so that the most costly source is at the top - bool operator<(const CostSource& other) const - { - double c1 = cost * getVolume(); - double c2 = other.cost * other.getVolume(); - if (c1 > c2) - return true; - if (c1 < c2) - return false; - if (cost < other.cost) - return false; - if (cost > other.cost) - return true; - return aabb_min < other.aabb_min; - } -}; - -struct CollisionResult; - -/** \brief Representation of a collision checking request */ -struct CollisionRequest -{ - /** \brief The group name to check collisions for (optional; if empty, assume the complete robot). Descendent links - * are included. */ - std::string group_name = ""; - - /** \brief If true, use padded collision environment */ - bool pad_environment_collisions = true; - - /** \brief If true, do self collision check with padded robot links */ - bool pad_self_collisions = false; - - /** \brief If true, compute proximity distance */ - bool distance = false; - - /** \brief If true, return detailed distance information. Distance must be set to true as well */ - bool detailed_distance = false; - - /** \brief If true, a collision cost is computed */ - bool cost = false; - - /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */ - bool contacts = false; - - /** \brief Overall maximum number of contacts to compute */ - std::size_t max_contacts = 1; - - /** \brief Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at different - * configurations) */ - std::size_t max_contacts_per_pair = 1; - - /** \brief When costs are computed, this value defines how many of the top cost sources should be returned */ - std::size_t max_cost_sources = 1; - - /** \brief Function call that decides whether collision detection should stop. */ - std::function is_done = nullptr; - - /** \brief Flag indicating whether information about detected collisions should be reported */ - bool verbose = false; -}; - -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 -}; -} -using DistanceRequestType = DistanceRequestTypes::DistanceRequestType; - -/** \brief Representation of a distance-reporting request */ -struct DistanceRequest -{ - /*** \brief Compute \e active_components_only_ based on \e req_. This - includes links that are in the kinematic model but outside this group, if those links are descendants of - joints in this group that have their values updated. */ - void enableGroup(const moveit::core::RobotModelConstPtr& robot_model) - { - if (robot_model->hasJointModelGroup(group_name)) - { - active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet(); - } - else - { - active_components_only = nullptr; - } - } - - /// Indicate if nearest point information should be calculated - bool enable_nearest_points = false; - - /// Indicate if a signed distance should be calculated in a collision. - bool enable_signed_distance = false; - - /// 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 because it is used to reserve space. - DistanceRequestType type = DistanceRequestType::GLOBAL; - - /// Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) - std::size_t max_contacts_per_body = 1; - - /// The group name - std::string group_name = ""; - - /// The set of active components to check - const std::set* active_components_only = nullptr; - - /// The allowed collision matrix used to filter checks - const AllowedCollisionMatrix* acm = nullptr; - - /// Only calculate distances for objects within this threshold to each other. - /// If set, this can significantly reduce the number of queries. - double distance_threshold = std::numeric_limits::max(); - - /// Log debug information - bool verbose = false; - - /// Indicate if gradient should be calculated between each object. - /// This is the normalized vector connecting the closest points on the two objects. - bool compute_gradient = false; -}; - -/** \brief Generic representation of the distance information for a pair of objects */ -// TODO(#267): Enable check - for some reason clang-tidy wants to rename this struct to "i0" -struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning -{ - DistanceResultsData() - { - clear(); - } - - /// The distance between two objects. If two objects are in collision, distance <= 0. - double distance; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// The nearest points - Eigen::Vector3d nearest_points[2]; - - /// The object link names - std::string link_names[2]; - - /// The object body type - BodyType body_types[2]; - - /** Normalized vector connecting closest points (from link_names[0] to link_names[1]) - - Usually, when checking convex to convex, the normal is connecting closest points. - However, FCL in case of non-convex to non-convex or convex to non-convex returns - the contact normal for one of the two triangles that are in contact. */ - Eigen::Vector3d normal; - - /// Clear structure data - void clear() - { - distance = std::numeric_limits::max(); - nearest_points[0].setZero(); - nearest_points[1].setZero(); - body_types[0] = BodyType::WORLD_OBJECT; - body_types[1] = BodyType::WORLD_OBJECT; - link_names[0] = ""; - link_names[1] = ""; - normal.setZero(); - } - - /// Compare if the distance is less than another - bool operator<(const DistanceResultsData& other) - { - return (distance < other.distance); - } - - /// Compare if the distance is greater than another - bool operator>(const DistanceResultsData& other) - { - return (distance > other.distance); - } -}; - -/** \brief Mapping between the names of the collision objects and the DistanceResultData. */ -using DistanceMap = std::map, std::vector >; - -/** \brief Result of a distance request. */ -struct DistanceResult -{ - /// Indicates if two objects were in collision - bool collision = false; - - /// ResultsData for the two objects with the minimum distance - DistanceResultsData minimum_distance; - - /// A map of distance data for each link in the req.active_components_only - DistanceMap distances; - - /// Clear structure data - void clear() - { - collision = false; - minimum_distance.clear(); - distances.clear(); - } -}; - -/** \brief Representation of a collision checking result */ -struct CollisionResult -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Clear a previously stored result */ - void clear() - { - collision = false; - distance = std::numeric_limits::max(); - distance_result.clear(); - contact_count = 0; - contacts.clear(); - cost_sources.clear(); - } - - /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ - void print() const; - - /** \brief True if collision was found, false otherwise */ - bool collision = false; - - /** \brief Closest distance between two bodies */ - double distance = std::numeric_limits::max(); - - /** \brief Distance data for each link */ - DistanceResult distance_result; - - /** \brief Number of contacts returned */ - std::size_t contact_count = 0; - - /** \brief A map returning the pairs of body ids in contact, plus their contact details */ - using ContactMap = std::map, std::vector >; - ContactMap contacts; - - /** \brief These are the individual cost sources when costs are computed */ - std::set cost_sources; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.hpp new file mode 100644 index 0000000000..19777a044f --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.hpp @@ -0,0 +1,371 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc + +/** \brief The types of bodies that are considered for collision */ +namespace BodyTypes +{ +/** \brief The types of bodies that are considered for collision */ +enum Type +{ + /** \brief A link on the robot */ + ROBOT_LINK, + + /** \brief A body attached to a robot link */ + ROBOT_ATTACHED, + + /** \brief A body in the environment */ + WORLD_OBJECT +}; +} // namespace BodyTypes + +/** \brief The types of bodies that are considered for collision */ +using BodyType = BodyTypes::Type; + +/** \brief Definition of a contact point */ +struct Contact +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief contact position */ + Eigen::Vector3d pos; + + /** \brief normal unit vector at contact */ + Eigen::Vector3d normal; + + /** \brief depth (penetration between bodies) */ + double depth = 0.0; + + /** \brief The id of the first body involved in the contact */ + std::string body_name_1; + + /** \brief The type of the first body involved in the contact */ + BodyType body_type_1; + + /** \brief The id of the second body involved in the contact */ + std::string body_name_2; + + /** \brief The type of the second body involved in the contact */ + BodyType body_type_2; + + /** \brief The distance percentage between casted poses until collision. + * + * If the value is 0, then the collision occurred in the start pose. If the value is 1, then the collision occurred + * in the end pose. */ + double percent_interpolation = 0.0; + + /** \brief The two nearest points connecting the two bodies */ + Eigen::Vector3d nearest_points[2]; +}; + +/** \brief When collision costs are computed, this structure contains information about the partial cost incurred in a + * particular volume */ +struct CostSource +{ + /// The minimum bound of the AABB defining the volume responsible for this partial cost + std::array aabb_min; + + /// The maximum bound of the AABB defining the volume responsible for this partial cost + std::array aabb_max; + + /// The partial cost (the probability of existence for the object there is a collision with) + double cost = 0.0; + + /// Get the volume of the AABB around the cost source + double getVolume() const + { + return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); + } + + /// Order cost sources so that the most costly source is at the top + bool operator<(const CostSource& other) const + { + double c1 = cost * getVolume(); + double c2 = other.cost * other.getVolume(); + if (c1 > c2) + return true; + if (c1 < c2) + return false; + if (cost < other.cost) + return false; + if (cost > other.cost) + return true; + return aabb_min < other.aabb_min; + } +}; + +struct CollisionResult; + +/** \brief Representation of a collision checking request */ +struct CollisionRequest +{ + /** \brief The group name to check collisions for (optional; if empty, assume the complete robot). Descendent links + * are included. */ + std::string group_name = ""; + + /** \brief If true, use padded collision environment */ + bool pad_environment_collisions = true; + + /** \brief If true, do self collision check with padded robot links */ + bool pad_self_collisions = false; + + /** \brief If true, compute proximity distance */ + bool distance = false; + + /** \brief If true, return detailed distance information. Distance must be set to true as well */ + bool detailed_distance = false; + + /** \brief If true, a collision cost is computed */ + bool cost = false; + + /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */ + bool contacts = false; + + /** \brief Overall maximum number of contacts to compute */ + std::size_t max_contacts = 1; + + /** \brief Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at different + * configurations) */ + std::size_t max_contacts_per_pair = 1; + + /** \brief When costs are computed, this value defines how many of the top cost sources should be returned */ + std::size_t max_cost_sources = 1; + + /** \brief Function call that decides whether collision detection should stop. */ + std::function is_done = nullptr; + + /** \brief Flag indicating whether information about detected collisions should be reported */ + bool verbose = false; +}; + +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 +}; +} +using DistanceRequestType = DistanceRequestTypes::DistanceRequestType; + +/** \brief Representation of a distance-reporting request */ +struct DistanceRequest +{ + /*** \brief Compute \e active_components_only_ based on \e req_. This + includes links that are in the kinematic model but outside this group, if those links are descendants of + joints in this group that have their values updated. */ + void enableGroup(const moveit::core::RobotModelConstPtr& robot_model) + { + if (robot_model->hasJointModelGroup(group_name)) + { + active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet(); + } + else + { + active_components_only = nullptr; + } + } + + /// Indicate if nearest point information should be calculated + bool enable_nearest_points = false; + + /// Indicate if a signed distance should be calculated in a collision. + bool enable_signed_distance = false; + + /// 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 because it is used to reserve space. + DistanceRequestType type = DistanceRequestType::GLOBAL; + + /// Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) + std::size_t max_contacts_per_body = 1; + + /// The group name + std::string group_name = ""; + + /// The set of active components to check + const std::set* active_components_only = nullptr; + + /// The allowed collision matrix used to filter checks + const AllowedCollisionMatrix* acm = nullptr; + + /// Only calculate distances for objects within this threshold to each other. + /// If set, this can significantly reduce the number of queries. + double distance_threshold = std::numeric_limits::max(); + + /// Log debug information + bool verbose = false; + + /// Indicate if gradient should be calculated between each object. + /// This is the normalized vector connecting the closest points on the two objects. + bool compute_gradient = false; +}; + +/** \brief Generic representation of the distance information for a pair of objects */ +// TODO(#267): Enable check - for some reason clang-tidy wants to rename this struct to "i0" +struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning +{ + DistanceResultsData() + { + clear(); + } + + /// The distance between two objects. If two objects are in collision, distance <= 0. + double distance; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// The nearest points + Eigen::Vector3d nearest_points[2]; + + /// The object link names + std::string link_names[2]; + + /// The object body type + BodyType body_types[2]; + + /** Normalized vector connecting closest points (from link_names[0] to link_names[1]) + + Usually, when checking convex to convex, the normal is connecting closest points. + However, FCL in case of non-convex to non-convex or convex to non-convex returns + the contact normal for one of the two triangles that are in contact. */ + Eigen::Vector3d normal; + + /// Clear structure data + void clear() + { + distance = std::numeric_limits::max(); + nearest_points[0].setZero(); + nearest_points[1].setZero(); + body_types[0] = BodyType::WORLD_OBJECT; + body_types[1] = BodyType::WORLD_OBJECT; + link_names[0] = ""; + link_names[1] = ""; + normal.setZero(); + } + + /// Compare if the distance is less than another + bool operator<(const DistanceResultsData& other) + { + return (distance < other.distance); + } + + /// Compare if the distance is greater than another + bool operator>(const DistanceResultsData& other) + { + return (distance > other.distance); + } +}; + +/** \brief Mapping between the names of the collision objects and the DistanceResultData. */ +using DistanceMap = std::map, std::vector >; + +/** \brief Result of a distance request. */ +struct DistanceResult +{ + /// Indicates if two objects were in collision + bool collision = false; + + /// ResultsData for the two objects with the minimum distance + DistanceResultsData minimum_distance; + + /// A map of distance data for each link in the req.active_components_only + DistanceMap distances; + + /// Clear structure data + void clear() + { + collision = false; + minimum_distance.clear(); + distances.clear(); + } +}; + +/** \brief Representation of a collision checking result */ +struct CollisionResult +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Clear a previously stored result */ + void clear() + { + collision = false; + distance = std::numeric_limits::max(); + distance_result.clear(); + contact_count = 0; + contacts.clear(); + cost_sources.clear(); + } + + /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ + void print() const; + + /** \brief True if collision was found, false otherwise */ + bool collision = false; + + /** \brief Closest distance between two bodies */ + double distance = std::numeric_limits::max(); + + /** \brief Distance data for each link */ + DistanceResult distance_result; + + /** \brief Number of contacts returned */ + std::size_t contact_count = 0; + + /** \brief A map returning the pairs of body ids in contact, plus their contact details */ + using ContactMap = std::map, std::vector >; + ContactMap contacts; + + /** \brief These are the individual cost sources when costs are computed */ + std::set cost_sources; +}; +} // namespace collision_detection 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 d63bb5aaff..113eff6e43 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,67 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan */ #pragma once - -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator); // Defines CollisionDetectorAllocatorPtr, ConstPtr, WeakPtr... etc - -/** \brief An allocator for a compatible CollisionWorld/CollisionRobot pair. */ -class CollisionDetectorAllocator -{ -public: - virtual ~CollisionDetectorAllocator() - { - } - - /** A unique name identifying the CollisionWorld/CollisionRobot pairing. */ - virtual const std::string& getName() const = 0; - - /** create a new CollisionWorld for checking collisions with the supplied world. */ - virtual CollisionEnvPtr allocateEnv(const WorldPtr& world, - const moveit::core::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 CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const = 0; - - /** create a new CollisionEnv given a robot_model with a new empty world */ - virtual CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const = 0; -}; - -/** \brief Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. */ -template -class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator -{ -public: - const std::string& getName() const override - { - return CollisionDetectorAllocatorType::NAME; - } - - CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override - { - return std::make_shared(robot_model, world); - } - - CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override - { - return std::make_shared(dynamic_cast(*orig), world); - } - - CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override - { - return std::make_shared(robot_model); - } - - /** Create an allocator for collision detectors. */ - static CollisionDetectorAllocatorPtr create() - { - return std::make_shared(); - } -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.hpp new file mode 100644 index 0000000000..d9ece9f39f --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.hpp @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Acorn Pooley, Ioan Sucan */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator); // Defines CollisionDetectorAllocatorPtr, ConstPtr, WeakPtr... etc + +/** \brief An allocator for a compatible CollisionWorld/CollisionRobot pair. */ +class CollisionDetectorAllocator +{ +public: + virtual ~CollisionDetectorAllocator() + { + } + + /** A unique name identifying the CollisionWorld/CollisionRobot pairing. */ + virtual const std::string& getName() const = 0; + + /** create a new CollisionWorld for checking collisions with the supplied world. */ + virtual CollisionEnvPtr allocateEnv(const WorldPtr& world, + const moveit::core::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 CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const = 0; + + /** create a new CollisionEnv given a robot_model with a new empty world */ + virtual CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const = 0; +}; + +/** \brief Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. */ +template +class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator +{ +public: + const std::string& getName() const override + { + return CollisionDetectorAllocatorType::NAME; + } + + CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override + { + return std::make_shared(robot_model, world); + } + + CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override + { + return std::make_shared(dynamic_cast(*orig), world); + } + + CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override + { + return std::make_shared(robot_model); + } + + /** Create an allocator for collision detectors. */ + static CollisionDetectorAllocatorPtr create() + { + return std::make_shared(); + } +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h index c846274021..5de5a9ee47 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,293 +47,5 @@ /* Author: Ioan Sucan, Jens Petit */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionEnv); // Defines CollisionEnvPtr, ConstPtr, WeakPtr... etc - -/** \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 - */ - CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - - /** @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 moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, - double scale = 1.0); - - /** \brief Copy constructor */ - CollisionEnv(const CollisionEnv& other, const WorldPtr& world); - - virtual ~CollisionEnv() - { - } - - /** @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 */ - virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const = 0; - - /** \brief Check for self collision. 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 checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; - - /** \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 moveit::core::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 moveit::core::RobotState& state, - 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 moveit::core::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 moveit::core::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). - * 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 moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - 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). - * 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 moveit::core::RobotState& state1, - const moveit::core::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 moveit::core::RobotState& state) const = 0; - - /** \brief The distance to self-collision given the robot is at state \e state. */ - inline double distanceSelf(const moveit::core::RobotState& state) const - { - DistanceRequest req; - DistanceResult res; - - req.enableGroup(getRobotModel()); - distanceSelf(req, res, state); - return res.minimum_distance.distance; - } - - /** \brief The distance to self-collision given the robot is at state \e state, ignoring - the distances between links that are allowed to always collide (as specified by \e acm) */ - inline double distanceSelf(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const - { - DistanceRequest req; - DistanceResult res; - - req.enableGroup(getRobotModel()); - req.acm = &acm; - distanceSelf(req, res, 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 moveit::core::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 moveit::core::RobotState& state, bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.verbose = verbose; - req.enableGroup(getRobotModel()); - - distanceRobot(req, res, 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 moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, - bool verbose = false) const - { - DistanceRequest req; - DistanceResult res; - - req.acm = &acm; - req.verbose = verbose; - req.enableGroup(getRobotModel()); - - distanceRobot(req, res, state); - return res.minimum_distance.distance; - } - - /** set the world to use. - * This can be expensive unless the new and old world are empty. - * Passing nullptr 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_; - } - - using ObjectPtr = World::ObjectPtr; - using ObjectConstPtr = World::ObjectConstPtr; - - /** @brief The kinematic model corresponding to this collision model*/ - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - /** @brief Set the link padding for a particular link. - * @param link_name The link name to set padding for - * @param padding The padding to set (in meters) - */ - void setLinkPadding(const std::string& link_name, double padding); - - /** @brief Get the link padding for a particular link*/ - double getLinkPadding(const std::string& link_name) const; - - /** @brief Set the link paddings using a map (from link names to padding value) */ - void setLinkPadding(const std::map& padding); - - /** @brief Get the link paddings as a map (from link names to padding value) */ - const std::map& getLinkPadding() const; - - /** @brief Set the scaling for a particular link*/ - void setLinkScale(const std::string& link_name, double scale); - - /** @brief Set the scaling for a particular link*/ - double getLinkScale(const std::string& link_name) const; - - /** @brief Set the link scaling using a map (from link names to scale value) */ - void setLinkScale(const std::map& scale); - - /** @brief Get the link scaling as a map (from link names to scale value)*/ - const std::map& getLinkScale() const; - - /** @brief Set the link padding (for every link)*/ - void setPadding(double padding); - - /** @brief Set the link scaling (for every link)*/ - void setScale(double scale); - - /** @brief Set the link padding from a vector of messages*/ - void setPadding(const std::vector& padding); - - /** @brief Get the link padding as a vector of messages*/ - void getPadding(std::vector& padding) const; - - /** @brief Set the link scaling from a vector of messages*/ - void setScale(const std::vector& scale); - - /** @brief Get the link scaling as a vector of messages*/ - void getScale(std::vector& scale) const; - -protected: - /** @brief When the scale or padding is changed for a set of links by any of the functions in this class, - updatedPaddingOrScaling() function is called. - This function has an empty default implementation. The intention is to override this function in a derived class - to allow for updating - additional structures that may need such updating when link scale or padding changes. - @param links the names of the links whose padding or scaling were updated */ - virtual void updatedPaddingOrScaling(const std::vector& links); - - /** @brief The kinematic model corresponding to this collision model*/ - moveit::core::RobotModelConstPtr robot_model_; - - /** @brief The internally maintained map (from link names to padding)*/ - std::map link_padding_; - - /** @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_ -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.hpp new file mode 100644 index 0000000000..b05ad97cb9 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.hpp @@ -0,0 +1,327 @@ +/********************************************************************* + * 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 +#include +#include +#include +#include + +namespace collision_detection +{ +MOVEIT_CLASS_FORWARD(CollisionEnv); // Defines CollisionEnvPtr, ConstPtr, WeakPtr... etc + +/** \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 + */ + CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + /** @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 moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + /** \brief Copy constructor */ + CollisionEnv(const CollisionEnv& other, const WorldPtr& world); + + virtual ~CollisionEnv() + { + } + + /** @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 */ + virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const = 0; + + /** \brief Check for self collision. 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 checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0; + + /** \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 moveit::core::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 moveit::core::RobotState& state, + 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 moveit::core::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 moveit::core::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). + * 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 moveit::core::RobotState& state1, const moveit::core::RobotState& state2, + 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). + * 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 moveit::core::RobotState& state1, + const moveit::core::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 moveit::core::RobotState& state) const = 0; + + /** \brief The distance to self-collision given the robot is at state \e state. */ + inline double distanceSelf(const moveit::core::RobotState& state) const + { + DistanceRequest req; + DistanceResult res; + + req.enableGroup(getRobotModel()); + distanceSelf(req, res, state); + return res.minimum_distance.distance; + } + + /** \brief The distance to self-collision given the robot is at state \e state, ignoring + the distances between links that are allowed to always collide (as specified by \e acm) */ + inline double distanceSelf(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const + { + DistanceRequest req; + DistanceResult res; + + req.enableGroup(getRobotModel()); + req.acm = &acm; + distanceSelf(req, res, 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 moveit::core::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 moveit::core::RobotState& state, bool verbose = false) const + { + DistanceRequest req; + DistanceResult res; + + req.verbose = verbose; + req.enableGroup(getRobotModel()); + + distanceRobot(req, res, 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 moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + bool verbose = false) const + { + DistanceRequest req; + DistanceResult res; + + req.acm = &acm; + req.verbose = verbose; + req.enableGroup(getRobotModel()); + + distanceRobot(req, res, state); + return res.minimum_distance.distance; + } + + /** set the world to use. + * This can be expensive unless the new and old world are empty. + * Passing nullptr 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_; + } + + using ObjectPtr = World::ObjectPtr; + using ObjectConstPtr = World::ObjectConstPtr; + + /** @brief The kinematic model corresponding to this collision model*/ + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + /** @brief Set the link padding for a particular link. + * @param link_name The link name to set padding for + * @param padding The padding to set (in meters) + */ + void setLinkPadding(const std::string& link_name, double padding); + + /** @brief Get the link padding for a particular link*/ + double getLinkPadding(const std::string& link_name) const; + + /** @brief Set the link paddings using a map (from link names to padding value) */ + void setLinkPadding(const std::map& padding); + + /** @brief Get the link paddings as a map (from link names to padding value) */ + const std::map& getLinkPadding() const; + + /** @brief Set the scaling for a particular link*/ + void setLinkScale(const std::string& link_name, double scale); + + /** @brief Set the scaling for a particular link*/ + double getLinkScale(const std::string& link_name) const; + + /** @brief Set the link scaling using a map (from link names to scale value) */ + void setLinkScale(const std::map& scale); + + /** @brief Get the link scaling as a map (from link names to scale value)*/ + const std::map& getLinkScale() const; + + /** @brief Set the link padding (for every link)*/ + void setPadding(double padding); + + /** @brief Set the link scaling (for every link)*/ + void setScale(double scale); + + /** @brief Set the link padding from a vector of messages*/ + void setPadding(const std::vector& padding); + + /** @brief Get the link padding as a vector of messages*/ + void getPadding(std::vector& padding) const; + + /** @brief Set the link scaling from a vector of messages*/ + void setScale(const std::vector& scale); + + /** @brief Get the link scaling as a vector of messages*/ + void getScale(std::vector& scale) const; + +protected: + /** @brief When the scale or padding is changed for a set of links by any of the functions in this class, + updatedPaddingOrScaling() function is called. + This function has an empty default implementation. The intention is to override this function in a derived class + to allow for updating + additional structures that may need such updating when link scale or padding changes. + @param links the names of the links whose padding or scaling were updated */ + virtual void updatedPaddingOrScaling(const std::vector& links); + + /** @brief The kinematic model corresponding to this collision model*/ + moveit::core::RobotModelConstPtr robot_model_; + + /** @brief The internally maintained map (from link names to padding)*/ + std::map link_padding_; + + /** @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_ +}; +} // namespace collision_detection 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 1192ff87f6..d42591a58d 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,225 +47,5 @@ /* Author: Ioan Sucan, E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -/** \brief Any pair of bodies can have a collision state associated to it */ -namespace AllowedCollision -{ -enum Type -{ - /** \brief Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in - a particular configuration of the robot, that configuration is considered to be in collision*/ - NEVER, - - /** \brief Collisions between a particular pair of bodies does not imply that the robot configuration is in - collision. There is no need to explicitly do a computation (to check for contacts) on this pair of bodies*/ - ALWAYS, - - /** \brief The collision is allowed depending on a predicate evaluated on the produced contact. If the - predicate returns true, this particular contact is deemed ok (or allowed), i.e., the contact does not - imply that the two bodies are in collision*/ - CONDITIONAL -}; -} // namespace AllowedCollision - -/** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is - * CONDITIONAL) */ -using DecideContactFn = std::function; - -MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc - -/** @class AllowedCollisionMatrix - * @brief Definition of a structure for the allowed collision matrix. All elements in the collision world are referred - * to by their names. - * This class represents which collisions are allowed to happen and which are not. */ -class AllowedCollisionMatrix -{ -public: - AllowedCollisionMatrix(); - - /** @brief Instantiate using a vector of names (corresponding to all the elements in the collision world). - * @param names a vector of names (corresponding to object IDs in the collision world). - * @param allowed If false, indicates that collisions between all elements must be checked for and no collisions - * will be ignored. */ - AllowedCollisionMatrix(const std::vector& names, bool allowed = false); - - /** @brief Construct from an SRDF representation */ - AllowedCollisionMatrix(const srdf::Model& srdf); - - /** @brief Construct the structure from a message representation */ - AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg); - - /** @brief Copy constructor */ - AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; - - /** @brief Copy assignment */ - AllowedCollisionMatrix& operator=(const AllowedCollisionMatrix&) = default; - - /** @brief Get the type of the allowed collision between two elements. - * Return true if the entry is included in the collision matrix. Return false if the entry is not found. - * @param name1 name of first element - * @param name2 name of second element - * @param allowed_collision_type The allowed collision type will be filled here */ - bool getEntry(const std::string& name1, const std::string& name2, - AllowedCollision::Type& allowed_collision_type) const; - - /** @brief Get the allowed collision predicate between two elements. - * Return true if a predicate for this entry is available in the collision matrix. - * Return false if the entry is not found. - * @param name1 name of first element - * @param name2 name of second element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled - * here */ - bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; - - /** @brief Check if the allowed collision matrix has an entry at all for an element. - * Returns true if the element is included. - * @param name name of the element */ - bool hasEntry(const std::string& name) const; - - /** @brief Check if the allowed collision matrix has an entry for a pair of elements. - * Returns true if the pair is included. - * @param name1 name of first element - * @param name2 name of second element*/ - bool hasEntry(const std::string& name1, const std::string& name2) const; - - /** @brief Remove an entry corresponding to a pair of elements. - * Nothing happens if the pair does not exist in the collision matrix. - * @param name1 name of first element - * @param name2 name of second element*/ - void removeEntry(const std::string& name1, const std::string& name2); - - /** @brief Remove all entries corresponding to a name (all pairs that include this name) - * @param name namespace*/ - void removeEntry(const std::string& name); - - /** @brief Set an entry corresponding to a pair of elements - * @param name1 name of first element - * @param name2 name of second element - * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ - void setEntry(const std::string& name1, const std::string& name2, bool allowed); - - /** @brief Set an entry corresponding to a pair of elements. - * - * This sets the type of the entry to AllowedCollision::CONDITIONAL. - * @param name1 name of first element - * @param name2 name of second element - * @param fn A callback function that is used to decide if collisions are allowed between the two elements */ - void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn); - - /** @brief Set the entries corresponding to a name. - * With each of the *known names* in the collision matrix, form a pair using the name - * specified as argument to this function and set the entry as indicated by \e allowed. - * As the set of known names might change in future, consider using setDefaultEntry() instead! - * @param name the object name - * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ - void setEntry(const std::string& name, bool allowed); - - /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names - * @param name name of first element - * @param other_names names of all other elements to pair with first element. The collision - * matrix entries will be set for all such pairs. - * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ - void setEntry(const std::string& name, const std::vector& other_names, bool allowed); - - /** @brief Set an entry corresponding to all possible pairs between two sets of elements - * @param names1 First set of names - * @param names2 Second set of names - * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ - void setEntry(const std::vector& names1, const std::vector& names2, bool allowed); - - /** @brief Set an entry corresponding to all known pairs - * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions - * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an - * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ - void setEntry(bool allowed); - - /** @brief Get all the names known to the collision matrix */ - void getAllEntryNames(std::vector& names) const; - - /** @brief Get the allowed collision matrix as a message */ - void getMessage(moveit_msgs::msg::AllowedCollisionMatrix& msg) const; - - /** @brief Clear the allowed collision matrix */ - void clear(); - - /** @brief Get the size of the allowed collision matrix (number of specified entries) */ - std::size_t getSize() const - { - return entries_.size(); - } - - /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry(). - * @param name The name of the element for which to set the default value - * @param allowed If false, indicates that collisions between \e name and any other element must be checked for and - * no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and - * any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ - void setDefaultEntry(const std::string& name, bool allowed); - - /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry(). - * @param name The name of the element for which to set the default value - * @param fn A callback function that is used to decide if collisions are allowed between \e name and some other - * element is expected here. */ - void setDefaultEntry(const std::string& name, DecideContactFn& fn); - - /** @brief Get the type of the allowed collision to be considered by default for an element. - * Return true if a default value was found for the specified element. Return false otherwise. - * @param name name of the element - * @param allowed_collision The default allowed collision type will be filled here */ - bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const; - - /** @brief Get the type of the allowed collision between to be considered by default for an element. - * Return true if a default value was found for the specified element. Return false otherwise. - * @param name name of the element - * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ - bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const; - - /** @brief Get the allowed collision predicate between two elements. - * Return true if a predicate for entry is included in the collision matrix (if the type is AllowedCollision::CONDITIONAL) - * or if one was computed from defaults. Return false if the entry is not found. - * @param name1 name of first element - * @param name2 name of second element - * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ - bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; - - /** @brief Get the type of the allowed collision between two elements. - * Return true if the entry is included in the collision matrix or if specified defaults were found. - * Return false if the entry is not found. - * @param name1 name of first element - * @param name2 name of second element - * @param allowed_collision The allowed collision type will be filled here */ - bool getAllowedCollision(const std::string& name1, const std::string& name2, - AllowedCollision::Type& allowed_collision) const; - - /** @brief Print the allowed collision matrix */ - void print(std::ostream& out) const; - -private: - bool getDefaultEntry(const std::string& name1, const std::string& name2, - AllowedCollision::Type& allowed_collision) const; - - std::map > entries_; - std::map > allowed_contacts_; - - std::map default_entries_; - std::map default_allowed_contacts_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.hpp new file mode 100644 index 0000000000..fff68a3142 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.hpp @@ -0,0 +1,259 @@ +/********************************************************************* + * 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, E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +/** \brief Any pair of bodies can have a collision state associated to it */ +namespace AllowedCollision +{ +enum Type +{ + /** \brief Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in + a particular configuration of the robot, that configuration is considered to be in collision*/ + NEVER, + + /** \brief Collisions between a particular pair of bodies does not imply that the robot configuration is in + collision. There is no need to explicitly do a computation (to check for contacts) on this pair of bodies*/ + ALWAYS, + + /** \brief The collision is allowed depending on a predicate evaluated on the produced contact. If the + predicate returns true, this particular contact is deemed ok (or allowed), i.e., the contact does not + imply that the two bodies are in collision*/ + CONDITIONAL +}; +} // namespace AllowedCollision + +/** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is + * CONDITIONAL) */ +using DecideContactFn = std::function; + +MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc + +/** @class AllowedCollisionMatrix + * @brief Definition of a structure for the allowed collision matrix. All elements in the collision world are referred + * to by their names. + * This class represents which collisions are allowed to happen and which are not. */ +class AllowedCollisionMatrix +{ +public: + AllowedCollisionMatrix(); + + /** @brief Instantiate using a vector of names (corresponding to all the elements in the collision world). + * @param names a vector of names (corresponding to object IDs in the collision world). + * @param allowed If false, indicates that collisions between all elements must be checked for and no collisions + * will be ignored. */ + AllowedCollisionMatrix(const std::vector& names, bool allowed = false); + + /** @brief Construct from an SRDF representation */ + AllowedCollisionMatrix(const srdf::Model& srdf); + + /** @brief Construct the structure from a message representation */ + AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg); + + /** @brief Copy constructor */ + AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default; + + /** @brief Copy assignment */ + AllowedCollisionMatrix& operator=(const AllowedCollisionMatrix&) = default; + + /** @brief Get the type of the allowed collision between two elements. + * Return true if the entry is included in the collision matrix. Return false if the entry is not found. + * @param name1 name of first element + * @param name2 name of second element + * @param allowed_collision_type The allowed collision type will be filled here */ + bool getEntry(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision_type) const; + + /** @brief Get the allowed collision predicate between two elements. + * Return true if a predicate for this entry is available in the collision matrix. + * Return false if the entry is not found. + * @param name1 name of first element + * @param name2 name of second element + * @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled + * here */ + bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; + + /** @brief Check if the allowed collision matrix has an entry at all for an element. + * Returns true if the element is included. + * @param name name of the element */ + bool hasEntry(const std::string& name) const; + + /** @brief Check if the allowed collision matrix has an entry for a pair of elements. + * Returns true if the pair is included. + * @param name1 name of first element + * @param name2 name of second element*/ + bool hasEntry(const std::string& name1, const std::string& name2) const; + + /** @brief Remove an entry corresponding to a pair of elements. + * Nothing happens if the pair does not exist in the collision matrix. + * @param name1 name of first element + * @param name2 name of second element*/ + void removeEntry(const std::string& name1, const std::string& name2); + + /** @brief Remove all entries corresponding to a name (all pairs that include this name) + * @param name namespace*/ + void removeEntry(const std::string& name); + + /** @brief Set an entry corresponding to a pair of elements + * @param name1 name of first element + * @param name2 name of second element + * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions + * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setEntry(const std::string& name1, const std::string& name2, bool allowed); + + /** @brief Set an entry corresponding to a pair of elements. + * + * This sets the type of the entry to AllowedCollision::CONDITIONAL. + * @param name1 name of first element + * @param name2 name of second element + * @param fn A callback function that is used to decide if collisions are allowed between the two elements */ + void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn); + + /** @brief Set the entries corresponding to a name. + * With each of the *known names* in the collision matrix, form a pair using the name + * specified as argument to this function and set the entry as indicated by \e allowed. + * As the set of known names might change in future, consider using setDefaultEntry() instead! + * @param name the object name + * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions + * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setEntry(const std::string& name, bool allowed); + + /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names + * @param name name of first element + * @param other_names names of all other elements to pair with first element. The collision + * matrix entries will be set for all such pairs. + * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions + * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setEntry(const std::string& name, const std::vector& other_names, bool allowed); + + /** @brief Set an entry corresponding to all possible pairs between two sets of elements + * @param names1 First set of names + * @param names2 Second set of names + * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions + * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setEntry(const std::vector& names1, const std::vector& names2, bool allowed); + + /** @brief Set an entry corresponding to all known pairs + * @param allowed If false, indicates that collisions between two elements must be checked for and no collisions + * will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an + * explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setEntry(bool allowed); + + /** @brief Get all the names known to the collision matrix */ + void getAllEntryNames(std::vector& names) const; + + /** @brief Get the allowed collision matrix as a message */ + void getMessage(moveit_msgs::msg::AllowedCollisionMatrix& msg) const; + + /** @brief Clear the allowed collision matrix */ + void clear(); + + /** @brief Get the size of the allowed collision matrix (number of specified entries) */ + std::size_t getSize() const + { + return entries_.size(); + } + + /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry(). + * @param name The name of the element for which to set the default value + * @param allowed If false, indicates that collisions between \e name and any other element must be checked for and + * no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and + * any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/ + void setDefaultEntry(const std::string& name, bool allowed); + + /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry(). + * @param name The name of the element for which to set the default value + * @param fn A callback function that is used to decide if collisions are allowed between \e name and some other + * element is expected here. */ + void setDefaultEntry(const std::string& name, DecideContactFn& fn); + + /** @brief Get the type of the allowed collision to be considered by default for an element. + * Return true if a default value was found for the specified element. Return false otherwise. + * @param name name of the element + * @param allowed_collision The default allowed collision type will be filled here */ + bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const; + + /** @brief Get the type of the allowed collision between to be considered by default for an element. + * Return true if a default value was found for the specified element. Return false otherwise. + * @param name name of the element + * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ + bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const; + + /** @brief Get the allowed collision predicate between two elements. + * Return true if a predicate for entry is included in the collision matrix (if the type is AllowedCollision::CONDITIONAL) + * or if one was computed from defaults. Return false if the entry is not found. + * @param name1 name of first element + * @param name2 name of second element + * @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */ + bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const; + + /** @brief Get the type of the allowed collision between two elements. + * Return true if the entry is included in the collision matrix or if specified defaults were found. + * Return false if the entry is not found. + * @param name1 name of first element + * @param name2 name of second element + * @param allowed_collision The allowed collision type will be filled here */ + bool getAllowedCollision(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const; + + /** @brief Print the allowed collision matrix */ + void print(std::ostream& out) const; + +private: + bool getDefaultEntry(const std::string& name1, const std::string& name2, + AllowedCollision::Type& allowed_collision) const; + + std::map > entries_; + std::map > allowed_contacts_; + + std::map default_entries_; + std::map default_allowed_contacts_; +}; +} // namespace collision_detection 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 36069f95a5..38cc245c27 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,29 +47,5 @@ /* Author: Adam Leeper */ #pragma once - -#include -#include - -namespace collision_detection -{ -/** @brief Re-proceses contact normals for an octomap by estimating a metaball - * iso-surface using the centers of occupied cells in a neighborhood of the contact point. - * - * This is an implementation of the algorithm described in: - * A. Leeper, S. Chan, K. Salisbury. Point Clouds Can Be Represented as Implicit - * Surfaces for Constraint-Based Haptic Rendering. ICRA, May 2012, St Paul, MN. - * http://adamleeper.com/research/papers/2012_ICRA_leeper-chan-salisbury.pdf - * - * @param The octomap originally used for collision detection. - * @param The collision result (which will get its normals updated) - * @param The distance, as a multiple of the octomap cell size, from which to include neighboring cells. - * @param The minimum angle change required for a normal to be over-written - * @param Whether to request a depth estimate from the algorithm (experimental...) - * @param The iso-surface threshold value (0.5 is a reasonable default). - * @param The metaball radius, as a multiple of the octomap cell size (1.5 is a reasonable default) - */ -int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res, - 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); -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.hpp new file mode 100644 index 0000000000..1316d23c4b --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.hpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Adam Leeper */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +/** @brief Re-proceses contact normals for an octomap by estimating a metaball + * iso-surface using the centers of occupied cells in a neighborhood of the contact point. + * + * This is an implementation of the algorithm described in: + * A. Leeper, S. Chan, K. Salisbury. Point Clouds Can Be Represented as Implicit + * Surfaces for Constraint-Based Haptic Rendering. ICRA, May 2012, St Paul, MN. + * http://adamleeper.com/research/papers/2012_ICRA_leeper-chan-salisbury.pdf + * + * @param The octomap originally used for collision detection. + * @param The collision result (which will get its normals updated) + * @param The distance, as a multiple of the octomap cell size, from which to include neighboring cells. + * @param The minimum angle change required for a normal to be over-written + * @param Whether to request a depth estimate from the algorithm (experimental...) + * @param The iso-surface threshold value (0.5 is a reasonable default). + * @param The metaball radius, as a multiple of the octomap cell size (1.5 is a reasonable default) + */ +int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res, + 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); +} // namespace collision_detection 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 226d4e4f8c..d4fd16611e 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,64 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(CollisionPlugin); // Defines CollisionPluginPtr, ConstPtr, WeakPtr... etc - -/** - * @brief Plugin API for loading a custom collision detection robot/world. - * - * Typical Usage: - * 
- *   namespace my_collision_checker
- *   {
- *
- *   class MyCollisionDetectorAllocator :
- *     public collision_detection::CollisionDetectorAllocatorTemplate
- *   {
- *     public:
- *       const std::string& getName() const override {
- *         static const std::string NAME = "my_checker";
- *         return NAME;
- *       }
- *       static const std::string NAME_;
- *   };
- *
- *   }
- *
- *   namespace collision_detection
- *   {
- *
- *   class MyCollisionDetectionLoader : public CollisionPlugin
- *   {
- *   public:
- *     virtual bool initialize(const planning_scene::PlanningScenePtr& scene) const
- *     {
- *       scene->allocateCollisionDetector(my_collision_checker::MyCollisionDetectorAllocator::create());
-         return true;
- *     }
- *   };
- *
- */ -class CollisionPlugin -{ -public: - CollisionPlugin() - { - } - virtual ~CollisionPlugin() - { - } - - /** - * @brief This should be used to load your collision plugin. - */ - virtual bool initialize(const planning_scene::PlanningScenePtr& scene) const = 0; -}; - -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.hpp new file mode 100644 index 0000000000..afc032700a --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.hpp @@ -0,0 +1,96 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014 Fetch Robotics 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 Fetch Robotics 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include + +namespace collision_detection +{ +MOVEIT_CLASS_FORWARD(CollisionPlugin); // Defines CollisionPluginPtr, ConstPtr, WeakPtr... etc + +/** + * @brief Plugin API for loading a custom collision detection robot/world. + * + * Typical Usage: + * 
+ *   namespace my_collision_checker
+ *   {
+ *
+ *   class MyCollisionDetectorAllocator :
+ *     public collision_detection::CollisionDetectorAllocatorTemplate
+ *   {
+ *     public:
+ *       const std::string& getName() const override {
+ *         static const std::string NAME = "my_checker";
+ *         return NAME;
+ *       }
+ *       static const std::string NAME_;
+ *   };
+ *
+ *   }
+ *
+ *   namespace collision_detection
+ *   {
+ *
+ *   class MyCollisionDetectionLoader : public CollisionPlugin
+ *   {
+ *   public:
+ *     virtual bool initialize(const planning_scene::PlanningScenePtr& scene) const
+ *     {
+ *       scene->allocateCollisionDetector(my_collision_checker::MyCollisionDetectorAllocator::create());
+         return true;
+ *     }
+ *   };
+ *
+ */ +class CollisionPlugin +{ +public: + CollisionPlugin() + { + } + virtual ~CollisionPlugin() + { + } + + /** + * @brief This should be used to load your collision plugin. + */ + virtual bool initialize(const planning_scene::PlanningScenePtr& scene) const = 0; +}; + +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.h index 05c539854b..488a734dbd 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,31 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace collision_detection -{ -/** Helper class to activate a specific collision plugin for a PlanningScene */ -class CollisionPluginCache -{ -public: - CollisionPluginCache(); - ~CollisionPluginCache(); - - /** - * @brief Activate a specific collision plugin for the given planning scene instance. - * @param name The plugin name. - * @param scene The planning scene instance. - * @param exclusive If true, sets the new plugin to be the only one. - * @return success / failure - */ - bool activate(const std::string& name, const planning_scene::PlanningScenePtr& scene); - -private: - MOVEIT_CLASS_FORWARD(CollisionPluginCacheImpl); - CollisionPluginCacheImplPtr cache_; -}; - -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.hpp new file mode 100644 index 0000000000..f692448516 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin_cache.hpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014 Fetch Robotics 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 Fetch Robotics 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace collision_detection +{ +/** Helper class to activate a specific collision plugin for a PlanningScene */ +class CollisionPluginCache +{ +public: + CollisionPluginCache(); + ~CollisionPluginCache(); + + /** + * @brief Activate a specific collision plugin for the given planning scene instance. + * @param name The plugin name. + * @param scene The planning scene instance. + * @param exclusive If true, sets the new plugin to be the only one. + * @return success / failure + */ + bool activate(const std::string& name, const planning_scene::PlanningScenePtr& scene); + +private: + MOVEIT_CLASS_FORWARD(CollisionPluginCacheImpl); + CollisionPluginCacheImplPtr cache_; +}; + +} // namespace collision_detection 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 61caac8646..1ade4aff95 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,44 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace collision_detection -{ -void getCollisionMarkersFromContacts(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, - const CollisionResult::ContactMap& con, const std_msgs::msg::ColorRGBA& color, - const rclcpp::Duration& lifetime, const double radius = 0.035); - -void getCollisionMarkersFromContacts(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, - const CollisionResult::ContactMap& con); - -/// \todo add a class for managing cost sources -void getCostMarkers(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, - std::set& cost_sources); - -void getCostMarkers(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, - std::set& cost_sources, const std_msgs::msg::ColorRGBA& color, - const rclcpp::Duration& lifetime); - -double getTotalCost(const std::set& cost_sources); - -void removeCostSources(std::set& cost_sources, const std::set& cost_sources_to_remove, - double overlap_fraction); -void intersectCostSources(std::set& cost_sources, const std::set& a, - const std::set& b); -void removeOverlapping(std::set& cost_sources, double overlap_fraction); - -bool getSensorPositioning(geometry_msgs::msg::Point& point, const std::set& cost_sources); - -void costSourceToMsg(const CostSource& cost_source, moveit_msgs::msg::CostSource& msg); -void contactToMsg(const Contact& contact, moveit_msgs::msg::ContactInformation& msg); -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.hpp new file mode 100644 index 0000000000..f55f01e35c --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.hpp @@ -0,0 +1,78 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace collision_detection +{ +void getCollisionMarkersFromContacts(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, + const CollisionResult::ContactMap& con, const std_msgs::msg::ColorRGBA& color, + const rclcpp::Duration& lifetime, const double radius = 0.035); + +void getCollisionMarkersFromContacts(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, + const CollisionResult::ContactMap& con); + +/// \todo add a class for managing cost sources +void getCostMarkers(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, + std::set& cost_sources); + +void getCostMarkers(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id, + std::set& cost_sources, const std_msgs::msg::ColorRGBA& color, + const rclcpp::Duration& lifetime); + +double getTotalCost(const std::set& cost_sources); + +void removeCostSources(std::set& cost_sources, const std::set& cost_sources_to_remove, + double overlap_fraction); +void intersectCostSources(std::set& cost_sources, const std::set& a, + const std::set& b); +void removeOverlapping(std::set& cost_sources, double overlap_fraction); + +bool getSensorPositioning(geometry_msgs::msg::Point& point, const std::set& cost_sources); + +void costSourceToMsg(const CostSource& cost_source, moveit_msgs::msg::CostSource& msg); +void contactToMsg(const Contact& contact, moveit_msgs::msg::ContactInformation& msg); +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.h b/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.h index 514cd5a061..4622e9428b 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,86 +47,5 @@ /* Author: Ioan Sucan, Jon Binney */ #pragma once - -#include - -#include -#include -#include -#include -#include - -namespace collision_detection -{ -typedef octomap::OcTreeNode OccMapNode; - -class OccMapTree : public octomap::OcTree -{ -public: - OccMapTree(double resolution) : octomap::OcTree(resolution) - { - } - - OccMapTree(const std::string& filename) : octomap::OcTree(filename) - { - } - - /** @brief lock the underlying octree. it will not be read or written by the - * monitor until unlockTree() is called */ - void lockRead() - { - tree_mutex_.lock_shared(); - } - - /** @brief unlock the underlying octree. */ - void unlockRead() - { - tree_mutex_.unlock_shared(); - } - - /** @brief lock the underlying octree. it will not be read or written by the - * monitor until unlockTree() is called */ - void lockWrite() - { - tree_mutex_.lock(); - } - - /** @brief unlock the underlying octree. */ - void unlockWrite() - { - tree_mutex_.unlock(); - } - - using ReadLock = std::shared_lock; - using WriteLock = std::unique_lock; - - ReadLock reading() - { - return ReadLock(tree_mutex_); - } - - WriteLock writing() - { - return WriteLock(tree_mutex_); - } - - void triggerUpdateCallback() - { - if (update_callback_) - update_callback_(); - } - - /** @brief Set the callback to trigger when updates are received */ - void setUpdateCallback(const std::function& update_callback) - { - update_callback_ = update_callback; - } - -private: - std::shared_mutex tree_mutex_; - std::function update_callback_; -}; - -using OccMapTreePtr = std::shared_ptr; -using OccMapTreeConstPtr = std::shared_ptr; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.hpp new file mode 100644 index 0000000000..514cd5a061 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.hpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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, Jon Binney */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace collision_detection +{ +typedef octomap::OcTreeNode OccMapNode; + +class OccMapTree : public octomap::OcTree +{ +public: + OccMapTree(double resolution) : octomap::OcTree(resolution) + { + } + + OccMapTree(const std::string& filename) : octomap::OcTree(filename) + { + } + + /** @brief lock the underlying octree. it will not be read or written by the + * monitor until unlockTree() is called */ + void lockRead() + { + tree_mutex_.lock_shared(); + } + + /** @brief unlock the underlying octree. */ + void unlockRead() + { + tree_mutex_.unlock_shared(); + } + + /** @brief lock the underlying octree. it will not be read or written by the + * monitor until unlockTree() is called */ + void lockWrite() + { + tree_mutex_.lock(); + } + + /** @brief unlock the underlying octree. */ + void unlockWrite() + { + tree_mutex_.unlock(); + } + + using ReadLock = std::shared_lock; + using WriteLock = std::unique_lock; + + ReadLock reading() + { + return ReadLock(tree_mutex_); + } + + WriteLock writing() + { + return WriteLock(tree_mutex_); + } + + void triggerUpdateCallback() + { + if (update_callback_) + update_callback_(); + } + + /** @brief Set the callback to trigger when updates are received */ + void setUpdateCallback(const std::function& update_callback) + { + update_callback_ = update_callback; + } + +private: + std::shared_mutex tree_mutex_; + std::function update_callback_; +}; + +using OccMapTreePtr = std::shared_ptr; +using OccMapTreeConstPtr = std::shared_ptr; +} // namespace collision_detection diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 061a304085..29593909ff 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -34,348 +46,6 @@ /* Author: Jens Petit */ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -/** \brief Brings the panda robot in user defined home position */ -inline void setToHome(moveit::core::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(); -} - -template -class CollisionDetectorPandaTest : public testing::Test -{ -public: - std::shared_ptr value_; - -protected: - void SetUp() override - { - value_ = std::make_shared(); - robot_model_ = moveit::core::loadTestingRobotModel("panda"); - robot_model_ok_ = static_cast(robot_model_); - - acm_ = std::make_shared(*robot_model_->getSRDF()); - - cenv_ = value_->allocateEnv(robot_model_); - - robot_state_ = std::make_shared(robot_model_); - setToHome(*robot_state_); - } - - void TearDown() override - { - } - - bool robot_model_ok_; - - moveit::core::RobotModelPtr robot_model_; - - collision_detection::CollisionEnvPtr cenv_; - - collision_detection::AllowedCollisionMatrixPtr acm_; - - moveit::core::RobotStatePtr robot_state_; -}; - -TYPED_TEST_CASE_P(CollisionDetectorPandaTest); - -/** \brief Correct setup testing. */ -TYPED_TEST_P(CollisionDetectorPandaTest, InitOK) -{ - ASSERT_TRUE(this->robot_model_ok_); -} - -/** \brief Tests the default values specified in the SRDF if they are collision free. */ -TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); -} - -/** \brief A configuration where the robot should collide with itself. */ -TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) -{ - // Sets the joints into a colliding configuration - double joint2 = 0.15; - double joint4 = -3.0; - this->robot_state_->setJointPositions("panda_joint2", &joint2); - this->robot_state_->setJointPositions("panda_joint4", &joint4); - this->robot_state_->update(); - - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_TRUE(res.collision); -} - -/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ -TYPED_TEST_P(CollisionDetectorPandaTest, 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; - this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity()); - - this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); - res.clear(); - - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_TRUE(res.collision); - res.clear(); - - pos1.translation().z() = 0.25; - this->cenv_->getWorld()->moveObject("box", pos1); - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); - res.clear(); - - pos1.translation().z() = 0.05; - this->cenv_->getWorld()->moveObject("box", pos1); - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_TRUE(res.collision); - res.clear(); - - pos1.translation().z() = 0.25; - this->cenv_->getWorld()->moveObject("box", pos1); - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); - res.clear(); - - this->cenv_->getWorld()->moveObject("box", pos1); - ASSERT_FALSE(res.collision); -} - -/** \brief Adding obstacles to the world which are tested against the robot. */ -TYPED_TEST_P(CollisionDetectorPandaTest, 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; - this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity()); - - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->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. */ -TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) -{ - collision_detection::CollisionRequest req; - req.contacts = true; - req.max_contacts = 10; - collision_detection::CollisionResult res; - - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->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; - this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity()); - - this->cenv_->setLinkPadding("panda_hand", 0.08); - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_TRUE(res.collision); - res.clear(); - - this->cenv_->setLinkPadding("panda_hand", 0.0); - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); -} - -/** \brief Tests the distance reporting with the robot itself */ -TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf) -{ - collision_detection::CollisionRequest req; - req.distance = true; - collision_detection::CollisionResult res; - this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); - EXPECT_NEAR(res.distance, 0.022, 0.001); -} - -TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) -{ - collision_detection::CollisionRequest req; - req.distance = true; - collision_detection::CollisionResult res; - - // 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; - this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity()); - - this->cenv_->setLinkPadding("panda_hand", 0.0); - this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); - ASSERT_FALSE(res.collision); - EXPECT_NEAR(res.distance, 0.029, 0.01); -} - -template -class DistanceCheckPandaTest : public CollisionDetectorPandaTest -{ -}; - -TYPED_TEST_CASE_P(DistanceCheckPandaTest); - -TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle) -{ - std::set active_components{ this->robot_model_->getLinkModel("panda_hand") }; - collision_detection::DistanceRequest req; - req.type = collision_detection::DistanceRequestTypes::SINGLE; - req.active_components_only = &active_components; - req.enable_signed_distance = true; - - random_numbers::RandomNumberGenerator rng(0x47110815); - double min_distance = std::numeric_limits::max(); - for (int i = 0; i < 10; ++i) - { - collision_detection::DistanceResult res; - - shapes::ShapeConstPtr shape = std::make_shared(rng.uniform01(), rng.uniform01()); - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = - Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7)); - double quat[4]; - rng.quaternion(quat); - pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix(); - - this->cenv_->getWorld()->addToObject("collection", Eigen::Isometry3d::Identity(), shape, pose); - this->cenv_->getWorld()->removeObject("object"); - this->cenv_->getWorld()->addToObject("object", pose, shape, Eigen::Isometry3d::Identity()); - - this->cenv_->distanceRobot(req, res, *this->robot_state_); - auto& distances1 = res.distances[std::pair("collection", "panda_hand")]; - auto& distances2 = res.distances[std::pair("object", "panda_hand")]; - ASSERT_EQ(distances1.size(), 1u) << "no distance reported for collection/panda_hand"; - ASSERT_EQ(distances2.size(), 1u) << "no distance reported for object/panda_hand"; - - double collection_distance = distances1[0].distance; - min_distance = std::min(min_distance, distances2[0].distance); - ASSERT_NEAR(collection_distance, min_distance, 1e-5) - << "distance to collection is greater than distance to minimum of individual objects after " << i << " rounds"; - } -} - -// FCL < 0.6 completely fails the DistancePoints test, so we have two test -// suites, one with and one without the test. -template -class DistanceFullPandaTest : public DistanceCheckPandaTest -{ -}; - -TYPED_TEST_CASE_P(DistanceFullPandaTest); - -/** \brief Tests if nearest points are computed correctly. */ -TYPED_TEST_P(DistanceFullPandaTest, DistancePoints) -{ - // Adding the box right in front of the robot hand - shapes::Box* 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; - this->cenv_->getWorld()->addToObject("box", shape_ptr, pos); - - collision_detection::DistanceRequest req; - req.acm = &*this->acm_; - req.type = collision_detection::DistanceRequestTypes::SINGLE; - req.enable_nearest_points = true; - req.max_contacts_per_body = 1; - - collision_detection::DistanceResult res; - this->cenv_->distanceRobot(req, res, *this->robot_state_); - - // Checks a particular point is inside the box - auto check_in_box = [&](const Eigen::Vector3d& p) { - Eigen::Vector3d in_box = pos.inverse() * p; - - constexpr double eps = 1e-5; - EXPECT_LE(std::abs(in_box.x()), shape->size[0] + eps); - EXPECT_LE(std::abs(in_box.y()), shape->size[1] + eps); - EXPECT_LE(std::abs(in_box.z()), shape->size[2] + eps); - }; - - // Check that all points reported on "box" are actually on the box and not - // on the robot - for (auto& distance : res.distances) - { - for (auto& pair : distance.second) - { - if (pair.link_names[0] == "box") - { - check_in_box(pair.nearest_points[0]); - } - else if (pair.link_names[1] == "box") - { - check_in_box(pair.nearest_points[1]); - } - else - { - ADD_FAILURE() << "Unrecognized link names"; - } - } - } -} - -REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, - RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); - -REGISTER_TYPED_TEST_SUITE_P(DistanceCheckPandaTest, DistanceSingle); - -REGISTER_TYPED_TEST_SUITE_P(DistanceFullPandaTest, DistancePoints); +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.hpp new file mode 100644 index 0000000000..4d7b1e095b --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.hpp @@ -0,0 +1,383 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/** \brief Brings the panda robot in user defined home position */ +inline void setToHome(moveit::core::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(); +} + +template +class CollisionDetectorPandaTest : public testing::Test +{ +public: + std::shared_ptr value_; + +protected: + void SetUp() override + { + value_ = std::make_shared(); + robot_model_ = moveit::core::loadTestingRobotModel("panda"); + robot_model_ok_ = static_cast(robot_model_); + + acm_ = std::make_shared(*robot_model_->getSRDF()); + + cenv_ = value_->allocateEnv(robot_model_); + + robot_state_ = std::make_shared(robot_model_); + setToHome(*robot_state_); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + moveit::core::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr cenv_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + moveit::core::RobotStatePtr robot_state_; +}; + +TYPED_TEST_CASE_P(CollisionDetectorPandaTest); + +/** \brief Correct setup testing. */ +TYPED_TEST_P(CollisionDetectorPandaTest, InitOK) +{ + ASSERT_TRUE(this->robot_model_ok_); +} + +/** \brief Tests the default values specified in the SRDF if they are collision free. */ +TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief A configuration where the robot should collide with itself. */ +TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision) +{ + // Sets the joints into a colliding configuration + double joint2 = 0.15; + double joint4 = -3.0; + this->robot_state_->setJointPositions("panda_joint2", &joint2); + this->robot_state_->setJointPositions("panda_joint4", &joint4); + this->robot_state_->update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */ +TYPED_TEST_P(CollisionDetectorPandaTest, 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; + this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity()); + + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + pos1.translation().z() = 0.05; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + this->cenv_->getWorld()->moveObject("box", pos1); + ASSERT_FALSE(res.collision); +} + +/** \brief Adding obstacles to the world which are tested against the robot. */ +TYPED_TEST_P(CollisionDetectorPandaTest, 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; + this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity()); + + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->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. */ +TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 10; + collision_detection::CollisionResult res; + + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->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; + this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity()); + + this->cenv_->setLinkPadding("panda_hand", 0.08); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_TRUE(res.collision); + res.clear(); + + this->cenv_->setLinkPadding("panda_hand", 0.0); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); +} + +/** \brief Tests the distance reporting with the robot itself */ +TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf) +{ + collision_detection::CollisionRequest req; + req.distance = true; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + EXPECT_NEAR(res.distance, 0.022, 0.001); +} + +TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld) +{ + collision_detection::CollisionRequest req; + req.distance = true; + collision_detection::CollisionResult res; + + // 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; + this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity()); + + this->cenv_->setLinkPadding("panda_hand", 0.0); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + EXPECT_NEAR(res.distance, 0.029, 0.01); +} + +template +class DistanceCheckPandaTest : public CollisionDetectorPandaTest +{ +}; + +TYPED_TEST_CASE_P(DistanceCheckPandaTest); + +TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle) +{ + std::set active_components{ this->robot_model_->getLinkModel("panda_hand") }; + collision_detection::DistanceRequest req; + req.type = collision_detection::DistanceRequestTypes::SINGLE; + req.active_components_only = &active_components; + req.enable_signed_distance = true; + + random_numbers::RandomNumberGenerator rng(0x47110815); + double min_distance = std::numeric_limits::max(); + for (int i = 0; i < 10; ++i) + { + collision_detection::DistanceResult res; + + shapes::ShapeConstPtr shape = std::make_shared(rng.uniform01(), rng.uniform01()); + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = + Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7)); + double quat[4]; + rng.quaternion(quat); + pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix(); + + this->cenv_->getWorld()->addToObject("collection", Eigen::Isometry3d::Identity(), shape, pose); + this->cenv_->getWorld()->removeObject("object"); + this->cenv_->getWorld()->addToObject("object", pose, shape, Eigen::Isometry3d::Identity()); + + this->cenv_->distanceRobot(req, res, *this->robot_state_); + auto& distances1 = res.distances[std::pair("collection", "panda_hand")]; + auto& distances2 = res.distances[std::pair("object", "panda_hand")]; + ASSERT_EQ(distances1.size(), 1u) << "no distance reported for collection/panda_hand"; + ASSERT_EQ(distances2.size(), 1u) << "no distance reported for object/panda_hand"; + + double collection_distance = distances1[0].distance; + min_distance = std::min(min_distance, distances2[0].distance); + ASSERT_NEAR(collection_distance, min_distance, 1e-5) + << "distance to collection is greater than distance to minimum of individual objects after " << i << " rounds"; + } +} + +// FCL < 0.6 completely fails the DistancePoints test, so we have two test +// suites, one with and one without the test. +template +class DistanceFullPandaTest : public DistanceCheckPandaTest +{ +}; + +TYPED_TEST_CASE_P(DistanceFullPandaTest); + +/** \brief Tests if nearest points are computed correctly. */ +TYPED_TEST_P(DistanceFullPandaTest, DistancePoints) +{ + // Adding the box right in front of the robot hand + shapes::Box* 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; + this->cenv_->getWorld()->addToObject("box", shape_ptr, pos); + + collision_detection::DistanceRequest req; + req.acm = &*this->acm_; + req.type = collision_detection::DistanceRequestTypes::SINGLE; + req.enable_nearest_points = true; + req.max_contacts_per_body = 1; + + collision_detection::DistanceResult res; + this->cenv_->distanceRobot(req, res, *this->robot_state_); + + // Checks a particular point is inside the box + auto check_in_box = [&](const Eigen::Vector3d& p) { + Eigen::Vector3d in_box = pos.inverse() * p; + + constexpr double eps = 1e-5; + EXPECT_LE(std::abs(in_box.x()), shape->size[0] + eps); + EXPECT_LE(std::abs(in_box.y()), shape->size[1] + eps); + EXPECT_LE(std::abs(in_box.z()), shape->size[2] + eps); + }; + + // Check that all points reported on "box" are actually on the box and not + // on the robot + for (auto& distance : res.distances) + { + for (auto& pair : distance.second) + { + if (pair.link_names[0] == "box") + { + check_in_box(pair.nearest_points[0]); + } + else if (pair.link_names[1] == "box") + { + check_in_box(pair.nearest_points[1]); + } + else + { + ADD_FAILURE() << "Unrecognized link names"; + } + } + } +} + +REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, + RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld); + +REGISTER_TYPED_TEST_SUITE_P(DistanceCheckPandaTest, DistanceSingle); + +REGISTER_TYPED_TEST_SUITE_P(DistanceFullPandaTest, DistancePoints); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index 660ee09012..563103c5aa 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -34,536 +46,6 @@ /* Author: E. Gil Jones */ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -template -class CollisionDetectorTest : public ::testing::Test -{ -public: - std::shared_ptr value_; - -protected: - CollisionDetectorTest() - { - } - - void SetUp() override - { - value_ = std::make_shared(); - robot_model_ = moveit::core::loadTestingRobotModel("pr2"); - robot_model_ok_ = static_cast(robot_model_); - kinect_dae_resource_ = "package://moveit_resources_pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; - - acm_ = std::make_shared(robot_model_->getLinkModelNames(), true); - - cenv_ = value_->allocateEnv(robot_model_); - } - - void TearDown() override - { - } - - bool robot_model_ok_; - - moveit::core::RobotModelPtr robot_model_; - - collision_detection::CollisionEnvPtr cenv_; - - collision_detection::AllowedCollisionMatrixPtr acm_; - - std::string kinect_dae_resource_; -}; - -#ifdef NDEBUG -#define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL) -#else // Don't perform timing checks in Debug mode (but evaluate expression) -#define EXPECT_TIME_LT(EXPR, VAL) static_cast(EXPR) -#endif - -TYPED_TEST_CASE_P(CollisionDetectorTest); - -TYPED_TEST_P(CollisionDetectorTest, InitOK) -{ - ASSERT_TRUE(this->robot_model_ok_); -} - -TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision) -{ - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_FALSE(res.collision); -} - -TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res1; - collision_detection::CollisionResult res2; - collision_detection::CollisionResult res3; - // req.contacts = true; - // req.max_contacts = 100; - - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); - offset.translation().x() = .01; - - // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); - robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("base_bellow_link", offset); - robot_state.update(); - - this->acm_->setEntry("base_link", "base_bellow_link", false); - this->cenv_->checkSelfCollision(req, res1, robot_state, *this->acm_); - ASSERT_TRUE(res1.collision); - - this->acm_->setEntry("base_link", "base_bellow_link", true); - this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); - ASSERT_FALSE(res2.collision); - - // req.verbose = true; - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); - robot_state.update(); - - this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - this->cenv_->checkSelfCollision(req, res3, robot_state, *this->acm_); - ASSERT_TRUE(res3.collision); -} - -TYPED_TEST_P(CollisionDetectorTest, ContactReporting) -{ - collision_detection::CollisionRequest req; - req.contacts = true; - req.max_contacts = 1; - - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); - offset.translation().x() = .01; - - // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); - - robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("base_bellow_link", offset); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); - robot_state.update(); - - this->acm_->setEntry("base_link", "base_bellow_link", false); - this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - - collision_detection::CollisionResult res; - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - EXPECT_EQ(res.contacts.size(), 1u); - EXPECT_EQ(res.contacts.begin()->second.size(), 1u); - - res.clear(); - req.max_contacts = 2; - req.max_contacts_per_pair = 1; - // req.verbose = true; - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - EXPECT_EQ(res.contacts.size(), 2u); - EXPECT_EQ(res.contacts.begin()->second.size(), 1u); - - res.contacts.clear(); - res.contact_count = 0; - - req.max_contacts = 10; - req.max_contacts_per_pair = 2; - this->acm_ = - std::make_shared(this->robot_model_->getLinkModelNames(), false); - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - EXPECT_LE(res.contacts.size(), 10u); - EXPECT_LE(res.contact_count, 10u); -} - -TYPED_TEST_P(CollisionDetectorTest, ContactPositions) -{ - collision_detection::CollisionRequest req; - req.contacts = true; - req.max_contacts = 1; - - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); - - pos1.translation().x() = 5.0; - pos2.translation().x() = 5.01; - - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); - robot_state.update(); - - this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); - - collision_detection::CollisionResult res; - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - ASSERT_EQ(res.contacts.size(), 1u); - ASSERT_EQ(res.contacts.begin()->second.size(), 1u); - - for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin(); - it != res.contacts.end(); ++it) - { - EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33); - } - - pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * - Eigen::Quaterniond(sqrt(1 - pow(0.258, 2)), 0.0, 0.258, 0.0)); - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); - robot_state.update(); - - collision_detection::CollisionResult res2; - this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); - ASSERT_TRUE(res2.collision); - ASSERT_EQ(res2.contacts.size(), 1u); - ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); - - for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin(); - it != res2.contacts.end(); ++it) - { - EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33); - } - - pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); - pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * - Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0).normalized()); - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); - robot_state.update(); - - collision_detection::CollisionResult res3; - this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); - ASSERT_FALSE(res3.collision); -} - -TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - - this->acm_ = - std::make_shared(this->robot_model_->getLinkModelNames(), true); - - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); - pos1.translation().x() = 5.0; - - // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); - robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); - robot_state.update(); - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_FALSE(res.collision); - - shapes::Shape* shape = new shapes::Box(.1, .1, .1); - this->cenv_->getWorld()->addToObject("box", pos1, shapes::ShapeConstPtr(shape), Eigen::Isometry3d::Identity()); - - res = collision_detection::CollisionResult(); - this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - - // deletes shape - this->cenv_->getWorld()->removeObject("box"); - - shape = new shapes::Box(.1, .1, .1); - std::vector shapes; - EigenSTL::vector_Isometry3d poses; - shapes.push_back(shapes::ShapeConstPtr(shape)); - poses.push_back(Eigen::Isometry3d::Identity()); - std::vector touch_links; - robot_state.attachBody("box", poses[0], shapes, poses, touch_links, "r_gripper_palm_link"); - - res = collision_detection::CollisionResult(); - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - - // deletes shape - robot_state.clearAttachedBody("box"); - - touch_links.push_back("r_gripper_palm_link"); - touch_links.push_back("r_gripper_motor_accelerometer_link"); - shapes[0] = std::make_shared(.1, .1, .1); - robot_state.attachBody("box", poses[0], shapes, poses, touch_links, "r_gripper_palm_link"); - robot_state.update(); - - res = collision_detection::CollisionResult(); - this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); - ASSERT_FALSE(res.collision); - - pos1.translation().x() = 5.01; - shapes::Shape* coll = new shapes::Box(.1, .1, .1); - this->cenv_->getWorld()->addToObject("coll", pos1, shapes::ShapeConstPtr(coll), Eigen::Isometry3d::Identity()); - res = collision_detection::CollisionResult(); - this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); - - this->acm_->setEntry("coll", "r_gripper_palm_link", true); - res = collision_detection::CollisionResult(); - this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); - ASSERT_TRUE(res.collision); -} - -TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) -{ - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - - collision_detection::CollisionEnvPtr new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); - - auto before = std::chrono::system_clock::now(); - new_cenv->checkSelfCollision(req, res, robot_state); - double first_check = - std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - before = std::chrono::system_clock::now(); - new_cenv->checkSelfCollision(req, res, robot_state); - double second_check = - std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - - EXPECT_TIME_LT(fabs(first_check - second_check), .05); - - std::vector shapes; - shapes.resize(1); - - shapes[0].reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); - - EigenSTL::vector_Isometry3d poses; - poses.push_back(Eigen::Isometry3d::Identity()); - - std::vector touch_links; - robot_state.attachBody("kinect", poses[0], shapes, poses, touch_links, "r_gripper_palm_link"); - - before = std::chrono::system_clock::now(); - new_cenv->checkSelfCollision(req, res, robot_state); - first_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - before = std::chrono::system_clock::now(); - new_cenv->checkSelfCollision(req, res, robot_state); - second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - - // the first check is going to take a while, as data must be constructed - EXPECT_TIME_LT(second_check, .1); - - collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); - before = std::chrono::system_clock::now(); - new_cenv->checkSelfCollision(req, res, robot_state); - first_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - before = std::chrono::system_clock::now(); - new_cenv->checkSelfCollision(req, res, robot_state); - second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - - EXPECT_TIME_LT(fabs(first_check - second_check), .05); -} - -TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) -{ - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - - shapes::ShapeConstPtr shape(shapes::createMeshFromResource(this->kinect_dae_resource_)); - Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); - pos2.translation().x() = 10.0; - - this->cenv_->getWorld()->addToObject("kinect", pos1, shape, Eigen::Isometry3d::Identity()); - - moveit::core::RobotState robot_state(this->robot_model_); - robot_state.setToDefaultValues(); - robot_state.update(); - - auto before = std::chrono::system_clock::now(); - this->cenv_->checkRobotCollision(req, res, robot_state); - double first_check = - std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - before = std::chrono::system_clock::now(); - this->cenv_->checkRobotCollision(req, res, robot_state); - double second_check = - std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - - EXPECT_TIME_LT(second_check, .05); - - collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); - this->cenv_->getWorld()->removeObject("kinect"); - - moveit::core::RobotState robot_state1(this->robot_model_); - moveit::core::RobotState robot_state2(this->robot_model_); - robot_state1.setToDefaultValues(); - robot_state2.setToDefaultValues(); - robot_state1.update(); - robot_state2.update(); - - std::vector touch_links; - Eigen::Isometry3d identity_transform{ Eigen::Isometry3d::Identity() }; - robot_state1.attachBody("kinect", identity_transform, object->shapes_, object->shape_poses_, touch_links, - "r_gripper_palm_link"); - - EigenSTL::vector_Isometry3d other_poses; - other_poses.push_back(pos2); - - // This creates a new set of constant properties for the attached body, which happens to be the same as the one above; - robot_state2.attachBody("kinect", identity_transform, object->shapes_, object->shape_poses_, touch_links, - "r_gripper_palm_link"); - - // going to take a while, but that's fine - res = collision_detection::CollisionResult(); - this->cenv_->checkSelfCollision(req, res, robot_state1); - - EXPECT_TRUE(res.collision); - - before = std::chrono::system_clock::now(); - this->cenv_->checkSelfCollision(req, res, robot_state1, *this->acm_); - first_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - before = std::chrono::system_clock::now(); - req.verbose = true; - res = collision_detection::CollisionResult(); - this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_); - second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); - - EXPECT_TIME_LT(first_check, .05); - EXPECT_TIME_LT(fabs(first_check - second_check), .1); -} - -TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) -{ - EigenSTL::vector_Isometry3d poses; - std::vector shapes; - for (unsigned int i = 0; i < 10000; ++i) - { - poses.push_back(Eigen::Isometry3d::Identity()); - shapes.push_back(std::make_shared(.01, .01, .01)); - } - auto start = std::chrono::system_clock::now(); - this->cenv_->getWorld()->addToObject("map", shapes, poses); - double t = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); - // TODO (j-petit): investigate why bullet collision checking is considerably slower here - EXPECT_TIME_LT(t, 5.0); - // this is not really a failure; it is just that slow; - // looking into doing collision checking with a voxel grid. - RCLCPP_INFO(rclcpp::get_logger("moveit.core.collision_detection.bullet"), "Adding boxes took %g", t); -} - -TYPED_TEST_P(CollisionDetectorTest, MoveMesh) -{ - moveit::core::RobotState robot_state1(this->robot_model_); - robot_state1.setToDefaultValues(); - robot_state1.update(); - - Eigen::Isometry3d kinect_pose; - kinect_pose.setIdentity(); - shapes::ShapePtr kinect_shape; - kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); - - this->cenv_->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(); - this->cenv_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); - } -} - -TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) -{ - moveit::core::RobotState robot_state1(this->robot_model_); - robot_state1.setToDefaultValues(); - robot_state1.update(); - - collision_detection::CollisionRequest req1; - collision_detection::CollisionResult res1; - - ASSERT_FALSE(res1.collision); - - EigenSTL::vector_Isometry3d poses; - std::vector shapes; - for (unsigned int i = 0; i < 5; ++i) - { - this->cenv_->getWorld()->removeObject("shape"); - shapes.clear(); - poses.clear(); - shapes.push_back(std::make_shared(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)); - poses.push_back(Eigen::Isometry3d::Identity()); - this->cenv_->getWorld()->addToObject("shape", shapes, poses); - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); - ASSERT_TRUE(res.collision); - } - - Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); - shapes::ShapePtr kinect_shape; - kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); - this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); - collision_detection::CollisionRequest req2; - collision_detection::CollisionResult res2; - this->cenv_->checkCollision(req2, res2, robot_state1, *this->acm_); - ASSERT_TRUE(res2.collision); - for (unsigned int i = 0; i < 5; ++i) - { - this->cenv_->getWorld()->removeObject("shape"); - shapes.clear(); - poses.clear(); - shapes.push_back(std::make_shared(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)); - poses.push_back(Eigen::Isometry3d::Identity()); - this->cenv_->getWorld()->addToObject("shape", shapes, poses); - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); - ASSERT_TRUE(res.collision); - } -} - -REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, - ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, - TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize); +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.hpp new file mode 100644 index 0000000000..0a652d0b78 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.hpp @@ -0,0 +1,571 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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: E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +template +class CollisionDetectorTest : public ::testing::Test +{ +public: + std::shared_ptr value_; + +protected: + CollisionDetectorTest() + { + } + + void SetUp() override + { + value_ = std::make_shared(); + robot_model_ = moveit::core::loadTestingRobotModel("pr2"); + robot_model_ok_ = static_cast(robot_model_); + kinect_dae_resource_ = "package://moveit_resources_pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae"; + + acm_ = std::make_shared(robot_model_->getLinkModelNames(), true); + + cenv_ = value_->allocateEnv(robot_model_); + } + + void TearDown() override + { + } + + bool robot_model_ok_; + + moveit::core::RobotModelPtr robot_model_; + + collision_detection::CollisionEnvPtr cenv_; + + collision_detection::AllowedCollisionMatrixPtr acm_; + + std::string kinect_dae_resource_; +}; + +#ifdef NDEBUG +#define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL) +#else // Don't perform timing checks in Debug mode (but evaluate expression) +#define EXPECT_TIME_LT(EXPR, VAL) static_cast(EXPR) +#endif + +TYPED_TEST_CASE_P(CollisionDetectorTest); + +TYPED_TEST_P(CollisionDetectorTest, InitOK) +{ + ASSERT_TRUE(this->robot_model_ok_); +} + +TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision) +{ + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_FALSE(res.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, LinksInCollision) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res1; + collision_detection::CollisionResult res2; + collision_detection::CollisionResult res3; + // req.contacts = true; + // req.max_contacts = 100; + + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); + offset.translation().x() = .01; + + // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); + robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("base_bellow_link", offset); + robot_state.update(); + + this->acm_->setEntry("base_link", "base_bellow_link", false); + this->cenv_->checkSelfCollision(req, res1, robot_state, *this->acm_); + ASSERT_TRUE(res1.collision); + + this->acm_->setEntry("base_link", "base_bellow_link", true); + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); + ASSERT_FALSE(res2.collision); + + // req.verbose = true; + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); + robot_state.update(); + + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + this->cenv_->checkSelfCollision(req, res3, robot_state, *this->acm_); + ASSERT_TRUE(res3.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, ContactReporting) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 1; + + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d offset = Eigen::Isometry3d::Identity(); + offset.translation().x() = .01; + + // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset); + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity()); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset); + + robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("base_bellow_link", offset); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity()); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset); + robot_state.update(); + + this->acm_->setEntry("base_link", "base_bellow_link", false); + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + EXPECT_EQ(res.contacts.size(), 1u); + EXPECT_EQ(res.contacts.begin()->second.size(), 1u); + + res.clear(); + req.max_contacts = 2; + req.max_contacts_per_pair = 1; + // req.verbose = true; + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + EXPECT_EQ(res.contacts.size(), 2u); + EXPECT_EQ(res.contacts.begin()->second.size(), 1u); + + res.contacts.clear(); + res.contact_count = 0; + + req.max_contacts = 10; + req.max_contacts_per_pair = 2; + this->acm_ = + std::make_shared(this->robot_model_->getLinkModelNames(), false); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + EXPECT_LE(res.contacts.size(), 10u); + EXPECT_LE(res.contact_count, 10u); +} + +TYPED_TEST_P(CollisionDetectorTest, ContactPositions) +{ + collision_detection::CollisionRequest req; + req.contacts = true; + req.max_contacts = 1; + + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); + + pos1.translation().x() = 5.0; + pos2.translation().x() = 5.01; + + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); + robot_state.update(); + + this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false); + + collision_detection::CollisionResult res; + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + ASSERT_EQ(res.contacts.size(), 1u); + ASSERT_EQ(res.contacts.begin()->second.size(), 1u); + + for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin(); + it != res.contacts.end(); ++it) + { + EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33); + } + + pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * + Eigen::Quaterniond(sqrt(1 - pow(0.258, 2)), 0.0, 0.258, 0.0)); + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); + robot_state.update(); + + collision_detection::CollisionResult res2; + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); + ASSERT_TRUE(res2.collision); + ASSERT_EQ(res2.contacts.size(), 1u); + ASSERT_EQ(res2.contacts.begin()->second.size(), 1u); + + for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin(); + it != res2.contacts.end(); ++it) + { + EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33); + } + + pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * + Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0).normalized()); + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2); + robot_state.update(); + + collision_detection::CollisionResult res3; + this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_); + ASSERT_FALSE(res3.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + this->acm_ = + std::make_shared(this->robot_model_->getLinkModelNames(), true); + + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + pos1.translation().x() = 5.0; + + // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1); + robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1); + robot_state.update(); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_FALSE(res.collision); + + shapes::Shape* shape = new shapes::Box(.1, .1, .1); + this->cenv_->getWorld()->addToObject("box", pos1, shapes::ShapeConstPtr(shape), Eigen::Isometry3d::Identity()); + + res = collision_detection::CollisionResult(); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + + // deletes shape + this->cenv_->getWorld()->removeObject("box"); + + shape = new shapes::Box(.1, .1, .1); + std::vector shapes; + EigenSTL::vector_Isometry3d poses; + shapes.push_back(shapes::ShapeConstPtr(shape)); + poses.push_back(Eigen::Isometry3d::Identity()); + std::vector touch_links; + robot_state.attachBody("box", poses[0], shapes, poses, touch_links, "r_gripper_palm_link"); + + res = collision_detection::CollisionResult(); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + + // deletes shape + robot_state.clearAttachedBody("box"); + + touch_links.push_back("r_gripper_palm_link"); + touch_links.push_back("r_gripper_motor_accelerometer_link"); + shapes[0] = std::make_shared(.1, .1, .1); + robot_state.attachBody("box", poses[0], shapes, poses, touch_links, "r_gripper_palm_link"); + robot_state.update(); + + res = collision_detection::CollisionResult(); + this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_); + ASSERT_FALSE(res.collision); + + pos1.translation().x() = 5.01; + shapes::Shape* coll = new shapes::Box(.1, .1, .1); + this->cenv_->getWorld()->addToObject("coll", pos1, shapes::ShapeConstPtr(coll), Eigen::Isometry3d::Identity()); + res = collision_detection::CollisionResult(); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); + + this->acm_->setEntry("coll", "r_gripper_palm_link", true); + res = collision_detection::CollisionResult(); + this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_); + ASSERT_TRUE(res.collision); +} + +TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester) +{ + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + collision_detection::CollisionEnvPtr new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); + + auto before = std::chrono::system_clock::now(); + new_cenv->checkSelfCollision(req, res, robot_state); + double first_check = + std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + before = std::chrono::system_clock::now(); + new_cenv->checkSelfCollision(req, res, robot_state); + double second_check = + std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + + EXPECT_TIME_LT(fabs(first_check - second_check), .05); + + std::vector shapes; + shapes.resize(1); + + shapes[0].reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + + EigenSTL::vector_Isometry3d poses; + poses.push_back(Eigen::Isometry3d::Identity()); + + std::vector touch_links; + robot_state.attachBody("kinect", poses[0], shapes, poses, touch_links, "r_gripper_palm_link"); + + before = std::chrono::system_clock::now(); + new_cenv->checkSelfCollision(req, res, robot_state); + first_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + before = std::chrono::system_clock::now(); + new_cenv->checkSelfCollision(req, res, robot_state); + second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + + // the first check is going to take a while, as data must be constructed + EXPECT_TIME_LT(second_check, .1); + + collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld()); + before = std::chrono::system_clock::now(); + new_cenv->checkSelfCollision(req, res, robot_state); + first_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + before = std::chrono::system_clock::now(); + new_cenv->checkSelfCollision(req, res, robot_state); + second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + + EXPECT_TIME_LT(fabs(first_check - second_check), .05); +} + +TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached) +{ + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + + shapes::ShapeConstPtr shape(shapes::createMeshFromResource(this->kinect_dae_resource_)); + Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity(); + pos2.translation().x() = 10.0; + + this->cenv_->getWorld()->addToObject("kinect", pos1, shape, Eigen::Isometry3d::Identity()); + + moveit::core::RobotState robot_state(this->robot_model_); + robot_state.setToDefaultValues(); + robot_state.update(); + + auto before = std::chrono::system_clock::now(); + this->cenv_->checkRobotCollision(req, res, robot_state); + double first_check = + std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + before = std::chrono::system_clock::now(); + this->cenv_->checkRobotCollision(req, res, robot_state); + double second_check = + std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + + EXPECT_TIME_LT(second_check, .05); + + collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect"); + this->cenv_->getWorld()->removeObject("kinect"); + + moveit::core::RobotState robot_state1(this->robot_model_); + moveit::core::RobotState robot_state2(this->robot_model_); + robot_state1.setToDefaultValues(); + robot_state2.setToDefaultValues(); + robot_state1.update(); + robot_state2.update(); + + std::vector touch_links; + Eigen::Isometry3d identity_transform{ Eigen::Isometry3d::Identity() }; + robot_state1.attachBody("kinect", identity_transform, object->shapes_, object->shape_poses_, touch_links, + "r_gripper_palm_link"); + + EigenSTL::vector_Isometry3d other_poses; + other_poses.push_back(pos2); + + // This creates a new set of constant properties for the attached body, which happens to be the same as the one above; + robot_state2.attachBody("kinect", identity_transform, object->shapes_, object->shape_poses_, touch_links, + "r_gripper_palm_link"); + + // going to take a while, but that's fine + res = collision_detection::CollisionResult(); + this->cenv_->checkSelfCollision(req, res, robot_state1); + + EXPECT_TRUE(res.collision); + + before = std::chrono::system_clock::now(); + this->cenv_->checkSelfCollision(req, res, robot_state1, *this->acm_); + first_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + before = std::chrono::system_clock::now(); + req.verbose = true; + res = collision_detection::CollisionResult(); + this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_); + second_check = std::chrono::duration_cast(std::chrono::system_clock::now() - before).count(); + + EXPECT_TIME_LT(first_check, .05); + EXPECT_TIME_LT(fabs(first_check - second_check), .1); +} + +TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed) +{ + EigenSTL::vector_Isometry3d poses; + std::vector shapes; + for (unsigned int i = 0; i < 10000; ++i) + { + poses.push_back(Eigen::Isometry3d::Identity()); + shapes.push_back(std::make_shared(.01, .01, .01)); + } + auto start = std::chrono::system_clock::now(); + this->cenv_->getWorld()->addToObject("map", shapes, poses); + double t = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); + // TODO (j-petit): investigate why bullet collision checking is considerably slower here + EXPECT_TIME_LT(t, 5.0); + // this is not really a failure; it is just that slow; + // looking into doing collision checking with a voxel grid. + RCLCPP_INFO(rclcpp::get_logger("moveit.core.collision_detection.bullet"), "Adding boxes took %g", t); +} + +TYPED_TEST_P(CollisionDetectorTest, MoveMesh) +{ + moveit::core::RobotState robot_state1(this->robot_model_); + robot_state1.setToDefaultValues(); + robot_state1.update(); + + Eigen::Isometry3d kinect_pose; + kinect_pose.setIdentity(); + shapes::ShapePtr kinect_shape; + kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + + this->cenv_->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(); + this->cenv_->getWorld()->moveShapeInObject("kinect", kinect_shape, np); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); + } +} + +TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize) +{ + moveit::core::RobotState robot_state1(this->robot_model_); + robot_state1.setToDefaultValues(); + robot_state1.update(); + + collision_detection::CollisionRequest req1; + collision_detection::CollisionResult res1; + + ASSERT_FALSE(res1.collision); + + EigenSTL::vector_Isometry3d poses; + std::vector shapes; + for (unsigned int i = 0; i < 5; ++i) + { + this->cenv_->getWorld()->removeObject("shape"); + shapes.clear(); + poses.clear(); + shapes.push_back(std::make_shared(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)); + poses.push_back(Eigen::Isometry3d::Identity()); + this->cenv_->getWorld()->addToObject("shape", shapes, poses); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); + ASSERT_TRUE(res.collision); + } + + Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity(); + shapes::ShapePtr kinect_shape; + kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_)); + this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose); + collision_detection::CollisionRequest req2; + collision_detection::CollisionResult res2; + this->cenv_->checkCollision(req2, res2, robot_state1, *this->acm_); + ASSERT_TRUE(res2.collision); + for (unsigned int i = 0; i < 5; ++i) + { + this->cenv_->getWorld()->removeObject("shape"); + shapes.clear(); + poses.clear(); + shapes.push_back(std::make_shared(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)); + poses.push_back(Eigen::Isometry3d::Identity()); + this->cenv_->getWorld()->addToObject("shape", shapes, poses); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + this->cenv_->checkCollision(req, res, robot_state1, *this->acm_); + ASSERT_TRUE(res.collision); + } +} + +REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, + ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, + TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize); 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 d610d4b43c..9a91e74a33 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,320 +47,5 @@ /* Author: Ioan Sucan, Acorn Pooley, Sachin Chitta */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace shapes -{ -MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc -} - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(World); // Defines WorldPtr, ConstPtr, WeakPtr... etc - -/** \brief Maintain a representation of the environment */ -class World -{ -public: - /** \brief Constructor */ - World(); - - /** \brief A copy constructor. - * \e other should not be changed while the copy constructor is running - * This does copy on write and should be quick. */ - World(const World& other); - - virtual ~World(); - - /**********************************************************************/ - /* Collision Bodies */ - /**********************************************************************/ - - MOVEIT_STRUCT_FORWARD(Object); - - /** \brief A representation of an object */ - struct Object - { - Object(const std::string& object_id) : id_(object_id) - { - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief The id for this object */ - std::string id_; - - /** \brief The object's pose. All shapes and subframes are defined relative to this frame. - * This frame is returned when getTransform() is called with the object's name. */ - Eigen::Isometry3d pose_; - - /** \brief All the shapes making up this object. - * - * The pose of each Shape is stored in the corresponding element of the shape_poses_ array. */ - std::vector shapes_; - - /** \brief The poses of the corresponding entries in shapes_, relative to the object pose. - * - * @copydetails shapes_ */ - EigenSTL::vector_Isometry3d shape_poses_; - - /** \brief The poses of the corresponding entries in shapes_, relative to the world frame. - * - * @copydetails shapes_ */ - EigenSTL::vector_Isometry3d global_shape_poses_; - - /** \brief Transforms from the object pose to subframes on the object. - * Use them to define points of interest on an object to plan with - * (e.g. screwdriver/tip, kettle/spout, mug/base). - */ - moveit::core::FixedTransformsMap subframe_poses_; - - /** \brief Transforms from the world frame to the object subframes. - */ - moveit::core::FixedTransformsMap global_subframe_poses_; - }; - - /** \brief Get the list of Object ids */ - std::vector getObjectIds() const; - - /** \brief Get a particular object */ - ObjectConstPtr getObject(const std::string& object_id) const; - - /** iterator over the objects in the world. */ - using const_iterator = std::map::const_iterator; - /** iterator pointing to first change */ - const_iterator begin() const - { - return objects_.begin(); - } - /** iterator pointing to end of changes */ - const_iterator end() const - { - return objects_.end(); - } - /** number of changes stored */ - std::size_t size() const - { - return objects_.size(); - } - /** find changes for a named object */ - const_iterator find(const std::string& object_id) const - { - return objects_.find(object_id); - } - - /** \brief Check if a particular object exists in the collision world*/ - bool hasObject(const std::string& object_id) const; - - /** \brief Check if an object or subframe with given name exists in the collision world. - * A subframe name needs to be prefixed with the object's name separated by a slash. */ - bool knowsTransform(const std::string& name) const; - - /** \brief Get the transform to an object or subframe with given name. - * If name does not exist, a std::runtime_error is thrown. - * A subframe name needs to be prefixed with the object's name separated by a slash. - * The transform is global (relative to the world origin). - * The returned transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getTransform(const std::string& name) const; - - /** \brief Get the transform to an object or subframe with given name. - * If name does not exist, returns an identity transform and sets frame_found to false. - * A subframe name needs to be prefixed with the object's name separated by a slash. - * The transform is global (relative to the world origin). - * The returned transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const; - - /** \brief Get the global transform to a shape of an object with multiple shapes. - * shape_index is the index of the object (counting from 0) and needs to be valid. - * This function is used to construct the collision environment. */ - const Eigen::Isometry3d& getGlobalShapeTransform(const std::string& object_id, int shape_index) const; - - /** \brief Get the global transforms to the shapes of an object. - * This function is used to construct the collision environment. */ - const EigenSTL::vector_Isometry3d& getGlobalShapeTransforms(const std::string& object_id) const; - - /** \brief Add a pose and shapes to an object in the map. - * This function makes repeated calls to addToObjectInternal() to add the - * shapes one by one.*/ - void addToObject(const std::string& object_id, const Eigen::Isometry3d& pose, - const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses); - - /** \brief Add shapes to an object in the map. - * This function makes repeated calls to addToObjectInternal() to add the - * shapes one by one. */ - void addToObject(const std::string& object_id, const std::vector& shapes, - const EigenSTL::vector_Isometry3d& shape_poses) - { - addToObject(object_id, Eigen::Isometry3d::Identity(), shapes, shape_poses); - } - - /** \brief Add a pose and shape to an object. - * If the object already exists, this call will add the shape to the object - * at the specified pose. Otherwise, the object is created and the - * specified shape is added. This calls addToObjectInternal(). - * shape_pose is defined relative to the object's pose, not to the world frame. */ - void addToObject(const std::string& object_id, const Eigen::Isometry3d& pose, const shapes::ShapeConstPtr& shape, - const Eigen::Isometry3d& shape_pose) - { - addToObject(object_id, pose, std::vector{ shape }, EigenSTL::vector_Isometry3d{ shape_pose }); - } - - /** \brief Add a shape to an object. - * If the object already exists, this call will add the shape to the object - * at the specified pose. Otherwise, the object is created and the - * specified shape is added. This calls addToObjectInternal(). - * shape_pose is defined relative to the object's pose, not to the world frame. */ - void addToObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose) - { - addToObject(object_id, Eigen::Isometry3d::Identity(), std::vector{ shape }, - EigenSTL::vector_Isometry3d{ shape_pose }); - } - - /** \brief Update the pose of a shape in an object. Shape equality is - * verified by comparing pointers. Returns true on success. */ - bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, - const Eigen::Isometry3d& shape_pose); - - /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */ - bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses); - - /** \brief Move the object pose (thus moving all shapes and subframes in the object) - * according to the given transform specified in world frame. - * The transform is relative to and changes the object pose. It does not replace it. - */ - bool moveObject(const std::string& object_id, const Eigen::Isometry3d& transform); - - /** \brief Set the pose of an object. The pose is specified in the world frame. */ - bool setObjectPose(const std::string& object_id, const Eigen::Isometry3d& pose); - - /** \brief Remove shape from object. - * Shape equality is verified by comparing pointers. Ownership of the - * object is renounced (i.e. object is deleted if no external references - * exist) if this was the last shape in the object. - * Returns true on success and false if the object did not exist or did not - * contain the shape. */ - bool removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape); - - /** \brief Remove a particular object. - * If there are no external pointers to the corresponding instance of - * Object, the memory is freed. - * Returns true on success and false if no such object was found. */ - bool removeObject(const std::string& object_id); - - /** \brief Set subframes on an object. The frames are relative to the object pose. */ - bool setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses); - - /** \brief Clear all objects. - * If there are no other pointers to corresponding instances of Objects, - * the memory is freed. */ - void clearObjects(); - - enum ActionBits - { - UNINITIALIZED = 0, - CREATE = 1, /** object was created */ - DESTROY = 2, /** object was destroyed */ - MOVE_SHAPE = 4, /** one or more shapes in object were moved */ - ADD_SHAPE = 8, /** shape(s) were added to object */ - REMOVE_SHAPE = 16, /** shape(s) were removed from object */ - }; - - /** \brief Represents an action that occurred on an object in the world. - * Several bits may be set indicating several things happened to the object. - * If the DESTROY bit is set, other bits will not be set. */ - class Action - { - public: - Action() : action_(UNINITIALIZED) - { - } - Action(int v) : action_(v) - { - } - operator ActionBits() const - { - return ActionBits(action_); - } - - private: - int action_; - }; - -private: - class Observer; - -public: - class ObserverHandle - { - public: - ObserverHandle() : observer_(nullptr) - { - } - - private: - ObserverHandle(const Observer* o) : observer_(o) - { - } - const Observer* observer_; - friend class World; - }; - - using ObserverCallbackFn = std::function; - - /** \brief register a callback function for notification of changes. - * \e callback will be called right after any change occurs to any Object. - * \e observer is the object which is requesting the changes. It is only - * used for identifying the callback in removeObserver(). */ - ObserverHandle addObserver(const ObserverCallbackFn& callback); - - /** \brief remove a notifier callback */ - void removeObserver(const ObserverHandle observer_handle); - - /** send notification of change to all objects to a particular observer. - * Used which switching from one world to another. */ - void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const; - -private: - /** notify all observers of a change */ - void notify(const ObjectConstPtr& /*obj*/, Action /*action*/); - - /** send notification of change to all objects. */ - void notifyAll(Action action); - - /** \brief Make sure that the object named \e id is known only to this - * instance of the World. If the object is known outside of it, a - * clone is made so that it can be safely modified later on. */ - void ensureUnique(ObjectPtr& obj); - - /* Add a shape with no checking */ - virtual void addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape, - const Eigen::Isometry3d& shape_pose); - - /** \brief Updates the global shape and subframe poses. */ - void updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses = true, bool update_subframe_poses = true); - - /** The objects maintained in the world */ - std::map objects_; - - /** Wrapper for a callback function to call when something changes in the world */ - class Observer - { - public: - Observer(const ObserverCallbackFn& callback) : callback_(callback) - { - } - ObserverCallbackFn callback_; - }; - - /// All registered observers of this world representation - std::vector observers_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/world.hpp new file mode 100644 index 0000000000..a3f41643b3 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.hpp @@ -0,0 +1,354 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Acorn Pooley, Sachin Chitta */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace shapes +{ +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc +} + +namespace collision_detection +{ +MOVEIT_CLASS_FORWARD(World); // Defines WorldPtr, ConstPtr, WeakPtr... etc + +/** \brief Maintain a representation of the environment */ +class World +{ +public: + /** \brief Constructor */ + World(); + + /** \brief A copy constructor. + * \e other should not be changed while the copy constructor is running + * This does copy on write and should be quick. */ + World(const World& other); + + virtual ~World(); + + /**********************************************************************/ + /* Collision Bodies */ + /**********************************************************************/ + + MOVEIT_STRUCT_FORWARD(Object); + + /** \brief A representation of an object */ + struct Object + { + Object(const std::string& object_id) : id_(object_id) + { + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief The id for this object */ + std::string id_; + + /** \brief The object's pose. All shapes and subframes are defined relative to this frame. + * This frame is returned when getTransform() is called with the object's name. */ + Eigen::Isometry3d pose_; + + /** \brief All the shapes making up this object. + * + * The pose of each Shape is stored in the corresponding element of the shape_poses_ array. */ + std::vector shapes_; + + /** \brief The poses of the corresponding entries in shapes_, relative to the object pose. + * + * @copydetails shapes_ */ + EigenSTL::vector_Isometry3d shape_poses_; + + /** \brief The poses of the corresponding entries in shapes_, relative to the world frame. + * + * @copydetails shapes_ */ + EigenSTL::vector_Isometry3d global_shape_poses_; + + /** \brief Transforms from the object pose to subframes on the object. + * Use them to define points of interest on an object to plan with + * (e.g. screwdriver/tip, kettle/spout, mug/base). + */ + moveit::core::FixedTransformsMap subframe_poses_; + + /** \brief Transforms from the world frame to the object subframes. + */ + moveit::core::FixedTransformsMap global_subframe_poses_; + }; + + /** \brief Get the list of Object ids */ + std::vector getObjectIds() const; + + /** \brief Get a particular object */ + ObjectConstPtr getObject(const std::string& object_id) const; + + /** iterator over the objects in the world. */ + using const_iterator = std::map::const_iterator; + /** iterator pointing to first change */ + const_iterator begin() const + { + return objects_.begin(); + } + /** iterator pointing to end of changes */ + const_iterator end() const + { + return objects_.end(); + } + /** number of changes stored */ + std::size_t size() const + { + return objects_.size(); + } + /** find changes for a named object */ + const_iterator find(const std::string& object_id) const + { + return objects_.find(object_id); + } + + /** \brief Check if a particular object exists in the collision world*/ + bool hasObject(const std::string& object_id) const; + + /** \brief Check if an object or subframe with given name exists in the collision world. + * A subframe name needs to be prefixed with the object's name separated by a slash. */ + bool knowsTransform(const std::string& name) const; + + /** \brief Get the transform to an object or subframe with given name. + * If name does not exist, a std::runtime_error is thrown. + * A subframe name needs to be prefixed with the object's name separated by a slash. + * The transform is global (relative to the world origin). + * The returned transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getTransform(const std::string& name) const; + + /** \brief Get the transform to an object or subframe with given name. + * If name does not exist, returns an identity transform and sets frame_found to false. + * A subframe name needs to be prefixed with the object's name separated by a slash. + * The transform is global (relative to the world origin). + * The returned transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const; + + /** \brief Get the global transform to a shape of an object with multiple shapes. + * shape_index is the index of the object (counting from 0) and needs to be valid. + * This function is used to construct the collision environment. */ + const Eigen::Isometry3d& getGlobalShapeTransform(const std::string& object_id, int shape_index) const; + + /** \brief Get the global transforms to the shapes of an object. + * This function is used to construct the collision environment. */ + const EigenSTL::vector_Isometry3d& getGlobalShapeTransforms(const std::string& object_id) const; + + /** \brief Add a pose and shapes to an object in the map. + * This function makes repeated calls to addToObjectInternal() to add the + * shapes one by one.*/ + void addToObject(const std::string& object_id, const Eigen::Isometry3d& pose, + const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses); + + /** \brief Add shapes to an object in the map. + * This function makes repeated calls to addToObjectInternal() to add the + * shapes one by one. */ + void addToObject(const std::string& object_id, const std::vector& shapes, + const EigenSTL::vector_Isometry3d& shape_poses) + { + addToObject(object_id, Eigen::Isometry3d::Identity(), shapes, shape_poses); + } + + /** \brief Add a pose and shape to an object. + * If the object already exists, this call will add the shape to the object + * at the specified pose. Otherwise, the object is created and the + * specified shape is added. This calls addToObjectInternal(). + * shape_pose is defined relative to the object's pose, not to the world frame. */ + void addToObject(const std::string& object_id, const Eigen::Isometry3d& pose, const shapes::ShapeConstPtr& shape, + const Eigen::Isometry3d& shape_pose) + { + addToObject(object_id, pose, std::vector{ shape }, EigenSTL::vector_Isometry3d{ shape_pose }); + } + + /** \brief Add a shape to an object. + * If the object already exists, this call will add the shape to the object + * at the specified pose. Otherwise, the object is created and the + * specified shape is added. This calls addToObjectInternal(). + * shape_pose is defined relative to the object's pose, not to the world frame. */ + void addToObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose) + { + addToObject(object_id, Eigen::Isometry3d::Identity(), std::vector{ shape }, + EigenSTL::vector_Isometry3d{ shape_pose }); + } + + /** \brief Update the pose of a shape in an object. Shape equality is + * verified by comparing pointers. Returns true on success. */ + bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, + const Eigen::Isometry3d& shape_pose); + + /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */ + bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses); + + /** \brief Move the object pose (thus moving all shapes and subframes in the object) + * according to the given transform specified in world frame. + * The transform is relative to and changes the object pose. It does not replace it. + */ + bool moveObject(const std::string& object_id, const Eigen::Isometry3d& transform); + + /** \brief Set the pose of an object. The pose is specified in the world frame. */ + bool setObjectPose(const std::string& object_id, const Eigen::Isometry3d& pose); + + /** \brief Remove shape from object. + * Shape equality is verified by comparing pointers. Ownership of the + * object is renounced (i.e. object is deleted if no external references + * exist) if this was the last shape in the object. + * Returns true on success and false if the object did not exist or did not + * contain the shape. */ + bool removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape); + + /** \brief Remove a particular object. + * If there are no external pointers to the corresponding instance of + * Object, the memory is freed. + * Returns true on success and false if no such object was found. */ + bool removeObject(const std::string& object_id); + + /** \brief Set subframes on an object. The frames are relative to the object pose. */ + bool setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses); + + /** \brief Clear all objects. + * If there are no other pointers to corresponding instances of Objects, + * the memory is freed. */ + void clearObjects(); + + enum ActionBits + { + UNINITIALIZED = 0, + CREATE = 1, /** object was created */ + DESTROY = 2, /** object was destroyed */ + MOVE_SHAPE = 4, /** one or more shapes in object were moved */ + ADD_SHAPE = 8, /** shape(s) were added to object */ + REMOVE_SHAPE = 16, /** shape(s) were removed from object */ + }; + + /** \brief Represents an action that occurred on an object in the world. + * Several bits may be set indicating several things happened to the object. + * If the DESTROY bit is set, other bits will not be set. */ + class Action + { + public: + Action() : action_(UNINITIALIZED) + { + } + Action(int v) : action_(v) + { + } + operator ActionBits() const + { + return ActionBits(action_); + } + + private: + int action_; + }; + +private: + class Observer; + +public: + class ObserverHandle + { + public: + ObserverHandle() : observer_(nullptr) + { + } + + private: + ObserverHandle(const Observer* o) : observer_(o) + { + } + const Observer* observer_; + friend class World; + }; + + using ObserverCallbackFn = std::function; + + /** \brief register a callback function for notification of changes. + * \e callback will be called right after any change occurs to any Object. + * \e observer is the object which is requesting the changes. It is only + * used for identifying the callback in removeObserver(). */ + ObserverHandle addObserver(const ObserverCallbackFn& callback); + + /** \brief remove a notifier callback */ + void removeObserver(const ObserverHandle observer_handle); + + /** send notification of change to all objects to a particular observer. + * Used which switching from one world to another. */ + void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const; + +private: + /** notify all observers of a change */ + void notify(const ObjectConstPtr& /*obj*/, Action /*action*/); + + /** send notification of change to all objects. */ + void notifyAll(Action action); + + /** \brief Make sure that the object named \e id is known only to this + * instance of the World. If the object is known outside of it, a + * clone is made so that it can be safely modified later on. */ + void ensureUnique(ObjectPtr& obj); + + /* Add a shape with no checking */ + virtual void addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape, + const Eigen::Isometry3d& shape_pose); + + /** \brief Updates the global shape and subframe poses. */ + void updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses = true, bool update_subframe_poses = true); + + /** The objects maintained in the world */ + std::map objects_; + + /** Wrapper for a callback function to call when something changes in the world */ + class Observer + { + public: + Observer(const ObserverCallbackFn& callback) : callback_(callback) + { + } + ObserverCallbackFn callback_; + }; + + /// All registered observers of this world representation + std::vector observers_; +}; +} // namespace collision_detection 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 eed71965cc..97ec35ab19 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,95 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan, Sachin Chitta */ #pragma once - -#include -#include - -namespace collision_detection -{ -MOVEIT_CLASS_FORWARD(WorldDiff); // Defines WorldDiffPtr, ConstPtr, WeakPtr... etc - -/** \brief Maintain a diff list of changes that have happened to a World. */ -class WorldDiff -{ -public: - /** \brief Constructor */ - WorldDiff(); - - /** \brief Constructor */ - WorldDiff(const WorldPtr& world); - - /** \brief copy constructor. */ - WorldDiff(WorldDiff& other); - - ~WorldDiff(); - - /** \brief Set which world to record. Records all objects in old world (if - * any) as DESTROYED and all objects in new world as CREATED and ADD_SHAPE - * */ - void setWorld(const WorldPtr& world); - - /** \brief Set which world to record. Erases all previously recorded - * changes. */ - void reset(const WorldPtr& world); - - /** \brief Turn off recording and erase all previously recorded changes. */ - void reset(); - - /** \brief Return all the changes that have been recorded */ - const std::map& getChanges() const - { - return changes_; - } - - using const_iterator = std::map::const_iterator; - /** iterator pointing to first change */ - const_iterator begin() const - { - return changes_.begin(); - } - /** iterator pointing to end of changes */ - const_iterator end() const - { - return changes_.end(); - } - /** number of changes stored */ - size_t size() const - { - return changes_.size(); - } - /** find changes for a named object */ - const_iterator find(const std::string& id) const - { - return changes_.find(id); - } - /** set the entry for an id */ - void set(const std::string& id, World::Action val) - { - if (val) - { - changes_[id] = val; - } - else - { - changes_.erase(id); - } - } - - /** \brief Clear the internally maintained vector of changes */ - void clearChanges(); - -private: - /** \brief Notification function */ - void notify(const World::ObjectConstPtr& /*obj*/, World::Action /*action*/); - - /** keep changes in a map so they can be coalesced */ - std::map changes_; - - /* observer handle for world callback */ - World::ObserverHandle observer_handle_; - - /* used to unregister the notifier */ - WorldWeakPtr world_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.hpp new file mode 100644 index 0000000000..7e35a692a5 --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.hpp @@ -0,0 +1,129 @@ +/********************************************************************* + * 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: Acorn Pooley, Ioan Sucan, Sachin Chitta */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +MOVEIT_CLASS_FORWARD(WorldDiff); // Defines WorldDiffPtr, ConstPtr, WeakPtr... etc + +/** \brief Maintain a diff list of changes that have happened to a World. */ +class WorldDiff +{ +public: + /** \brief Constructor */ + WorldDiff(); + + /** \brief Constructor */ + WorldDiff(const WorldPtr& world); + + /** \brief copy constructor. */ + WorldDiff(WorldDiff& other); + + ~WorldDiff(); + + /** \brief Set which world to record. Records all objects in old world (if + * any) as DESTROYED and all objects in new world as CREATED and ADD_SHAPE + * */ + void setWorld(const WorldPtr& world); + + /** \brief Set which world to record. Erases all previously recorded + * changes. */ + void reset(const WorldPtr& world); + + /** \brief Turn off recording and erase all previously recorded changes. */ + void reset(); + + /** \brief Return all the changes that have been recorded */ + const std::map& getChanges() const + { + return changes_; + } + + using const_iterator = std::map::const_iterator; + /** iterator pointing to first change */ + const_iterator begin() const + { + return changes_.begin(); + } + /** iterator pointing to end of changes */ + const_iterator end() const + { + return changes_.end(); + } + /** number of changes stored */ + size_t size() const + { + return changes_.size(); + } + /** find changes for a named object */ + const_iterator find(const std::string& id) const + { + return changes_.find(id); + } + /** set the entry for an id */ + void set(const std::string& id, World::Action val) + { + if (val) + { + changes_[id] = val; + } + else + { + changes_.erase(id); + } + } + + /** \brief Clear the internally maintained vector of changes */ + void clearChanges(); + +private: + /** \brief Notification function */ + void notify(const World::ObjectConstPtr& /*obj*/, World::Action /*action*/); + + /** keep changes in a map so they can be coalesced */ + std::map changes_; + + /* observer handle for world callback */ + World::ObserverHandle observer_handle_; + + /* used to unregister the notifier */ + WorldWeakPtr world_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index 2605289cab..917c7f7fc8 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Jia Pan, Jens Petit */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp index 8ee2090959..d956756aad 100644 --- a/moveit_core/collision_detection/src/collision_common.cpp +++ b/moveit_core/collision_detection/src/collision_common.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index a353ec54cc..2b065fc0eb 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan, Jens Petit */ -#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index eb2ef1fa1a..61950548b6 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan, E. Gil Jones */ -#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index aca52fa2c8..1dc1688d22 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -34,8 +34,8 @@ /* Author: Adam Leeper */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_plugin_cache.cpp b/moveit_core/collision_detection/src/collision_plugin_cache.cpp index 619bece8f4..adba5a3988 100644 --- a/moveit_core/collision_detection/src/collision_plugin_cache.cpp +++ b/moveit_core/collision_detection/src/collision_plugin_cache.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/collision_tools.cpp b/moveit_core/collision_detection/src/collision_tools.cpp index eaef0dbc8b..e9b69bf05a 100644 --- a/moveit_core/collision_detection/src/collision_tools.cpp +++ b/moveit_core/collision_detection/src/collision_tools.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index ce7c6006fc..b2262f93d5 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -34,7 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/collision_detection/src/world_diff.cpp b/moveit_core/collision_detection/src/world_diff.cpp index d5f8e65978..a58c5cebea 100644 --- a/moveit_core/collision_detection/src/world_diff.cpp +++ b/moveit_core/collision_detection/src/world_diff.cpp @@ -34,7 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#include +#include #include namespace collision_detection diff --git a/moveit_core/collision_detection/test/test_all_valid.cpp b/moveit_core/collision_detection/test/test_all_valid.cpp index 664d0e74cf..d16eb4dca4 100644 --- a/moveit_core/collision_detection/test/test_all_valid.cpp +++ b/moveit_core/collision_detection/test/test_all_valid.cpp @@ -35,8 +35,8 @@ /* Author: Simon Schmeisser */ #include -#include -#include +#include +#include TEST(AllValid, Instantiate) { diff --git a/moveit_core/collision_detection/test/test_world.cpp b/moveit_core/collision_detection/test/test_world.cpp index f2d2c09227..6ce68439ea 100644 --- a/moveit_core/collision_detection/test/test_world.cpp +++ b/moveit_core/collision_detection/test/test_world.cpp @@ -35,7 +35,7 @@ /* Author: Acorn Pooley */ #include -#include +#include #include #include diff --git a/moveit_core/collision_detection/test/test_world_diff.cpp b/moveit_core/collision_detection/test/test_world_diff.cpp index e073cb17c1..c87cb9396f 100644 --- a/moveit_core/collision_detection/test/test_world_diff.cpp +++ b/moveit_core/collision_detection/test/test_world_diff.cpp @@ -35,7 +35,7 @@ /* Author: Acorn Pooley */ #include -#include +#include #include TEST(WorldDiff, TrackChanges) diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h index d243c030be..98ef5335c8 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (Apache License) * @@ -19,65 +31,5 @@ /* Author: Levi Armstrong */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection_bullet -{ -template -using AlignedVector = std::vector>; - -template -using AlignedMap = std::map, Eigen::aligned_allocator>>; - -template -using AlignedUnorderedMap = std::unordered_map, std::equal_to, - Eigen::aligned_allocator>>; - -enum class CollisionObjectType -{ - USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ - - // all of the following convert the meshes to custom collision objects - CONVEX_HULL = 1, /**< @brief Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex - it will be converted) */ - MULTI_SPHERE = 2, /**< @brief Use the mesh and represent it by multiple spheres collision object */ - SDF = 3 /**< @brief Use the mesh and rpresent it by a signed distance fields collision object */ -}; - -/** \brief Bundles the data for a collision query */ -struct ContactTestData -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - ContactTestData(const std::vector& active, const double& contact_distance, - collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req) - : active(active), contact_distance(contact_distance), res(res), req(req), done(false), pair_done(false) - { - } - - const std::vector& active; - - /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ - const double& contact_distance; - - collision_detection::CollisionResult& res; - const collision_detection::CollisionRequest& req; - - /// Indicates if search is finished - bool done; - - /// Indicates if search between a single pair is finished - bool pair_done; -}; - -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.hpp new file mode 100644 index 0000000000..b9dd53a8c9 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.hpp @@ -0,0 +1,83 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2013, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +template +using AlignedVector = std::vector>; + +template +using AlignedMap = std::map, Eigen::aligned_allocator>>; + +template +using AlignedUnorderedMap = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + +enum class CollisionObjectType +{ + USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */ + + // all of the following convert the meshes to custom collision objects + CONVEX_HULL = 1, /**< @brief Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex + it will be converted) */ + MULTI_SPHERE = 2, /**< @brief Use the mesh and represent it by multiple spheres collision object */ + SDF = 3 /**< @brief Use the mesh and rpresent it by a signed distance fields collision object */ +}; + +/** \brief Bundles the data for a collision query */ +struct ContactTestData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ContactTestData(const std::vector& active, const double& contact_distance, + collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req) + : active(active), contact_distance(contact_distance), res(res), req(req), done(false), pair_done(false) + { + } + + const std::vector& active; + + /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ + const double& contact_distance; + + collision_detection::CollisionResult& res; + const collision_detection::CollisionRequest& req; + + /// Indicates if search is finished + bool done; + + /// Indicates if search between a single pair is finished + bool pair_done; +}; + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h index 946fd81125..8161abc6cf 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD-2-Clause) * @@ -32,126 +44,5 @@ /* Authors: Levi Armstrong, Jens Petit */ #pragma once - -#include -#include - -namespace collision_detection_bullet -{ -MOVEIT_CLASS_FORWARD(BulletBVHManager); - -/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ -class BulletBVHManager -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Constructor */ - BulletBVHManager(); - - virtual ~BulletBVHManager(); - - /** @brief Clone the manager - * - * This is to be used for multi threaded applications. A user should make a clone for each thread. */ - BulletBVHManagerPtr clone() const; - - /**@brief Find if a collision object already exists - * @param name The name of the collision object - * @return true if it exists, otherwise false. */ - bool hasCollisionObject(const std::string& name) const; - - /**@brief Remove an object from the checker - * @param name The name of the object - * @return true if successfully removed, otherwise false. */ - bool removeCollisionObject(const std::string& name); - - /**@brief Enable an object - * @param name The name of the object - * @return true if successfully enabled, otherwise false. */ - bool enableCollisionObject(const std::string& name); - - /**@brief Disable an object - * @param name The name of the object - * @return true if successfully disabled, otherwise false. */ - bool disableCollisionObject(const std::string& name); - - /**@brief Set a single static collision object's transform - * @param name The name of the object - * @param pose The transformation in world */ - void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); - - /**@brief Set which collision objects are active - * @param names A vector of collision object names */ - void setActiveCollisionObjects(const std::vector& names); - - /**@brief Get which collision objects are active - * @return A vector of collision object names */ - const std::vector& getActiveCollisionObjects() const; - - /**@brief Set the contact distance threshold for which collision should be considered through expanding the AABB by - * the - * contact_distance for all links. - * @param contact_distance The contact distance */ - void setContactDistanceThreshold(double contact_distance); - - /**@brief Get the contact distance threshold - * @return The contact distance */ - double getContactDistanceThreshold() const; - - /**@brief Perform a contact test for all objects - * @param collisions The Contact results data - * @param req The collision request data - * @param acm The allowed collision matrix - * @param self Used for indicating self collision checks */ - virtual void contactTest(collision_detection::CollisionResult& collisions, - const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm, bool self) = 0; - - /**@brief Add a collision object to the checker - * - * All objects are added should initially be added as static objects. Use the setContactRequest method of defining - * which collision objects are moving. - * - * @param name The name of the object, must be unique. - * @param mask_id User defined id which gets stored in the results structure. - * @param shapes A vector of shapes that make up the collision object. - * @param shape_poses A vector of poses for each shape, must be same length as shapes - * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is - * used for all shapes. - * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex - * hulls) - * @param enabled Indicate if the object is enabled for collision checking. - * @return true if successfully added, otherwise false. */ - /**@brief Add a tesseract collision object to the manager - * @param cow The tesseract bullet collision object */ - virtual void addCollisionObject(const CollisionObjectWrapperPtr& cow) = 0; - - const std::map& getCollisionObjects() const; - -protected: - /** @brief A map of collision objects being managed */ - std::map link2cow_; - - /** @brief A list of the active collision links */ - std::vector active_; - - /** @brief The contact distance threshold */ - double contact_distance_; - - /** @brief The bullet collision dispatcher used for getting object to object collision algorithm */ - std::unique_ptr dispatcher_; - - /** @brief The bullet collision dispatcher configuration information */ - btDispatcherInfo dispatch_info_; - - /** @brief The bullet collision configuration */ - btDefaultCollisionConfiguration coll_config_; - - /** @brief The bullet broadphase interface */ - std::unique_ptr broadphase_; - - /** \brief Callback function for culling objects before a broadphase check */ - BroadphaseFilterCallback filter_callback_; -}; -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.hpp new file mode 100644 index 0000000000..04f9fe5921 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.hpp @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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. + *********************************************************************/ + +/* Authors: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletBVHManager); + +/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ +class BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletBVHManager(); + + virtual ~BulletBVHManager(); + + /** @brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletBVHManagerPtr clone() const; + + /**@brief Find if a collision object already exists + * @param name The name of the collision object + * @return true if it exists, otherwise false. */ + bool hasCollisionObject(const std::string& name) const; + + /**@brief Remove an object from the checker + * @param name The name of the object + * @return true if successfully removed, otherwise false. */ + bool removeCollisionObject(const std::string& name); + + /**@brief Enable an object + * @param name The name of the object + * @return true if successfully enabled, otherwise false. */ + bool enableCollisionObject(const std::string& name); + + /**@brief Disable an object + * @param name The name of the object + * @return true if successfully disabled, otherwise false. */ + bool disableCollisionObject(const std::string& name); + + /**@brief Set a single static collision object's transform + * @param name The name of the object + * @param pose The transformation in world */ + void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose); + + /**@brief Set which collision objects are active + * @param names A vector of collision object names */ + void setActiveCollisionObjects(const std::vector& names); + + /**@brief Get which collision objects are active + * @return A vector of collision object names */ + const std::vector& getActiveCollisionObjects() const; + + /**@brief Set the contact distance threshold for which collision should be considered through expanding the AABB by + * the + * contact_distance for all links. + * @param contact_distance The contact distance */ + void setContactDistanceThreshold(double contact_distance); + + /**@brief Get the contact distance threshold + * @return The contact distance */ + double getContactDistanceThreshold() const; + + /**@brief Perform a contact test for all objects + * @param collisions The Contact results data + * @param req The collision request data + * @param acm The allowed collision matrix + * @param self Used for indicating self collision checks */ + virtual void contactTest(collision_detection::CollisionResult& collisions, + const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) = 0; + + /**@brief Add a collision object to the checker + * + * All objects are added should initially be added as static objects. Use the setContactRequest method of defining + * which collision objects are moving. + * + * @param name The name of the object, must be unique. + * @param mask_id User defined id which gets stored in the results structure. + * @param shapes A vector of shapes that make up the collision object. + * @param shape_poses A vector of poses for each shape, must be same length as shapes + * @param shape_types A vector of shape types for encode the collision object. If the vector is of length 1 it is + * used for all shapes. + * @param collision_object_types A int identifying a conversion mode for the object. (ex. convert meshes to convex + * hulls) + * @param enabled Indicate if the object is enabled for collision checking. + * @return true if successfully added, otherwise false. */ + /**@brief Add a tesseract collision object to the manager + * @param cow The tesseract bullet collision object */ + virtual void addCollisionObject(const CollisionObjectWrapperPtr& cow) = 0; + + const std::map& getCollisionObjects() const; + +protected: + /** @brief A map of collision objects being managed */ + std::map link2cow_; + + /** @brief A list of the active collision links */ + std::vector active_; + + /** @brief The contact distance threshold */ + double contact_distance_; + + /** @brief The bullet collision dispatcher used for getting object to object collision algorithm */ + std::unique_ptr dispatcher_; + + /** @brief The bullet collision dispatcher configuration information */ + btDispatcherInfo dispatch_info_; + + /** @brief The bullet collision configuration */ + btDefaultCollisionConfiguration coll_config_; + + /** @brief The bullet broadphase interface */ + std::unique_ptr broadphase_; + + /** \brief Callback function for culling objects before a broadphase check */ + BroadphaseFilterCallback filter_callback_; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h index 5435c6419d..f650423e44 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD-2-Clause) * @@ -32,50 +44,5 @@ /* Author: Levi Armstrong, Jens Petit */ #pragma once - -#include -#include -#include - -namespace collision_detection_bullet -{ -MOVEIT_CLASS_FORWARD(BulletCastBVHManager); - -/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ -class BulletCastBVHManager : public BulletBVHManager -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Constructor */ - BulletCastBVHManager() = default; - - ~BulletCastBVHManager() override = default; - - /**@brief Clone the manager - * - * This is to be used for multi threaded applications. A user should make a clone for each thread. */ - BulletCastBVHManagerPtr clone() const; - - /**@brief Set a single cast (moving) collision object's transforms - * - * This should only be used for moving objects. - * - * @param name The name of the object - * @param pose1 The start transformation in world - * @param pose2 The end transformation in world */ - void setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, - const Eigen::Isometry3d& pose2); - - /**@brief Perform a contact test for all objects - * @param collisions The Contact results data - * @param req The collision request data - * @param acm The allowed collision matrix */ - void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm, bool self) override; - - /**@brief Add a tesseract collision object to the manager - * @param cow The tesseract bullet collision object */ - void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; -}; -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.hpp new file mode 100644 index 0000000000..3cc0192305 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_cast_bvh_manager.hpp @@ -0,0 +1,81 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletCastBVHManager); + +/** @brief A bounding volume hierarchy (BVH) implementation of a tesseract contact manager */ +class BulletCastBVHManager : public BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletCastBVHManager() = default; + + ~BulletCastBVHManager() override = default; + + /**@brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletCastBVHManagerPtr clone() const; + + /**@brief Set a single cast (moving) collision object's transforms + * + * This should only be used for moving objects. + * + * @param name The name of the object + * @param pose1 The start transformation in world + * @param pose2 The end transformation in world */ + void setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1, + const Eigen::Isometry3d& pose2); + + /**@brief Perform a contact test for all objects + * @param collisions The Contact results data + * @param req The collision request data + * @param acm The allowed collision matrix */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) override; + + /**@brief Add a tesseract collision object to the manager + * @param cow The tesseract bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h index d4e0d8b9e0..0fcdc7a8ef 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD-2-Clause) * @@ -32,40 +44,5 @@ /* Author: Levi Armstrong, Jens Petit */ #pragma once - -#include -#include -#include - -namespace collision_detection_bullet -{ -MOVEIT_CLASS_FORWARD(BulletDiscreteBVHManager); - -/** @brief A bounding volume hierarchy (BVH) implementation of a discrete bullet manager */ -class BulletDiscreteBVHManager : public BulletBVHManager -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Constructor */ - BulletDiscreteBVHManager() = default; - - ~BulletDiscreteBVHManager() override = default; - - /**@brief Clone the manager - * - * This is to be used for multi threaded applications. A user should make a clone for each thread. */ - BulletDiscreteBVHManagerPtr clone() const; - - /**@brief Perform a contact test for all objects in the manager - * @param collisions The Contact results data - * @param acm The allowed collision matrix - * @param req The contact request */ - void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, - const collision_detection::AllowedCollisionMatrix* acm, bool self) override; - - /**@brief Add a bullet collision object to the manager - * @param cow The bullet collision object */ - void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; -}; -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.hpp new file mode 100644 index 0000000000..b9cca6d865 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_discrete_bvh_manager.hpp @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * 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. + * + * 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: Levi Armstrong, Jens Petit */ + +#pragma once + +#include +#include +#include + +namespace collision_detection_bullet +{ +MOVEIT_CLASS_FORWARD(BulletDiscreteBVHManager); + +/** @brief A bounding volume hierarchy (BVH) implementation of a discrete bullet manager */ +class BulletDiscreteBVHManager : public BulletBVHManager +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Constructor */ + BulletDiscreteBVHManager() = default; + + ~BulletDiscreteBVHManager() override = default; + + /**@brief Clone the manager + * + * This is to be used for multi threaded applications. A user should make a clone for each thread. */ + BulletDiscreteBVHManagerPtr clone() const; + + /**@brief Perform a contact test for all objects in the manager + * @param collisions The Contact results data + * @param acm The allowed collision matrix + * @param req The contact request */ + void contactTest(collision_detection::CollisionResult& collisions, const collision_detection::CollisionRequest& req, + const collision_detection::AllowedCollisionMatrix* acm, bool self) override; + + /**@brief Add a bullet collision object to the manager + * @param cow The bullet collision object */ + void addCollisionObject(const CollisionObjectWrapperPtr& cow) override; +}; +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h index 676e2f4eb8..320df7d4f3 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD-2-Clause) * @@ -33,687 +45,5 @@ /* Authors: John Schulman, Levi Armstrong */ #pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace collision_detection_bullet -{ -#define METERS - -const btScalar BULLET_MARGIN = 0.0f; -const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = 0.01f METERS; -const btScalar BULLET_LENGTH_TOLERANCE = 0.001f METERS; -const btScalar BULLET_EPSILON = 1e-3f; // numerical precision limit -const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = 0.00f; // All pairs closer than this distance get reported -const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true; - -MOVEIT_CLASS_FORWARD(CollisionObjectWrapper); - -/** \brief Allowed = true */ -bool acmCheck(const std::string& body_1, const std::string& body_2, - const collision_detection::AllowedCollisionMatrix* acm); - -/** \brief Converts eigen vector to bullet vector */ -inline btVector3 convertEigenToBt(const Eigen::Vector3d& v) -{ - return btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); -} - -/** \brief Converts bullet vector to eigen vector */ -inline Eigen::Vector3d convertBtToEigen(const btVector3& v) -{ - return Eigen::Vector3d(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); -} - -/** \brief Converts eigen quaternion to bullet quaternion */ -inline btQuaternion convertEigenToBt(const Eigen::Quaterniond& q) -{ - return btQuaternion(static_cast(q.x()), static_cast(q.y()), static_cast(q.z()), - static_cast(q.w())); -} - -/** \brief Converts eigen matrix to bullet matrix */ -inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r) -{ - return btMatrix3x3(static_cast(r(0, 0)), static_cast(r(0, 1)), static_cast(r(0, 2)), - static_cast(r(1, 0)), static_cast(r(1, 1)), static_cast(r(1, 2)), - static_cast(r(2, 0)), static_cast(r(2, 1)), static_cast(r(2, 2))); -} - -/** \brief Converts bullet transform to eigen transform */ -inline btTransform convertEigenToBt(const Eigen::Isometry3d& t) -{ - const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0); - const Eigen::Vector3d& tran = t.translation(); - - btMatrix3x3 mat = convertEigenToBt(rot); - btVector3 translation = convertEigenToBt(tran); - - return btTransform(mat, translation); -} - -/** @brief Tesseract bullet collision object. - * - * A wrapper around bullet's collision object which contains specific information related to bullet. One of the main - * differences is that a bullet collision object has a single world transformation and all shapes have transformation - * relative to this world transform. The default collision object category is active and active objects are checked - * against active objects and static objects whereas static objects are only checked against active ones. */ -class CollisionObjectWrapper : public btCollisionObject -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Standard constructor - * - * \param shape_poses Assumes all poses are in a single global frame */ - CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, bool active = true); - - /** \brief Constructor for attached robot objects - * - * \param shape_poses These poses are in the global (planning) frame */ - CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, - const std::set& touch_links); - - /** \brief Bitfield specifies to which group the object belongs */ - short int m_collisionFilterGroup; - - /** \brief Bitfield specifies against which other groups the object is collision checked */ - short int m_collisionFilterMask; - - /** \brief Indicates if the collision object is used for a collision check */ - bool m_enabled{ true }; - - /** \brief The robot links the collision objects is allowed to touch */ - std::set touch_links; - - /** @brief Get the collision object name */ - const std::string& getName() const - { - return name_; - } - - /** @brief Get a user defined type */ - const collision_detection::BodyType& getTypeID() const - { - return type_id_; - } - - /** \brief Check if two CollisionObjectWrapper objects point to the same source object - * \return True if same objects, false otherwise */ - bool sameObject(const CollisionObjectWrapper& other) const - { - return name_ == other.name_ && type_id_ == other.type_id_ && shapes_.size() == other.shapes_.size() && - shape_poses_.size() == other.shape_poses_.size() && - std::equal(shapes_.begin(), shapes_.end(), other.shapes_.begin()) && - std::equal(shape_poses_.begin(), shape_poses_.end(), other.shape_poses_.begin(), - [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); }); - } - - /** @brief Get the collision objects axis aligned bounding box - * @param aabb_min The minimum point - * @param aabb_max The maximum point */ - void getAABB(btVector3& aabb_min, btVector3& aabb_max) const - { - getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); - const btScalar& distance = getContactProcessingThreshold(); - // note that bullet expands each AABB by 4 cm - btVector3 contact_threshold(distance, distance, distance); - aabb_min -= contact_threshold; - aabb_max += contact_threshold; - } - - /** @brief Clones the collision objects but not the collision shape which is const. - * @return Shared Pointer to the cloned collision object */ - std::shared_ptr clone() - { - std::shared_ptr clone_cow( - new CollisionObjectWrapper(name_, type_id_, shapes_, shape_poses_, collision_object_types_, data_)); - clone_cow->setCollisionShape(getCollisionShape()); - clone_cow->setWorldTransform(getWorldTransform()); - clone_cow->m_collisionFilterGroup = m_collisionFilterGroup; - clone_cow->m_collisionFilterMask = m_collisionFilterMask; - clone_cow->m_enabled = m_enabled; - clone_cow->setBroadphaseHandle(nullptr); - clone_cow->touch_links = touch_links; - clone_cow->setContactProcessingThreshold(getContactProcessingThreshold()); - return clone_cow; - } - - /** \brief Manage memory of a raw pointer shape */ - template - void manage(T* t) - { - data_.push_back(std::shared_ptr(t)); - } - - /** \brief Manage memory of a shared pointer shape */ - template - void manage(std::shared_ptr t) - { - data_.push_back(t); - } - -protected: - /** @brief Special constructor used by the clone method */ - CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, - const std::vector& shapes, - const AlignedVector& shape_poses, - const std::vector& collision_object_types, - const std::vector>& data); - - /** \brief The name of the object, must be unique. */ - std::string name_; - - collision_detection::BodyType type_id_; - - /** @brief The shapes that define the collision object */ - std::vector shapes_; - - /** @brief The poses of the shapes, must be same length as m_shapes */ - AlignedVector shape_poses_; - - /** @brief The shape collision object type to be used. */ - std::vector collision_object_types_; - - /** @brief Manages the collision shape pointer so they get destroyed */ - std::vector> data_; -}; - -/** @brief Casted collision shape used for checking if an object is collision free between two discrete poses - * - * The cast is not explicitly computed but implicitly represented through the single shape and the transformation - * between the first and second pose. */ -struct CastHullShape : public btConvexShape -{ -public: - /** \brief The casted shape */ - btConvexShape* m_shape; - - /** \brief Transformation from the first pose to the second pose */ - btTransform shape_transform; - - CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), shape_transform(t01) - { - m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; - } - - void updateCastTransform(const btTransform& cast_transform) - { - shape_transform = cast_transform; - } - - /** \brief From both shape poses computes the support vertex and returns the larger one. */ - btVector3 localGetSupportingVertex(const btVector3& vec) const override - { - btVector3 support_vector_0 = m_shape->localGetSupportingVertex(vec); - btVector3 support_vector_1 = shape_transform * m_shape->localGetSupportingVertex(vec * shape_transform.getBasis()); - return (vec.dot(support_vector_0) > vec.dot(support_vector_1)) ? support_vector_0 : support_vector_1; - } - - void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* /*vectors*/, - btVector3* /*supportVerticesOut*/, - int /*numVectors*/) const override - { - throw std::runtime_error("not implemented"); - } - - /** \brief Shape specific fast recalculation of the AABB at a certain pose - * - * The AABB is not recalculated from scratch but updated depending on the given transformation. */ - void getAabb(const btTransform& transform_world, btVector3& aabbMin, btVector3& aabbMax) const override - { - m_shape->getAabb(transform_world, aabbMin, aabbMax); - btVector3 min1, max1; - m_shape->getAabb(transform_world * shape_transform, min1, max1); - aabbMin.setMin(min1); - aabbMax.setMax(max1); - } - - void getAabbSlow(const btTransform& /*t*/, btVector3& /*aabbMin*/, btVector3& /*aabbMax*/) const override - { - throw std::runtime_error("shouldn't happen"); - } - - void setLocalScaling(const btVector3& /*scaling*/) override - { - } - - const btVector3& getLocalScaling() const override - { - static btVector3 out(1, 1, 1); - return out; - } - - void setMargin(btScalar /*margin*/) override - { - } - - btScalar getMargin() const override - { - return 0; - } - - int getNumPreferredPenetrationDirections() const override - { - return 0; - } - - void getPreferredPenetrationDirection(int /*index*/, btVector3& /*penetrationVector*/) const override - { - throw std::runtime_error("not implemented"); - } - - void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const override - { - throw std::runtime_error("not implemented"); - } - - const char* getName() const override - { - return "CastHull"; - } - - btVector3 localGetSupportingVertexWithoutMargin(const btVector3& v) const override - { - return localGetSupportingVertex(v); - } -}; - -/** \brief Computes the local supporting vertex of a convex shape. - * - * If multiple vertices with equal support products exists, their average is calculated and returned. - * - * \param shape The convex shape to check - * \param localNormal The support direction to search for in shape local coordinates - * \param outsupport The value of the calculated support mapping - * \param outpt The computed support point */ -inline void getAverageSupport(const btConvexShape* shape, const btVector3& localNormal, float& outsupport, - btVector3& outpt) -{ - btVector3 pt_sum(0, 0, 0); - float pt_count = 0; - float max_support = -1000; - - const btPolyhedralConvexShape* pshape = dynamic_cast(shape); - if (pshape) - { - int n_pts = pshape->getNumVertices(); - - for (int i = 0; i < n_pts; ++i) - { - btVector3 pt; - pshape->getVertex(i, pt); - - float sup = pt.dot(localNormal); - if (sup > max_support + BULLET_EPSILON) - { - pt_count = 1; - pt_sum = pt; - max_support = sup; - } - else if (sup < max_support - BULLET_EPSILON) {} - else - { - pt_count += 1; - pt_sum += pt; - } - } - outsupport = max_support; - outpt = pt_sum / pt_count; - } - else - { - outpt = shape->localGetSupportingVertexWithoutMargin(localNormal); - outsupport = localNormal.dot(outpt); - } -} - -/** \brief Converts a bullet contact result to MoveIt format and adds it to the result data structure */ -inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, - const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions) -{ - assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); - assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); - const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); - const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); - - std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); - - const auto& it = collisions.res.contacts.find(pc); - bool found = (it != collisions.res.contacts.end()); - - collision_detection::Contact contact; - contact.body_name_1 = cd0->getName(); - contact.body_name_2 = cd1->getName(); - contact.depth = static_cast(cp.m_distance1); - contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); - contact.pos = convertBtToEigen(cp.m_positionWorldOnA); - contact.nearest_points[0] = contact.pos; - contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB); - - contact.body_type_1 = cd0->getTypeID(); - contact.body_type_2 = cd1->getTypeID(); - - if (!processResult(collisions, contact, pc, found)) - { - return 0; - } - - return 1; -} - -inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*index0*/, - const btCollisionObjectWrapper* colObj1Wrap, int /*index1*/, - ContactTestData& collisions) -{ - assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); - assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); - - const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); - const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); - - std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); - - const auto& it = collisions.res.contacts.find(pc); - bool found = (it != collisions.res.contacts.end()); - - collision_detection::Contact contact; - contact.body_name_1 = cd0->getName(); - contact.body_name_2 = cd1->getName(); - contact.depth = static_cast(cp.m_distance1); - contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); - contact.pos = convertBtToEigen(cp.m_positionWorldOnA); - - contact.body_type_1 = cd0->getTypeID(); - contact.body_type_2 = cd1->getTypeID(); - - collision_detection::Contact* col = processResult(collisions, contact, pc, found); - - if (!col) - { - return 0; - } - - assert(!((cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) && - (cd1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter))); - - bool cast_shape_is_first = cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; - - btVector3 normal_world_from_cast = (cast_shape_is_first ? -1 : 1) * cp.m_normalWorldOnB; - const btCollisionObjectWrapper* first_col_obj_wrap = (cast_shape_is_first ? colObj0Wrap : colObj1Wrap); - - // we want the contact information of the non-casted object come first, therefore swap values - if (cast_shape_is_first) - { - std::swap(col->nearest_points[0], col->nearest_points[1]); - contact.pos = convertBtToEigen(cp.m_positionWorldOnB); - std::swap(col->body_name_1, col->body_name_2); - std::swap(col->body_type_1, col->body_type_2); - col->normal *= -1; - } - - btTransform tf_world0, tf_world1; - const CastHullShape* shape = static_cast(first_col_obj_wrap->getCollisionShape()); - - tf_world0 = first_col_obj_wrap->getWorldTransform(); - tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->shape_transform; - - // transform normals into pose local coordinate systems - btVector3 normal_local0 = normal_world_from_cast * tf_world0.getBasis(); - btVector3 normal_local1 = normal_world_from_cast * tf_world1.getBasis(); - - btVector3 pt_local0; - float localsup0; - getAverageSupport(shape->m_shape, normal_local0, localsup0, pt_local0); - btVector3 pt_world0 = tf_world0 * pt_local0; - btVector3 pt_local1; - float localsup1; - getAverageSupport(shape->m_shape, normal_local1, localsup1, pt_local1); - btVector3 pt_world1 = tf_world1 * pt_local1; - - float sup0 = normal_world_from_cast.dot(pt_world0); - float sup1 = normal_world_from_cast.dot(pt_world1); - - // TODO: this section is potentially problematic. think hard about the math - if (sup0 - sup1 > BULLET_SUPPORT_FUNC_TOLERANCE) - { - col->percent_interpolation = 0; - } - else if (sup1 - sup0 > BULLET_SUPPORT_FUNC_TOLERANCE) - { - col->percent_interpolation = 1; - } - else - { - const btVector3& pt_on_cast = cast_shape_is_first ? cp.m_positionWorldOnA : cp.m_positionWorldOnB; - float l0c = (pt_on_cast - pt_world0).length(); - float l1c = (pt_on_cast - pt_world1).length(); - - if (l0c + l1c < BULLET_LENGTH_TOLERANCE) - { - col->percent_interpolation = .5; - } - else - { - col->percent_interpolation = static_cast(l0c / (l0c + l1c)); - } - } - - return 1; -} - -/** \brief Checks if the collision pair is kinematic vs kinematic objects */ -inline bool isOnlyKinematic(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) -{ - return cow0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter && - cow1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; -} - -/** @brief Callback structure for both discrete and continuous broadphase collision pair - * - * /e needsCollision is the callback executed before a narrowphase check is executed. - * /e addSingleResult is the callback executed after the narrowphase check delivers a result. */ -struct BroadphaseContactResultCallback -{ - ContactTestData& collisions_; - double contact_distance_; - const collision_detection::AllowedCollisionMatrix* acm_{ nullptr }; - - /** \brief Indicates if the callback is used for only self-collision checking */ - bool self_; - - /** \brief Indicates if the callback is used for casted collisions */ - bool cast_{ false }; - - BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, - const collision_detection::AllowedCollisionMatrix* acm, bool self, bool cast = false) - : collisions_(collisions), contact_distance_(contact_distance), acm_(acm), self_(self), cast_(cast) - { - } - - ~BroadphaseContactResultCallback() = default; - - /** \brief This callback is used for each overlapping pair in a pair cache of the broadphase interface to check if a - * collision check should done for the pair. */ - // TODO: Add check for two objects attached to the same link - bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const - { - if (cast_) - { - return !collisions_.done && !isOnlyKinematic(cow0, cow1) && !acmCheck(cow0->getName(), cow1->getName(), acm_); - } - else - { - return !collisions_.done && (self_ ? isOnlyKinematic(cow0, cow1) : !isOnlyKinematic(cow0, cow1)) && - !acmCheck(cow0->getName(), cow1->getName(), acm_); - } - } - - /** \brief This callback is used after btManifoldResult processed a collision result. */ - btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, - const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1); -}; - -struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult -{ - BroadphaseContactResultCallback& result_callback_; - - TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, - const btCollisionObjectWrapper* obj1Wrap, - BroadphaseContactResultCallback& result_callback) - : btManifoldResult(obj0Wrap, obj1Wrap), result_callback_(result_callback) - { - } - - void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override - { - if (result_callback_.collisions_.done || result_callback_.collisions_.pair_done || - depth > static_cast(result_callback_.contact_distance_)) - { - return; - } - - bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); - btVector3 point_a = pointInWorld + normalOnBInWorld * depth; - btVector3 local_a; - btVector3 local_b; - if (is_swapped) - { - local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); - local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } - else - { - local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); - local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } - - btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); - new_pt.m_positionWorldOnA = point_a; - new_pt.m_positionWorldOnB = pointInWorld; - - // BP mod, store contact triangles. - if (is_swapped) - { - new_pt.m_partId0 = m_partId1; - new_pt.m_partId1 = m_partId0; - new_pt.m_index0 = m_index1; - new_pt.m_index1 = m_index0; - } - else - { - new_pt.m_partId0 = m_partId0; - new_pt.m_partId1 = m_partId1; - new_pt.m_index0 = m_index0; - new_pt.m_index1 = m_index1; - } - - // experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; - result_callback_.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, - new_pt.m_index1); - } -}; - -/** @brief A callback function that is called as part of the broadphase collision checking. - * - * If the AABB of two collision objects are overlapping the processOverlap method is called and they are checked for - * collision/distance and the results are stored in collision_. */ -class TesseractCollisionPairCallback : public btOverlapCallback -{ - const btDispatcherInfo& dispatch_info_; - btCollisionDispatcher* dispatcher_; - - /** \brief Callback executed for each broadphase pair to check if needs collision */ - BroadphaseContactResultCallback& results_callback_; - -public: - TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher, - BroadphaseContactResultCallback& results_callback) - : dispatch_info_(dispatchInfo), dispatcher_(dispatcher), results_callback_(results_callback) - { - } - - ~TesseractCollisionPairCallback() override = default; - - bool processOverlap(btBroadphasePair& pair) override; -}; - -/** \brief Casts a geometric shape into a btCollisionShape */ -btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, - const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow); - -/** @brief Update a collision objects filters - * @param active A list of active collision objects - * @param cow The collision object to update. - * @param continuous Indicate if the object is a continuous collision object. - * - * Currently continuous collision objects can only be checked against static objects. Continuous to Continuous - * collision checking is currently not supports. TODO LEVI: Add support for Continuous to Continuous collision - * checking. */ -void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow); - -CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow); - -/** @brief Update the Broadphase AABB for the input collision object - * @param cow The collision objects - * @param broadphase The bullet broadphase interface - * @param dispatcher The bullet collision dispatcher */ -inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow, - const std::unique_ptr& broadphase, - const std::unique_ptr& dispatcher) -{ - btVector3 aabb_min, aabb_max; - cow->getAABB(aabb_min, aabb_max); - - assert(cow->getBroadphaseHandle() != nullptr); - broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get()); -} - -/** @brief Remove the collision object from broadphase - * @param cow The collision objects - * @param broadphase The bullet broadphase interface - * @param dispatcher The bullet collision dispatcher */ -inline void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr& cow, - const std::unique_ptr& broadphase, - const std::unique_ptr& dispatcher) -{ - btBroadphaseProxy* bp = cow->getBroadphaseHandle(); - if (bp) - { - // only clear the cached algorithms - broadphase->getOverlappingPairCache()->cleanProxyFromPairs(bp, dispatcher.get()); - broadphase->destroyProxy(bp, dispatcher.get()); - cow->setBroadphaseHandle(nullptr); - } -} - -/** @brief Add the collision object to broadphase - * @param cow The collision objects - * @param broadphase The bullet broadphase interface - * @param dispatcher The bullet collision dispatcher */ -void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, - const std::unique_ptr& broadphase, - const std::unique_ptr& dispatcher); - -struct BroadphaseFilterCallback : public btOverlapFilterCallback -{ - bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override; -}; - -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.hpp new file mode 100644 index 0000000000..6bacd5718b --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.hpp @@ -0,0 +1,719 @@ +/********************************************************************* + * Software License Agreement (BSD-2-Clause) + * + * Copyright (c) 2017, Southwest Research Institute + * Copyright (c) 2013, John Schulman + * 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. + * + * 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. + *********************************************************************/ + +/* Authors: John Schulman, Levi Armstrong */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace collision_detection_bullet +{ +#define METERS + +const btScalar BULLET_MARGIN = 0.0f; +const btScalar BULLET_SUPPORT_FUNC_TOLERANCE = 0.01f METERS; +const btScalar BULLET_LENGTH_TOLERANCE = 0.001f METERS; +const btScalar BULLET_EPSILON = 1e-3f; // numerical precision limit +const btScalar BULLET_DEFAULT_CONTACT_DISTANCE = 0.00f; // All pairs closer than this distance get reported +const bool BULLET_COMPOUND_USE_DYNAMIC_AABB = true; + +MOVEIT_CLASS_FORWARD(CollisionObjectWrapper); + +/** \brief Allowed = true */ +bool acmCheck(const std::string& body_1, const std::string& body_2, + const collision_detection::AllowedCollisionMatrix* acm); + +/** \brief Converts eigen vector to bullet vector */ +inline btVector3 convertEigenToBt(const Eigen::Vector3d& v) +{ + return btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); +} + +/** \brief Converts bullet vector to eigen vector */ +inline Eigen::Vector3d convertBtToEigen(const btVector3& v) +{ + return Eigen::Vector3d(static_cast(v.x()), static_cast(v.y()), static_cast(v.z())); +} + +/** \brief Converts eigen quaternion to bullet quaternion */ +inline btQuaternion convertEigenToBt(const Eigen::Quaterniond& q) +{ + return btQuaternion(static_cast(q.x()), static_cast(q.y()), static_cast(q.z()), + static_cast(q.w())); +} + +/** \brief Converts eigen matrix to bullet matrix */ +inline btMatrix3x3 convertEigenToBt(const Eigen::Matrix3d& r) +{ + return btMatrix3x3(static_cast(r(0, 0)), static_cast(r(0, 1)), static_cast(r(0, 2)), + static_cast(r(1, 0)), static_cast(r(1, 1)), static_cast(r(1, 2)), + static_cast(r(2, 0)), static_cast(r(2, 1)), static_cast(r(2, 2))); +} + +/** \brief Converts bullet transform to eigen transform */ +inline btTransform convertEigenToBt(const Eigen::Isometry3d& t) +{ + const Eigen::Matrix3d& rot = t.matrix().block<3, 3>(0, 0); + const Eigen::Vector3d& tran = t.translation(); + + btMatrix3x3 mat = convertEigenToBt(rot); + btVector3 translation = convertEigenToBt(tran); + + return btTransform(mat, translation); +} + +/** @brief Tesseract bullet collision object. + * + * A wrapper around bullet's collision object which contains specific information related to bullet. One of the main + * differences is that a bullet collision object has a single world transformation and all shapes have transformation + * relative to this world transform. The default collision object category is active and active objects are checked + * against active objects and static objects whereas static objects are only checked against active ones. */ +class CollisionObjectWrapper : public btCollisionObject +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Standard constructor + * + * \param shape_poses Assumes all poses are in a single global frame */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, bool active = true); + + /** \brief Constructor for attached robot objects + * + * \param shape_poses These poses are in the global (planning) frame */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::set& touch_links); + + /** \brief Bitfield specifies to which group the object belongs */ + short int m_collisionFilterGroup; + + /** \brief Bitfield specifies against which other groups the object is collision checked */ + short int m_collisionFilterMask; + + /** \brief Indicates if the collision object is used for a collision check */ + bool m_enabled{ true }; + + /** \brief The robot links the collision objects is allowed to touch */ + std::set touch_links; + + /** @brief Get the collision object name */ + const std::string& getName() const + { + return name_; + } + + /** @brief Get a user defined type */ + const collision_detection::BodyType& getTypeID() const + { + return type_id_; + } + + /** \brief Check if two CollisionObjectWrapper objects point to the same source object + * \return True if same objects, false otherwise */ + bool sameObject(const CollisionObjectWrapper& other) const + { + return name_ == other.name_ && type_id_ == other.type_id_ && shapes_.size() == other.shapes_.size() && + shape_poses_.size() == other.shape_poses_.size() && + std::equal(shapes_.begin(), shapes_.end(), other.shapes_.begin()) && + std::equal(shape_poses_.begin(), shape_poses_.end(), other.shape_poses_.begin(), + [](const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2) { return t1.isApprox(t2); }); + } + + /** @brief Get the collision objects axis aligned bounding box + * @param aabb_min The minimum point + * @param aabb_max The maximum point */ + void getAABB(btVector3& aabb_min, btVector3& aabb_max) const + { + getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); + const btScalar& distance = getContactProcessingThreshold(); + // note that bullet expands each AABB by 4 cm + btVector3 contact_threshold(distance, distance, distance); + aabb_min -= contact_threshold; + aabb_max += contact_threshold; + } + + /** @brief Clones the collision objects but not the collision shape which is const. + * @return Shared Pointer to the cloned collision object */ + std::shared_ptr clone() + { + std::shared_ptr clone_cow( + new CollisionObjectWrapper(name_, type_id_, shapes_, shape_poses_, collision_object_types_, data_)); + clone_cow->setCollisionShape(getCollisionShape()); + clone_cow->setWorldTransform(getWorldTransform()); + clone_cow->m_collisionFilterGroup = m_collisionFilterGroup; + clone_cow->m_collisionFilterMask = m_collisionFilterMask; + clone_cow->m_enabled = m_enabled; + clone_cow->setBroadphaseHandle(nullptr); + clone_cow->touch_links = touch_links; + clone_cow->setContactProcessingThreshold(getContactProcessingThreshold()); + return clone_cow; + } + + /** \brief Manage memory of a raw pointer shape */ + template + void manage(T* t) + { + data_.push_back(std::shared_ptr(t)); + } + + /** \brief Manage memory of a shared pointer shape */ + template + void manage(std::shared_ptr t) + { + data_.push_back(t); + } + +protected: + /** @brief Special constructor used by the clone method */ + CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id, + const std::vector& shapes, + const AlignedVector& shape_poses, + const std::vector& collision_object_types, + const std::vector>& data); + + /** \brief The name of the object, must be unique. */ + std::string name_; + + collision_detection::BodyType type_id_; + + /** @brief The shapes that define the collision object */ + std::vector shapes_; + + /** @brief The poses of the shapes, must be same length as m_shapes */ + AlignedVector shape_poses_; + + /** @brief The shape collision object type to be used. */ + std::vector collision_object_types_; + + /** @brief Manages the collision shape pointer so they get destroyed */ + std::vector> data_; +}; + +/** @brief Casted collision shape used for checking if an object is collision free between two discrete poses + * + * The cast is not explicitly computed but implicitly represented through the single shape and the transformation + * between the first and second pose. */ +struct CastHullShape : public btConvexShape +{ +public: + /** \brief The casted shape */ + btConvexShape* m_shape; + + /** \brief Transformation from the first pose to the second pose */ + btTransform shape_transform; + + CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), shape_transform(t01) + { + m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; + } + + void updateCastTransform(const btTransform& cast_transform) + { + shape_transform = cast_transform; + } + + /** \brief From both shape poses computes the support vertex and returns the larger one. */ + btVector3 localGetSupportingVertex(const btVector3& vec) const override + { + btVector3 support_vector_0 = m_shape->localGetSupportingVertex(vec); + btVector3 support_vector_1 = shape_transform * m_shape->localGetSupportingVertex(vec * shape_transform.getBasis()); + return (vec.dot(support_vector_0) > vec.dot(support_vector_1)) ? support_vector_0 : support_vector_1; + } + + void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* /*vectors*/, + btVector3* /*supportVerticesOut*/, + int /*numVectors*/) const override + { + throw std::runtime_error("not implemented"); + } + + /** \brief Shape specific fast recalculation of the AABB at a certain pose + * + * The AABB is not recalculated from scratch but updated depending on the given transformation. */ + void getAabb(const btTransform& transform_world, btVector3& aabbMin, btVector3& aabbMax) const override + { + m_shape->getAabb(transform_world, aabbMin, aabbMax); + btVector3 min1, max1; + m_shape->getAabb(transform_world * shape_transform, min1, max1); + aabbMin.setMin(min1); + aabbMax.setMax(max1); + } + + void getAabbSlow(const btTransform& /*t*/, btVector3& /*aabbMin*/, btVector3& /*aabbMax*/) const override + { + throw std::runtime_error("shouldn't happen"); + } + + void setLocalScaling(const btVector3& /*scaling*/) override + { + } + + const btVector3& getLocalScaling() const override + { + static btVector3 out(1, 1, 1); + return out; + } + + void setMargin(btScalar /*margin*/) override + { + } + + btScalar getMargin() const override + { + return 0; + } + + int getNumPreferredPenetrationDirections() const override + { + return 0; + } + + void getPreferredPenetrationDirection(int /*index*/, btVector3& /*penetrationVector*/) const override + { + throw std::runtime_error("not implemented"); + } + + void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const override + { + throw std::runtime_error("not implemented"); + } + + const char* getName() const override + { + return "CastHull"; + } + + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& v) const override + { + return localGetSupportingVertex(v); + } +}; + +/** \brief Computes the local supporting vertex of a convex shape. + * + * If multiple vertices with equal support products exists, their average is calculated and returned. + * + * \param shape The convex shape to check + * \param localNormal The support direction to search for in shape local coordinates + * \param outsupport The value of the calculated support mapping + * \param outpt The computed support point */ +inline void getAverageSupport(const btConvexShape* shape, const btVector3& localNormal, float& outsupport, + btVector3& outpt) +{ + btVector3 pt_sum(0, 0, 0); + float pt_count = 0; + float max_support = -1000; + + const btPolyhedralConvexShape* pshape = dynamic_cast(shape); + if (pshape) + { + int n_pts = pshape->getNumVertices(); + + for (int i = 0; i < n_pts; ++i) + { + btVector3 pt; + pshape->getVertex(i, pt); + + float sup = pt.dot(localNormal); + if (sup > max_support + BULLET_EPSILON) + { + pt_count = 1; + pt_sum = pt; + max_support = sup; + } + else if (sup < max_support - BULLET_EPSILON) {} + else + { + pt_count += 1; + pt_sum += pt; + } + } + outsupport = max_support; + outpt = pt_sum / pt_count; + } + else + { + outpt = shape->localGetSupportingVertexWithoutMargin(localNormal); + outsupport = localNormal.dot(outpt); + } +} + +/** \brief Converts a bullet contact result to MoveIt format and adds it to the result data structure */ +inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, + const btCollisionObjectWrapper* colObj1Wrap, ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + contact.nearest_points[0] = contact.pos; + contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd1->getTypeID(); + + if (!processResult(collisions, contact, pc, found)) + { + return 0; + } + + return 1; +} + +inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*index0*/, + const btCollisionObjectWrapper* colObj1Wrap, int /*index1*/, + ContactTestData& collisions) +{ + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); + + const CollisionObjectWrapper* cd0 = static_cast(colObj0Wrap->getCollisionObject()); + const CollisionObjectWrapper* cd1 = static_cast(colObj1Wrap->getCollisionObject()); + + std::pair pc = getObjectPairKey(cd0->getName(), cd1->getName()); + + const auto& it = collisions.res.contacts.find(pc); + bool found = (it != collisions.res.contacts.end()); + + collision_detection::Contact contact; + contact.body_name_1 = cd0->getName(); + contact.body_name_2 = cd1->getName(); + contact.depth = static_cast(cp.m_distance1); + contact.normal = convertBtToEigen(-1 * cp.m_normalWorldOnB); + contact.pos = convertBtToEigen(cp.m_positionWorldOnA); + + contact.body_type_1 = cd0->getTypeID(); + contact.body_type_2 = cd1->getTypeID(); + + collision_detection::Contact* col = processResult(collisions, contact, pc, found); + + if (!col) + { + return 0; + } + + assert(!((cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter) && + (cd1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter))); + + bool cast_shape_is_first = cd0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; + + btVector3 normal_world_from_cast = (cast_shape_is_first ? -1 : 1) * cp.m_normalWorldOnB; + const btCollisionObjectWrapper* first_col_obj_wrap = (cast_shape_is_first ? colObj0Wrap : colObj1Wrap); + + // we want the contact information of the non-casted object come first, therefore swap values + if (cast_shape_is_first) + { + std::swap(col->nearest_points[0], col->nearest_points[1]); + contact.pos = convertBtToEigen(cp.m_positionWorldOnB); + std::swap(col->body_name_1, col->body_name_2); + std::swap(col->body_type_1, col->body_type_2); + col->normal *= -1; + } + + btTransform tf_world0, tf_world1; + const CastHullShape* shape = static_cast(first_col_obj_wrap->getCollisionShape()); + + tf_world0 = first_col_obj_wrap->getWorldTransform(); + tf_world1 = first_col_obj_wrap->getWorldTransform() * shape->shape_transform; + + // transform normals into pose local coordinate systems + btVector3 normal_local0 = normal_world_from_cast * tf_world0.getBasis(); + btVector3 normal_local1 = normal_world_from_cast * tf_world1.getBasis(); + + btVector3 pt_local0; + float localsup0; + getAverageSupport(shape->m_shape, normal_local0, localsup0, pt_local0); + btVector3 pt_world0 = tf_world0 * pt_local0; + btVector3 pt_local1; + float localsup1; + getAverageSupport(shape->m_shape, normal_local1, localsup1, pt_local1); + btVector3 pt_world1 = tf_world1 * pt_local1; + + float sup0 = normal_world_from_cast.dot(pt_world0); + float sup1 = normal_world_from_cast.dot(pt_world1); + + // TODO: this section is potentially problematic. think hard about the math + if (sup0 - sup1 > BULLET_SUPPORT_FUNC_TOLERANCE) + { + col->percent_interpolation = 0; + } + else if (sup1 - sup0 > BULLET_SUPPORT_FUNC_TOLERANCE) + { + col->percent_interpolation = 1; + } + else + { + const btVector3& pt_on_cast = cast_shape_is_first ? cp.m_positionWorldOnA : cp.m_positionWorldOnB; + float l0c = (pt_on_cast - pt_world0).length(); + float l1c = (pt_on_cast - pt_world1).length(); + + if (l0c + l1c < BULLET_LENGTH_TOLERANCE) + { + col->percent_interpolation = .5; + } + else + { + col->percent_interpolation = static_cast(l0c / (l0c + l1c)); + } + } + + return 1; +} + +/** \brief Checks if the collision pair is kinematic vs kinematic objects */ +inline bool isOnlyKinematic(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) +{ + return cow0->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter && + cow1->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter; +} + +/** @brief Callback structure for both discrete and continuous broadphase collision pair + * + * /e needsCollision is the callback executed before a narrowphase check is executed. + * /e addSingleResult is the callback executed after the narrowphase check delivers a result. */ +struct BroadphaseContactResultCallback +{ + ContactTestData& collisions_; + double contact_distance_; + const collision_detection::AllowedCollisionMatrix* acm_{ nullptr }; + + /** \brief Indicates if the callback is used for only self-collision checking */ + bool self_; + + /** \brief Indicates if the callback is used for casted collisions */ + bool cast_{ false }; + + BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, + const collision_detection::AllowedCollisionMatrix* acm, bool self, bool cast = false) + : collisions_(collisions), contact_distance_(contact_distance), acm_(acm), self_(self), cast_(cast) + { + } + + ~BroadphaseContactResultCallback() = default; + + /** \brief This callback is used for each overlapping pair in a pair cache of the broadphase interface to check if a + * collision check should done for the pair. */ + // TODO: Add check for two objects attached to the same link + bool needsCollision(const CollisionObjectWrapper* cow0, const CollisionObjectWrapper* cow1) const + { + if (cast_) + { + return !collisions_.done && !isOnlyKinematic(cow0, cow1) && !acmCheck(cow0->getName(), cow1->getName(), acm_); + } + else + { + return !collisions_.done && (self_ ? isOnlyKinematic(cow0, cow1) : !isOnlyKinematic(cow0, cow1)) && + !acmCheck(cow0->getName(), cow1->getName(), acm_); + } + } + + /** \brief This callback is used after btManifoldResult processed a collision result. */ + btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, + const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1); +}; + +struct TesseractBroadphaseBridgedManifoldResult : public btManifoldResult +{ + BroadphaseContactResultCallback& result_callback_; + + TesseractBroadphaseBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, + const btCollisionObjectWrapper* obj1Wrap, + BroadphaseContactResultCallback& result_callback) + : btManifoldResult(obj0Wrap, obj1Wrap), result_callback_(result_callback) + { + } + + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) override + { + if (result_callback_.collisions_.done || result_callback_.collisions_.pair_done || + depth > static_cast(result_callback_.contact_distance_)) + { + return; + } + + bool is_swapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 point_a = pointInWorld + normalOnBInWorld * depth; + btVector3 local_a; + btVector3 local_b; + if (is_swapped) + { + local_a = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + else + { + local_a = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(point_a); + local_b = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint new_pt(local_a, local_b, normalOnBInWorld, depth); + new_pt.m_positionWorldOnA = point_a; + new_pt.m_positionWorldOnB = pointInWorld; + + // BP mod, store contact triangles. + if (is_swapped) + { + new_pt.m_partId0 = m_partId1; + new_pt.m_partId1 = m_partId0; + new_pt.m_index0 = m_index1; + new_pt.m_index1 = m_index0; + } + else + { + new_pt.m_partId0 = m_partId0; + new_pt.m_partId1 = m_partId1; + new_pt.m_index0 = m_index0; + new_pt.m_index1 = m_index1; + } + + // experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0_wrap = is_swapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1_wrap = is_swapped ? m_body0Wrap : m_body1Wrap; + result_callback_.addSingleResult(new_pt, obj0_wrap, new_pt.m_partId0, new_pt.m_index0, obj1_wrap, new_pt.m_partId1, + new_pt.m_index1); + } +}; + +/** @brief A callback function that is called as part of the broadphase collision checking. + * + * If the AABB of two collision objects are overlapping the processOverlap method is called and they are checked for + * collision/distance and the results are stored in collision_. */ +class TesseractCollisionPairCallback : public btOverlapCallback +{ + const btDispatcherInfo& dispatch_info_; + btCollisionDispatcher* dispatcher_; + + /** \brief Callback executed for each broadphase pair to check if needs collision */ + BroadphaseContactResultCallback& results_callback_; + +public: + TesseractCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher, + BroadphaseContactResultCallback& results_callback) + : dispatch_info_(dispatchInfo), dispatcher_(dispatcher), results_callback_(results_callback) + { + } + + ~TesseractCollisionPairCallback() override = default; + + bool processOverlap(btBroadphasePair& pair) override; +}; + +/** \brief Casts a geometric shape into a btCollisionShape */ +btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom, + const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow); + +/** @brief Update a collision objects filters + * @param active A list of active collision objects + * @param cow The collision object to update. + * @param continuous Indicate if the object is a continuous collision object. + * + * Currently continuous collision objects can only be checked against static objects. Continuous to Continuous + * collision checking is currently not supports. TODO LEVI: Add support for Continuous to Continuous collision + * checking. */ +void updateCollisionObjectFilters(const std::vector& active, CollisionObjectWrapper& cow); + +CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr& cow); + +/** @brief Update the Broadphase AABB for the input collision object + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void updateBroadphaseAABB(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btVector3 aabb_min, aabb_max; + cow->getAABB(aabb_min, aabb_max); + + assert(cow->getBroadphaseHandle() != nullptr); + broadphase->setAabb(cow->getBroadphaseHandle(), aabb_min, aabb_max, dispatcher.get()); +} + +/** @brief Remove the collision object from broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +inline void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher) +{ + btBroadphaseProxy* bp = cow->getBroadphaseHandle(); + if (bp) + { + // only clear the cached algorithms + broadphase->getOverlappingPairCache()->cleanProxyFromPairs(bp, dispatcher.get()); + broadphase->destroyProxy(bp, dispatcher.get()); + cow->setBroadphaseHandle(nullptr); + } +} + +/** @brief Add the collision object to broadphase + * @param cow The collision objects + * @param broadphase The bullet broadphase interface + * @param dispatcher The bullet collision dispatcher */ +void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr& cow, + const std::unique_ptr& broadphase, + const std::unique_ptr& dispatcher); + +struct BroadphaseFilterCallback : public btOverlapFilterCallback +{ + bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override; +}; + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h index b54bee4bb1..7df452bef4 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (Apache License) * @@ -19,58 +31,5 @@ /* Author: Levi Armstrong */ #pragma once - -#include -#include -#include -#include - -#include -#include -#include - -namespace collision_detection_bullet -{ -/** - * @brief Get a key for two object to search the collision matrix - * @param obj1 First collision object name - * @param obj2 Second collision object name - * @return The collision pair key - */ -inline std::pair getObjectPairKey(const std::string& obj1, const std::string& obj2) -{ - return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1); -} - -/** - * @brief This will check if a link is active provided a list. If the list is empty the link is considered active. - * @param active List of active link names - * @param name The name of link to check if it is active. - */ -inline bool isLinkActive(const std::vector& active, const std::string& name) -{ - return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); -} - -/** \brief Stores a single contact result in the requested way. - * \param found Indicates if a contact for this pair of objects has already been found - * \return Pointer to the newly inserted contact */ -collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact, - const std::pair& key, bool found); - -/** - * @brief Create a convex hull from vertices using Bullet Convex Hull Computer - * @param (Output) vertices A vector of vertices - * @param (Output) faces The first values indicates the number of vertices that define the face followed by the vertice - * index - * @param (input) input A vector of point to create a convex hull from - * @param (input) shrink If positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length - * units towards the center along its normal). - * @param (input) shrinkClamp If positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where - * "innerRadius" is the minimum distance of a face to the center of the convex hull. - * @return The number of faces. If less than zero an error occurred when trying to create the convex hull - */ -int createConvexHull(AlignedVector& vertices, std::vector& faces, - const AlignedVector& input, double shrink = -1, double shrinkClamp = -1); - -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.hpp new file mode 100644 index 0000000000..1784da1f13 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/contact_checker_common.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2017, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +namespace collision_detection_bullet +{ +/** + * @brief Get a key for two object to search the collision matrix + * @param obj1 First collision object name + * @param obj2 Second collision object name + * @return The collision pair key + */ +inline std::pair getObjectPairKey(const std::string& obj1, const std::string& obj2) +{ + return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1); +} + +/** + * @brief This will check if a link is active provided a list. If the list is empty the link is considered active. + * @param active List of active link names + * @param name The name of link to check if it is active. + */ +inline bool isLinkActive(const std::vector& active, const std::string& name) +{ + return active.empty() || (std::find(active.begin(), active.end(), name) != active.end()); +} + +/** \brief Stores a single contact result in the requested way. + * \param found Indicates if a contact for this pair of objects has already been found + * \return Pointer to the newly inserted contact */ +collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact, + const std::pair& key, bool found); + +/** + * @brief Create a convex hull from vertices using Bullet Convex Hull Computer + * @param (Output) vertices A vector of vertices + * @param (Output) faces The first values indicates the number of vertices that define the face followed by the vertice + * index + * @param (input) input A vector of point to create a convex hull from + * @param (input) shrink If positive, the convex hull is shrunken by that amount (each face is moved by "shrink" length + * units towards the center along its normal). + * @param (input) shrinkClamp If positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where + * "innerRadius" is the minimum distance of a face to the center of the convex hull. + * @return The number of faces. If less than zero an error occurred when trying to create the convex hull + */ +int createConvexHull(AlignedVector& vertices, std::vector& faces, + const AlignedVector& input, double shrink = -1, double shrinkClamp = -1); + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h index 9a9f4859ea..0678d43f35 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (Apache License) * @@ -19,31 +31,5 @@ /* Author: Levi Armstrong */ #pragma once - -#include - -#include -#include -#include -#include -#include - -#include - -namespace collision_detection_bullet -{ -/** \brief Recursively traverses robot from root to get all active links - * - * \param active_links Stores the active links - * \param urdf_link The current urdf link representation - * \param active Indicates if link is considered active */ -void getActiveLinkNamesRecursive(std::vector& active_links, const urdf::LinkConstSharedPtr& urdf_link, - bool active); - -shapes::ShapePtr constructShape(const urdf::Geometry* geom); - -Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose); - -rclcpp::Logger getLogger(); - -} // namespace collision_detection_bullet +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.hpp new file mode 100644 index 0000000000..c97e799ea5 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.hpp @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (Apache License) + * + * Copyright (c) 2018, Southwest Research Institute + * All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* Author: Levi Armstrong */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include + +namespace collision_detection_bullet +{ +/** \brief Recursively traverses robot from root to get all active links + * + * \param active_links Stores the active links + * \param urdf_link The current urdf link representation + * \param active Indicates if link is considered active */ +void getActiveLinkNamesRecursive(std::vector& active_links, const urdf::LinkConstSharedPtr& urdf_link, + bool active); + +shapes::ShapePtr constructShape(const urdf::Geometry* geom); + +Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose); + +rclcpp::Logger getLogger(); + +} // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h index ed608502b9..ee177d7306 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,19 +47,5 @@ /* Author: Jens Petit */ #pragma once - -#include -#include - -#include - -namespace collision_detection -{ -/** \brief An allocator for Bullet collision detectors */ -class MOVEIT_COLLISION_DETECTION_BULLET_EXPORT CollisionDetectorAllocatorBullet - : public CollisionDetectorAllocatorTemplate -{ -public: - static const std::string NAME; // defined in collision_env_bullet.cpp -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.hpp new file mode 100644 index 0000000000..50b533030a --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_allocator_bullet.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include + +#include + +namespace collision_detection +{ +/** \brief An allocator for Bullet collision detectors */ +class MOVEIT_COLLISION_DETECTION_BULLET_EXPORT CollisionDetectorAllocatorBullet + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_env_bullet.cpp +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h index 1453e09049..67cd42a0ea 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,15 +47,5 @@ /* Author: Jens Petit */ #pragma once - -#include -#include - -namespace collision_detection -{ -class CollisionDetectorBtPluginLoader : public CollisionPlugin -{ -public: - bool initialize(const planning_scene::PlanningScenePtr& scene) const override; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.hpp new file mode 100644 index 0000000000..fd06e39782 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_detector_bullet_plugin_loader.hpp @@ -0,0 +1,49 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +class CollisionDetectorBtPluginLoader : public CollisionPlugin +{ +public: + bool initialize(const planning_scene::PlanningScenePtr& scene) const override; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h index 7854022874..a1aed35c78 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,114 +47,5 @@ /* Author: Jens Petit */ #pragma once - -#include -#include -#include -#include - -namespace collision_detection -{ -/** \brief */ -class CollisionEnvBullet : public CollisionEnv -{ -public: - CollisionEnvBullet() = delete; - - CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - - CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, - double scale = 1.0); - - CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world); - - ~CollisionEnvBullet() override; - - CollisionEnvBullet(CollisionEnvBullet&) = delete; - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; - - void setWorld(const WorldPtr& world) override; - -protected: - /** \brief Updates the poses of the objects in the manager according to given robot state */ - void updateTransformsFromState(const moveit::core::RobotState& state, - const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; - - /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links - */ - void updatedPaddingOrScaling(const std::vector& links) override; - - /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ - void addAttachedObjects(const moveit::core::RobotState& state, - std::vector& cows) const; - - /** \brief Bundles the different checkSelfCollision functions into a single function */ - void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; - - void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const AllowedCollisionMatrix* acm) const; - - void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; - - /** \brief Construts a bullet collision object out of a robot link */ - void addLinkAsCollisionObject(const urdf::LinkSharedPtr& link); - - /** \brief Handles self collision checks */ - collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_{ - new collision_detection_bullet::BulletDiscreteBVHManager() - }; - - /** \brief Handles continuous robot world collision checks */ - collision_detection_bullet::BulletCastBVHManagerPtr manager_CCD_{ - new collision_detection_bullet::BulletCastBVHManager() - }; - - // Lock manager_ and manager_CCD_, for thread-safe collision tests - mutable std::mutex collision_env_mutex_; - - /** \brief Adds a world object to the collision managers */ - void addToManager(const World::Object* obj); - - /** \brief Updates a managed collision object with its world representation. - * - * We have three cases: 1) the object is part of the manager and not of world --> delete it - * 2) the object is not in the manager, therefore register to manager, - * 3) the object is in the manager then delete and add the modified */ - void updateManagedObject(const std::string& id); - - /** \brief The active links where active refers to the group which can collide with everything */ - std::vector active_; - -private: - /** \brief Callback function executed for each change to the world environment */ - void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); - - World::ObserverHandle observer_handle_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.hpp b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.hpp new file mode 100644 index 0000000000..953686bd95 --- /dev/null +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_env_bullet.hpp @@ -0,0 +1,148 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace collision_detection +{ +/** \brief */ +class CollisionEnvBullet : public CollisionEnv +{ +public: + CollisionEnvBullet() = delete; + + CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + CollisionEnvBullet(const CollisionEnvBullet& other, const WorldPtr& world); + + ~CollisionEnvBullet() override; + + CollisionEnvBullet(CollisionEnvBullet&) = delete; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void setWorld(const WorldPtr& world) override; + +protected: + /** \brief Updates the poses of the objects in the manager according to given robot state */ + void updateTransformsFromState(const moveit::core::RobotState& state, + const collision_detection_bullet::BulletDiscreteBVHManagerPtr& manager) const; + + /** \brief Updates the collision objects saved in the manager to reflect a new padding or scaling of the robot links + */ + void updatedPaddingOrScaling(const std::vector& links) override; + + /** \brief All of the attached objects in the robot state are wrapped into bullet collision objects */ + void addAttachedObjects(const moveit::core::RobotState& state, + std::vector& cows) const; + + /** \brief Bundles the different checkSelfCollision functions into a single function */ + void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelperCCD(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, + const AllowedCollisionMatrix* acm) const; + + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Construts a bullet collision object out of a robot link */ + void addLinkAsCollisionObject(const urdf::LinkSharedPtr& link); + + /** \brief Handles self collision checks */ + collision_detection_bullet::BulletDiscreteBVHManagerPtr manager_{ + new collision_detection_bullet::BulletDiscreteBVHManager() + }; + + /** \brief Handles continuous robot world collision checks */ + collision_detection_bullet::BulletCastBVHManagerPtr manager_CCD_{ + new collision_detection_bullet::BulletCastBVHManager() + }; + + // Lock manager_ and manager_CCD_, for thread-safe collision tests + mutable std::mutex collision_env_mutex_; + + /** \brief Adds a world object to the collision managers */ + void addToManager(const World::Object* obj); + + /** \brief Updates a managed collision object with its world representation. + * + * We have three cases: 1) the object is part of the manager and not of world --> delete it + * 2) the object is not in the manager, therefore register to manager, + * 3) the object is in the manager then delete and add the modified */ + void updateManagedObject(const std::string& id); + + /** \brief The active links where active refers to the group which can collide with everything */ + std::vector active_; + +private: + /** \brief Callback function executed for each change to the world environment */ + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + + World::ObserverHandle observer_handle_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp index e9af94b015..2883f264d8 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp @@ -31,7 +31,7 @@ /* Authors: Levi Armstrong, Jens Petit */ -#include +#include #include #include diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp index e7153a2b16..533782ed4b 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp @@ -31,13 +31,13 @@ /* Author: Levi Armstrong, Jens Petit */ -#include +#include #include #include #include #include -#include +#include namespace collision_detection_bullet { diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp index 29f56f3b3b..780cd74e22 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp @@ -31,11 +31,11 @@ /* Author: Levi Armstrong, Jens Petit */ -#include +#include #include #include -#include +#include namespace collision_detection_bullet { diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp index 3fe31c3824..45a8952f13 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_utils.cpp @@ -32,7 +32,7 @@ /* Authors: John Schulman, Levi Armstrong */ -#include +#include #include #include @@ -42,7 +42,7 @@ #include #include #include -#include +#include namespace collision_detection_bullet { diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp index 4da803358e..f0bffc58f4 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp @@ -33,11 +33,11 @@ * Author: Levi Armstrong */ -#include +#include #include #include -#include +#include namespace collision_detection_bullet { diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp index b326780991..2e308bfbd2 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp @@ -31,7 +31,7 @@ /* Author: Jorge Nicho*/ -#include +#include #include #include diff --git a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp index 5e319499f5..ba85581a27 100644 --- a/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp @@ -34,7 +34,7 @@ /* Author: Jens Petit */ -#include +#include #include namespace collision_detection diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp index 873dfb0a21..37210fe66d 100644 --- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp +++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp @@ -34,10 +34,10 @@ /* Author: Jens Petit */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp index 1e164b33f7..209b5bfcb1 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_panda.cpp @@ -34,8 +34,8 @@ /* Author: Jens Petit */ -#include -#include +#include +#include INSTANTIATE_TYPED_TEST_SUITE_P(BulletCollisionCheckPanda, CollisionDetectorPandaTest, collision_detection::CollisionDetectorAllocatorBullet); diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp index 997d73b689..4b69a4d17d 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_collision_detection_pr2.cpp @@ -34,8 +34,8 @@ /* Author: Jens Petit */ -#include -#include +#include +#include INSTANTIATE_TYPED_TEST_SUITE_P(BulletCollisionCheck, CollisionDetectorTest, collision_detection::CollisionDetectorAllocatorBullet); diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 16c62799ff..0bcc15c7df 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -37,16 +37,16 @@ #include #include -#include -#include -#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include #include #include 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 84e7f6266d..9f4d7152bb 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,336 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) -#include -#include -#include -#else -#include -#include -#include -#endif - -#include -#include - -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 moveit::core::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 moveit::core::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) - { - case BodyTypes::ROBOT_LINK: - return ptr.link->getName(); - case BodyTypes::ROBOT_ATTACHED: - return ptr.ab->getName(); - default: - break; - } - return ptr.obj->id_; - } - - /** \brief Returns a string of the corresponding \e type. */ - std::string getTypeString() const - { - switch (type) - { - case BodyTypes::ROBOT_LINK: - return "Robot link"; - case BodyTypes::ROBOT_ATTACHED: - return "Robot attached"; - default: - break; - } - return "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 moveit::core::LinkModel* link; - const moveit::core::AttachedBody* ab; - const World::Object* obj; - const void* raw; - } ptr; -}; - -/** \brief Data structure which is passed to the collision callback function of the collision manager. */ -struct CollisionData -{ - CollisionData() : req_(nullptr), active_components_only_(nullptr), res_(nullptr), acm_(nullptr), done_(false) - { - } - - CollisionData(const CollisionRequest* req, CollisionResult* res, const AllowedCollisionMatrix* acm) - : req_(req), active_components_only_(nullptr), res_(res), acm_(acm), done_(false) - { - } - - ~CollisionData() - { - } - - /** \brief Compute \e active_components_only_ based on the joint group specified in \e req_ */ - void enableGroup(const moveit::core::RobotModelConstPtr& robot_model); - - /** \brief The collision request passed by the user */ - const CollisionRequest* req_; - - /** \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 nullptr, all collisions are considered. */ - const std::set* active_components_only_; - - /** \brief The user-specified response location. */ - CollisionResult* res_; - - /** \brief The user-specified collision matrix (may be nullptr). */ - const AllowedCollisionMatrix* acm_; - - /** \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) - { - } - ~DistanceData() - { - } - - /** \brief Distance query request information. */ - const DistanceRequest* req; - - /** \brief Distance query results information. */ - DistanceResult* res; - - /** \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 moveit::core::LinkModel* link, int shape_index) - : collision_geometry_(collision_geometry) - , collision_geometry_data_(std::make_shared(link, shape_index)) - { - collision_geometry_->setUserData(collision_geometry_data_.get()); - } - - /** \brief Constructor for an attached body. */ - FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index) - : collision_geometry_(collision_geometry) - , collision_geometry_data_(std::make_shared(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_(std::make_shared(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) - { - if (!newType && collision_geometry_data_) - { - if (collision_geometry_data_->ptr.raw == reinterpret_cast(data)) - return; - } - collision_geometry_data_ = std::make_shared(data, shape_index); - 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); - void unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager); - 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 collision 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 distance 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 moveit::core::LinkModel* link, - int shape_index); - -/** \brief Create new FCLGeometry object out of attached body. */ -FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::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 moveit::core::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 moveit::core::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) -{ - ASSERT_ISOMETRY(b); -#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) - f = b.matrix(); -#else - Eigen::Quaterniond q(b.linear()); - f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z())); - f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z())); -#endif -} - -/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ -inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b) -{ - fcl::Transform3d t; - transform2fcl(b, t); - 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]); - c.normal = Eigen::Vector3d(fc.normal[0], fc.normal[1], fc.normal[2]); - c.depth = fc.penetration_depth; - const CollisionGeometryData* cgd1 = static_cast(fc.o1->getUserData()); - c.body_name_1 = cgd1->getID(); - c.body_type_1 = cgd1->type; - const CollisionGeometryData* cgd2 = static_cast(fc.o2->getUserData()); - c.body_name_2 = cgd2->getID(); - 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]; - cs.aabb_min[1] = fcs.aabb_min[1]; - cs.aabb_min[2] = fcs.aabb_min[2]; - - cs.aabb_max[0] = fcs.aabb_max[0]; - cs.aabb_max[1] = fcs.aabb_max[1]; - cs.aabb_max[2] = fcs.aabb_max[2]; - - cs.cost = fcs.cost_density; -} -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.hpp new file mode 100644 index 0000000000..6869a8ac77 --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.hpp @@ -0,0 +1,370 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) +#include +#include +#include +#else +#include +#include +#include +#endif + +#include +#include + +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 moveit::core::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 moveit::core::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) + { + case BodyTypes::ROBOT_LINK: + return ptr.link->getName(); + case BodyTypes::ROBOT_ATTACHED: + return ptr.ab->getName(); + default: + break; + } + return ptr.obj->id_; + } + + /** \brief Returns a string of the corresponding \e type. */ + std::string getTypeString() const + { + switch (type) + { + case BodyTypes::ROBOT_LINK: + return "Robot link"; + case BodyTypes::ROBOT_ATTACHED: + return "Robot attached"; + default: + break; + } + return "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 moveit::core::LinkModel* link; + const moveit::core::AttachedBody* ab; + const World::Object* obj; + const void* raw; + } ptr; +}; + +/** \brief Data structure which is passed to the collision callback function of the collision manager. */ +struct CollisionData +{ + CollisionData() : req_(nullptr), active_components_only_(nullptr), res_(nullptr), acm_(nullptr), done_(false) + { + } + + CollisionData(const CollisionRequest* req, CollisionResult* res, const AllowedCollisionMatrix* acm) + : req_(req), active_components_only_(nullptr), res_(res), acm_(acm), done_(false) + { + } + + ~CollisionData() + { + } + + /** \brief Compute \e active_components_only_ based on the joint group specified in \e req_ */ + void enableGroup(const moveit::core::RobotModelConstPtr& robot_model); + + /** \brief The collision request passed by the user */ + const CollisionRequest* req_; + + /** \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 nullptr, all collisions are considered. */ + const std::set* active_components_only_; + + /** \brief The user-specified response location. */ + CollisionResult* res_; + + /** \brief The user-specified collision matrix (may be nullptr). */ + const AllowedCollisionMatrix* acm_; + + /** \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) + { + } + ~DistanceData() + { + } + + /** \brief Distance query request information. */ + const DistanceRequest* req; + + /** \brief Distance query results information. */ + DistanceResult* res; + + /** \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 moveit::core::LinkModel* link, int shape_index) + : collision_geometry_(collision_geometry) + , collision_geometry_data_(std::make_shared(link, shape_index)) + { + collision_geometry_->setUserData(collision_geometry_data_.get()); + } + + /** \brief Constructor for an attached body. */ + FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index) + : collision_geometry_(collision_geometry) + , collision_geometry_data_(std::make_shared(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_(std::make_shared(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) + { + if (!newType && collision_geometry_data_) + { + if (collision_geometry_data_->ptr.raw == reinterpret_cast(data)) + return; + } + collision_geometry_data_ = std::make_shared(data, shape_index); + 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); + void unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager); + 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 collision 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 distance 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 moveit::core::LinkModel* link, + int shape_index); + +/** \brief Create new FCLGeometry object out of attached body. */ +FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::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 moveit::core::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 moveit::core::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) +{ + ASSERT_ISOMETRY(b); +#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) + f = b.matrix(); +#else + Eigen::Quaterniond q(b.linear()); + f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z())); + f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z())); +#endif +} + +/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ +inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b) +{ + fcl::Transform3d t; + transform2fcl(b, t); + 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]); + c.normal = Eigen::Vector3d(fc.normal[0], fc.normal[1], fc.normal[2]); + c.depth = fc.penetration_depth; + const CollisionGeometryData* cgd1 = static_cast(fc.o1->getUserData()); + c.body_name_1 = cgd1->getID(); + c.body_type_1 = cgd1->type; + const CollisionGeometryData* cgd2 = static_cast(fc.o2->getUserData()); + c.body_name_2 = cgd2->getID(); + 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]; + cs.aabb_min[1] = fcs.aabb_min[1]; + cs.aabb_min[2] = fcs.aabb_min[2]; + + cs.aabb_max[0] = fcs.aabb_max[0]; + cs.aabb_max[1] = fcs.aabb_max[1]; + cs.aabb_max[2] = fcs.aabb_max[2]; + + cs.cost = fcs.cost_density; +} +} // namespace collision_detection 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 77a84274cc..f673c6a5df 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,19 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan */ #pragma once - -#include -#include - -#include - -namespace collision_detection -{ -/** \brief An allocator for FCL collision detectors */ -class MOVEIT_COLLISION_DETECTION_FCL_EXPORT CollisionDetectorAllocatorFCL - : public CollisionDetectorAllocatorTemplate -{ -public: - static const std::string NAME; // defined in collision_env_fcl.cpp -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.hpp new file mode 100644 index 0000000000..b7a2bce548 --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Acorn Pooley, Ioan Sucan */ + +#pragma once + +#include +#include + +#include + +namespace collision_detection +{ +/** \brief An allocator for FCL collision detectors */ +class MOVEIT_COLLISION_DETECTION_FCL_EXPORT CollisionDetectorAllocatorFCL + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_env_fcl.cpp +}; +} // namespace collision_detection 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 0a967b90fc..3830750f37 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,15 +47,5 @@ /* Author: Bryce Willey */ #pragma once - -#include -#include - -namespace collision_detection -{ -class CollisionDetectorFCLPluginLoader : public CollisionPlugin -{ -public: - bool initialize(const planning_scene::PlanningScenePtr& scene) const override; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.hpp new file mode 100644 index 0000000000..6bc2a772ec --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.hpp @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Bryce Willey + * 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: Bryce Willey */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +class CollisionDetectorFCLPluginLoader : public CollisionPlugin +{ +public: + bool initialize(const planning_scene::PlanningScenePtr& scene) const override; +}; +} // namespace collision_detection 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 index cf5249fd70..ac6229523d 100644 --- 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,125 +47,5 @@ /* 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 moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); - - CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, - double scale = 1.0); - - CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world); - - ~CollisionEnvFCL() override; - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - - void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; - - void distanceSelf(const DistanceRequest& req, DistanceResult& res, - const moveit::core::RobotState& state) const override; - - void distanceRobot(const DistanceRequest& req, DistanceResult& res, - const moveit::core::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 moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; - - /** \brief Bundles the different checkRobotCollision functions into a single function */ - void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, - const moveit::core::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 moveit::core::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 moveit::core::RobotState& state, FCLManager& manager) const; - - /** \brief Converts all shapes which make up an attached 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 moveit::core::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_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.hpp new file mode 100644 index 0000000000..98146a6555 --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.hpp @@ -0,0 +1,159 @@ +/********************************************************************* + * 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 moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0); + + CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0, + double scale = 1.0); + + CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world); + + ~CollisionEnvFCL() override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + + void distanceSelf(const DistanceRequest& req, DistanceResult& res, + const moveit::core::RobotState& state) const override; + + void distanceRobot(const DistanceRequest& req, DistanceResult& res, + const moveit::core::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 moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const; + + /** \brief Bundles the different checkRobotCollision functions into a single function */ + void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, + const moveit::core::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 moveit::core::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 moveit::core::RobotState& state, FCLManager& manager) const; + + /** \brief Converts all shapes which make up an attached 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 moveit::core::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_; +}; +} // namespace collision_detection 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 6d5c774a87..8c649bdf7d 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,65 +47,5 @@ /* Author: Benjamin Scholz */ #pragma once - -#include - -#define FCL_VERSION_CHECK(major, minor, patch) ((major << 16) | (minor << 8) | (patch)) -#define MOVEIT_FCL_VERSION FCL_VERSION_CHECK(FCL_MAJOR_VERSION, FCL_MINOR_VERSION, FCL_PATCH_VERSION) - -#if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) -namespace fcl -{ -class CollisionGeometry; -using CollisionGeometryd = fcl::CollisionGeometry; -class CollisionObject; -using CollisionObjectd = fcl::CollisionObject; -class BroadPhaseCollisionManager; -using BroadPhaseCollisionManagerd = fcl::BroadPhaseCollisionManager; -class Transform3f; -using Transform3d = fcl::Transform3f; -class Contact; -using Contactd = fcl::Contact; -class CostSource; -using CostSourced = fcl::CostSource; -class CollisionRequest; -using CollisionRequestd = fcl::CollisionRequest; -class CollisionResult; -using CollisionResultd = fcl::CollisionResult; -class DistanceRequest; -using DistanceRequestd = fcl::DistanceRequest; -class DistanceResult; -using DistanceResultd = fcl::DistanceResult; -class Plane; -using Planed = fcl::Plane; -class Sphere; -using Sphered = fcl::Sphere; -class Box; -using Boxd = fcl::Box; -class Cylinder; -using Cylinderd = fcl::Cylinder; -class Cone; -using Coned = fcl::Cone; - -namespace details -{ -struct sse_meta_f4; -template -struct Vec3Data; -} // namespace details -template -class Vec3fX; -#if FCL_HAVE_SSE -using Vector3d = Vec3fX; -#else -using Vector3d = Vec3fX >; -#endif - -class OcTree; -using OcTreed = fcl::OcTree; -class OBBRSS; -using OBBRSSd = fcl::OBBRSS; -class DynamicAABBTreeCollisionManager; -using DynamicAABBTreeCollisionManagerd = fcl::DynamicAABBTreeCollisionManager; -} // namespace fcl -#endif +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.hpp new file mode 100644 index 0000000000..6d5c774a87 --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.hpp @@ -0,0 +1,99 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Hamburg University. + * 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: Benjamin Scholz */ + +#pragma once + +#include + +#define FCL_VERSION_CHECK(major, minor, patch) ((major << 16) | (minor << 8) | (patch)) +#define MOVEIT_FCL_VERSION FCL_VERSION_CHECK(FCL_MAJOR_VERSION, FCL_MINOR_VERSION, FCL_PATCH_VERSION) + +#if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) +namespace fcl +{ +class CollisionGeometry; +using CollisionGeometryd = fcl::CollisionGeometry; +class CollisionObject; +using CollisionObjectd = fcl::CollisionObject; +class BroadPhaseCollisionManager; +using BroadPhaseCollisionManagerd = fcl::BroadPhaseCollisionManager; +class Transform3f; +using Transform3d = fcl::Transform3f; +class Contact; +using Contactd = fcl::Contact; +class CostSource; +using CostSourced = fcl::CostSource; +class CollisionRequest; +using CollisionRequestd = fcl::CollisionRequest; +class CollisionResult; +using CollisionResultd = fcl::CollisionResult; +class DistanceRequest; +using DistanceRequestd = fcl::DistanceRequest; +class DistanceResult; +using DistanceResultd = fcl::DistanceResult; +class Plane; +using Planed = fcl::Plane; +class Sphere; +using Sphered = fcl::Sphere; +class Box; +using Boxd = fcl::Box; +class Cylinder; +using Cylinderd = fcl::Cylinder; +class Cone; +using Coned = fcl::Cone; + +namespace details +{ +struct sse_meta_f4; +template +struct Vec3Data; +} // namespace details +template +class Vec3fX; +#if FCL_HAVE_SSE +using Vector3d = Vec3fX; +#else +using Vector3d = Vec3fX >; +#endif + +class OcTree; +using OcTreed = fcl::OcTree; +class OBBRSS; +using OBBRSSd = fcl::OBBRSS; +class DynamicAABBTreeCollisionManager; +using DynamicAABBTreeCollisionManagerd = fcl::DynamicAABBTreeCollisionManager; +} // namespace fcl +#endif diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index faf5e8eab4..89ee984e09 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan, Jia Pan */ -#include +#include #include -#include +#include #include #include #include diff --git a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp index dd55132fd2..5ca379d82f 100644 --- a/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp @@ -34,7 +34,7 @@ /* Author: Bryce Willey */ -#include +#include #include 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 index 86a48b8320..b57e0767ea 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -34,11 +34,11 @@ /* Author: Ioan Sucan, Jens Petit */ -#include -#include -#include +#include +#include +#include -#include +#include #include #include #include diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp index 78aa8732b8..63a1dfa437 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_panda.cpp @@ -34,8 +34,8 @@ /* Author: Jens Petit */ -#include -#include +#include +#include INSTANTIATE_TYPED_TEST_SUITE_P(FCLCollisionCheckPanda, CollisionDetectorPandaTest, collision_detection::CollisionDetectorAllocatorFCL); diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp index 6818fb13a9..e34e28c3f0 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection_pr2.cpp @@ -34,8 +34,8 @@ /* Author: Jens Petit */ -#include -#include +#include +#include INSTANTIATE_TYPED_TEST_SUITE_P(FCLCollisionCheck, CollisionDetectorTest, collision_detection::CollisionDetectorAllocatorFCL); diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp index e1a59bbddb..53ad6f9d8a 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp @@ -36,15 +36,15 @@ #include -#include +#include -#include -#include -#include +#include +#include +#include #include -#include -#include +#include +#include #include #include 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 e40eefdf36..c18155ff68 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,148 +47,5 @@ /* Author: E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include - -namespace collision_detection -{ -MOVEIT_STRUCT_FORWARD(GroupStateRepresentation); -MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntry); - -/** collision volume representation for a particular pose and link group - * - * This stores posed spheres making up the collision volume for a particular - * link group in a particular pose. It is associated with a particular dfce_ - * and can only be used with that dfce_ (DistanceFieldCacheEntry -- see below). - * */ -struct GroupStateRepresentation -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - GroupStateRepresentation(){}; - GroupStateRepresentation(const GroupStateRepresentation& gsr) - { - link_body_decompositions_.resize(gsr.link_body_decompositions_.size()); - for (unsigned int i = 0; i < gsr.link_body_decompositions_.size(); ++i) - { - if (gsr.link_body_decompositions_[i]) - { - link_body_decompositions_[i] = - std::make_shared(*gsr.link_body_decompositions_[i]); - } - } - - link_distance_fields_.assign(gsr.link_distance_fields_.begin(), gsr.link_distance_fields_.end()); - - attached_body_decompositions_.resize(gsr.attached_body_decompositions_.size()); - for (unsigned int i = 0; i < gsr.attached_body_decompositions_.size(); ++i) - { - (*attached_body_decompositions_[i]) = (*gsr.attached_body_decompositions_[i]); - } - gradients_ = gsr.gradients_; - } - - /** dfce used to generate this GSR */ - DistanceFieldCacheEntryConstPtr dfce_; - - /** posed spheres representing collision volume for the links in the group - * (dfce_.group_name_) and all links below the group (i.e. links that can - * move if joints in the group move). These are posed in the global frame - * used for collision detection. */ - std::vector link_body_decompositions_; - - /** posed spheres representing collision volume for bodies attached to group - * links */ - std::vector attached_body_decompositions_; - - /** */ - std::vector link_distance_fields_; - - /** information about detected collisions, collected during collision - * detection. One entry for each link and one entry for each attached - * object. */ - std::vector gradients_; -}; - -/** collision volume representation for a particular link group of a robot - * - * This entry is specific to a particular robot model, a particular link group, - * a particular ACM, and a particular configuration of attached objects. It - * assumes the poses of joints above this group have not changed, but can be - * used with different poses of the joints within the group. */ -struct DistanceFieldCacheEntry -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** for checking collisions between this group and other objects */ - std::string group_name_; - /** RobotState that this cache entry represents */ - moveit::core::RobotStatePtr state_; - /** list of indices into the state_values_ vector. One index for each joint - * variable which is NOT in the group or a child of the group. In other - * words, variables which should not change if only joints in the group move. - */ - std::vector state_check_indices_; - /** all the joint variables from all the joints in the entire robot (whether - * in the group or not), excluding mimic joints. If 2 - * DistanceFieldCacheEntrys have identical state_values_ then they are - * equivalent (i.e. same robot state). */ - std::vector state_values_; - /* the acm used when generating this dfce. This dfce cannot be used to check - * collisions with a different acm. */ - collision_detection::AllowedCollisionMatrix acm_; - /** the distance field describing all links of the robot that are not in the - * group and their attached bodies */ - distance_field::DistanceFieldPtr distance_field_; - /** this can be used as a starting point for creating a - * GroupStateRepresentation needed for collision checking */ - GroupStateRepresentationPtr pregenerated_group_state_representation_; - /** names of all links in the group and all links below the group (links that - * will move if any of the joints in the group move) - */ - std::vector link_names_; - /** for each link in link_names_, true if the link has collision geometry */ - std::vector link_has_geometry_; - /** for each link in link_names_, index into the - * CollisionRobotDistanceField::link_body_decomposition_vector_ vector. This - * is 0 (and invalid) for links with no geometry. The - * link_body_decomposition_vector_ contains the (unposed) spheres that make - * up the collision geometry for the link */ - std::vector link_body_indices_; - /** for each link in link_names_, index into the - * RobotState::link_state_vector_ */ - std::vector link_state_indices_; - /** list of all bodies attached to links in link_names_ */ - std::vector attached_body_names_; - /** for each body in attached_body_names_, the index into the link state of - * the link the body is attached to. */ - std::vector attached_body_link_state_indices_; - /** for each link in link_names_ and for each body in attached_body_names_, - * true if this link should be checked for self collision */ - std::vector self_collision_enabled_; - /** for each link in link_names_, a vector of bool indicating for each other - * object (where object could be another link or an attacjed object) whether - * to check for collisions between this link and that object. Size of inner - * and outer lists are the same and equal the sum of the size of link_names_ - * and attached_body_names_ */ - std::vector> intra_group_collision_enabled_; -}; - -BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr& shape, double resolution); - -PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj, - double resolution); - -PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, - double resolution); - -PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, - double resolution); - -void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame, - visualization_msgs::msg::MarkerArray& body_marker_array); -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.hpp b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.hpp new file mode 100644 index 0000000000..0faa884f90 --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.hpp @@ -0,0 +1,182 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 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: E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace collision_detection +{ +MOVEIT_STRUCT_FORWARD(GroupStateRepresentation); +MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntry); + +/** collision volume representation for a particular pose and link group + * + * This stores posed spheres making up the collision volume for a particular + * link group in a particular pose. It is associated with a particular dfce_ + * and can only be used with that dfce_ (DistanceFieldCacheEntry -- see below). + * */ +struct GroupStateRepresentation +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + GroupStateRepresentation(){}; + GroupStateRepresentation(const GroupStateRepresentation& gsr) + { + link_body_decompositions_.resize(gsr.link_body_decompositions_.size()); + for (unsigned int i = 0; i < gsr.link_body_decompositions_.size(); ++i) + { + if (gsr.link_body_decompositions_[i]) + { + link_body_decompositions_[i] = + std::make_shared(*gsr.link_body_decompositions_[i]); + } + } + + link_distance_fields_.assign(gsr.link_distance_fields_.begin(), gsr.link_distance_fields_.end()); + + attached_body_decompositions_.resize(gsr.attached_body_decompositions_.size()); + for (unsigned int i = 0; i < gsr.attached_body_decompositions_.size(); ++i) + { + (*attached_body_decompositions_[i]) = (*gsr.attached_body_decompositions_[i]); + } + gradients_ = gsr.gradients_; + } + + /** dfce used to generate this GSR */ + DistanceFieldCacheEntryConstPtr dfce_; + + /** posed spheres representing collision volume for the links in the group + * (dfce_.group_name_) and all links below the group (i.e. links that can + * move if joints in the group move). These are posed in the global frame + * used for collision detection. */ + std::vector link_body_decompositions_; + + /** posed spheres representing collision volume for bodies attached to group + * links */ + std::vector attached_body_decompositions_; + + /** */ + std::vector link_distance_fields_; + + /** information about detected collisions, collected during collision + * detection. One entry for each link and one entry for each attached + * object. */ + std::vector gradients_; +}; + +/** collision volume representation for a particular link group of a robot + * + * This entry is specific to a particular robot model, a particular link group, + * a particular ACM, and a particular configuration of attached objects. It + * assumes the poses of joints above this group have not changed, but can be + * used with different poses of the joints within the group. */ +struct DistanceFieldCacheEntry +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** for checking collisions between this group and other objects */ + std::string group_name_; + /** RobotState that this cache entry represents */ + moveit::core::RobotStatePtr state_; + /** list of indices into the state_values_ vector. One index for each joint + * variable which is NOT in the group or a child of the group. In other + * words, variables which should not change if only joints in the group move. + */ + std::vector state_check_indices_; + /** all the joint variables from all the joints in the entire robot (whether + * in the group or not), excluding mimic joints. If 2 + * DistanceFieldCacheEntrys have identical state_values_ then they are + * equivalent (i.e. same robot state). */ + std::vector state_values_; + /* the acm used when generating this dfce. This dfce cannot be used to check + * collisions with a different acm. */ + collision_detection::AllowedCollisionMatrix acm_; + /** the distance field describing all links of the robot that are not in the + * group and their attached bodies */ + distance_field::DistanceFieldPtr distance_field_; + /** this can be used as a starting point for creating a + * GroupStateRepresentation needed for collision checking */ + GroupStateRepresentationPtr pregenerated_group_state_representation_; + /** names of all links in the group and all links below the group (links that + * will move if any of the joints in the group move) + */ + std::vector link_names_; + /** for each link in link_names_, true if the link has collision geometry */ + std::vector link_has_geometry_; + /** for each link in link_names_, index into the + * CollisionRobotDistanceField::link_body_decomposition_vector_ vector. This + * is 0 (and invalid) for links with no geometry. The + * link_body_decomposition_vector_ contains the (unposed) spheres that make + * up the collision geometry for the link */ + std::vector link_body_indices_; + /** for each link in link_names_, index into the + * RobotState::link_state_vector_ */ + std::vector link_state_indices_; + /** list of all bodies attached to links in link_names_ */ + std::vector attached_body_names_; + /** for each body in attached_body_names_, the index into the link state of + * the link the body is attached to. */ + std::vector attached_body_link_state_indices_; + /** for each link in link_names_ and for each body in attached_body_names_, + * true if this link should be checked for self collision */ + std::vector self_collision_enabled_; + /** for each link in link_names_, a vector of bool indicating for each other + * object (where object could be another link or an attacjed object) whether + * to check for collisions between this link and that object. Size of inner + * and outer lists are the same and equal the sum of the size of link_names_ + * and attached_body_names_ */ + std::vector> intra_group_collision_enabled_; +}; + +BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr& shape, double resolution); + +PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj, + double resolution); + +PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att, + double resolution); + +PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att, + double resolution); + +void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame, + visualization_msgs::msg::MarkerArray& body_marker_array); +} // namespace collision_detection 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 188be97465..7a4d99909c 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 @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,17 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan */ #pragma once - -#include -#include - -namespace collision_detection -{ -/** \brief An allocator for Distance Field collision detectors */ -class CollisionDetectorAllocatorDistanceField - : public CollisionDetectorAllocatorTemplate -{ -public: - static const std::string NAME; // defined in collision_env_distance_field.cpp -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.hpp b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.hpp new file mode 100644 index 0000000000..26c6d5ff73 --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.hpp @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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 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: Acorn Pooley, Ioan Sucan */ + +#pragma once + +#include +#include + +namespace collision_detection +{ +/** \brief An allocator for Distance Field collision detectors */ +class CollisionDetectorAllocatorDistanceField + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_env_distance_field.cpp +}; +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h index db557e9cf3..a251248fde 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,19 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan */ #pragma once - -#include -#include - -#include - -namespace collision_detection -{ -/** \brief An allocator for Hybrid collision detectors */ -class MOVEIT_COLLISION_DISTANCE_FIELD_EXPORT CollisionDetectorAllocatorHybrid - : public CollisionDetectorAllocatorTemplate -{ -public: - static const std::string NAME; // defined in collision_env_hybrid.cpp -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.hpp b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.hpp new file mode 100644 index 0000000000..a64c5b7fbb --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_hybrid.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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 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: Acorn Pooley, Ioan Sucan */ + +#pragma once + +#include +#include + +#include + +namespace collision_detection +{ +/** \brief An allocator for Hybrid collision detectors */ +class MOVEIT_COLLISION_DISTANCE_FIELD_EXPORT CollisionDetectorAllocatorHybrid + : public CollisionDetectorAllocatorTemplate +{ +public: + static const std::string NAME; // defined in collision_env_hybrid.cpp +}; +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 5887a9cdab..a115d416aa 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,502 +47,5 @@ /* Author: E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -enum CollisionType -{ - NONE = 0, - SELF = 1, - INTRA = 2, - ENVIRONMENT = 3, -}; - -struct CollisionSphere -{ - CollisionSphere(const Eigen::Vector3d& rel, double radius) - { - relative_vec_ = rel; - radius_ = radius; - } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Eigen::Vector3d relative_vec_; - double radius_; -}; - -struct GradientInfo -{ - GradientInfo() : closest_distance(DBL_MAX), collision(false) - { - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - double closest_distance; - bool collision; - EigenSTL::vector_Vector3d sphere_locations; - std::vector distances; - EigenSTL::vector_Vector3d gradients; - std::vector types; - std::vector sphere_radii; - std::string joint_name; - - void clear() - { - closest_distance = DBL_MAX; - collision = false; - sphere_locations.clear(); - distances.clear(); - gradients.clear(); - sphere_radii.clear(); - joint_name.clear(); - } -}; - -// Defines ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(PosedDistanceField); -MOVEIT_CLASS_FORWARD(BodyDecomposition); -MOVEIT_CLASS_FORWARD(PosedBodySphereDecomposition); -MOVEIT_CLASS_FORWARD(PosedBodyPointDecomposition); -MOVEIT_CLASS_FORWARD(PosedBodySphereDecompositionVector); -MOVEIT_CLASS_FORWARD(PosedBodyPointDecompositionVector); - -class PosedDistanceField : public distance_field::PropagationDistanceField -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PosedDistanceField(const Eigen::Vector3d& size, const Eigen::Vector3d& origin, double resolution, double max_distance, - bool propagate_negative_distances = false) - : distance_field::PropagationDistanceField(size.x(), size.y(), size.z(), resolution, origin.x(), origin.y(), - origin.z(), max_distance, propagate_negative_distances) - , pose_(Eigen::Isometry3d::Identity()) - { - } - - void updatePose(const Eigen::Isometry3d& transform) - { - pose_ = transform; - } - - const Eigen::Isometry3d& getPose() const - { - return pose_; - } - - /** - * @brief Gets not only the distance to the nearest cell but the gradient - * direction. The point - * defined by the x, y and z values is transform into the local distance field - * coordinate system using - * the current pose. - * The gradient is computed as a function of the distances of near-by cells. - * An uninitialized - * distance is returned if the cell is not valid for gradient production - * purposes. - * @param x input x value of the query point - * @param y input y value of the query point - * @param z input z value of the query point - * @param gradient_x output argument with the x value of the gradient - * @param gradient_y output argument with the y value of the gradient - * @param gradient_z output argument with the z value of the gradient - * @param in_bounds true if the point is within the bounds of the distance - * field, false otherwise - */ - double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z, - bool& in_bounds) const - { - // Transpose of a rotation matrix equals its inverse, but computationally cheaper - Eigen::Vector3d rel_pos = pose_.linear().transpose() * Eigen::Vector3d(x, y, z); - double gx, gy, gz; - double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(), - gx, gy, gz, in_bounds); - Eigen::Vector3d grad = pose_ * Eigen::Vector3d(gx, gy, gz); - gradient_x = grad.x(); - gradient_y = grad.y(); - gradient_z = grad.z(); - return res; - } - - /* - * @brief determines a set of gradients of the given collision spheres in the - * distance field - * @param sphere_list vector of the spheres that approximate a links geometry - * @param sphere_centers vector of points which indicate the center of each - * sphere in sphere_list - * @param gradient output argument to be populated with the resulting gradient - * calculation - * @param tolerance - * @param subtract_radii distance to the sphere centers will be computed by - * subtracting the sphere radius from the nearest point - * @param maximum_value - * @param stop_at_first_collision when true the computation is terminated when - * the first collision is found - */ - bool getCollisionSphereGradients(const std::vector& sphere_list, - const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient, - const CollisionType& type, double tolerance, bool subtract_radii, - double maximum_value, bool stop_at_first_collision); - -protected: - Eigen::Isometry3d pose_; -}; - -// determines set of collision spheres given a posed body; this is BAD! -// Allocation errors will happen; change this function so it does not return -// that vector by value -std::vector determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relativeTransform); - -// determines a set of gradients of the given collision spheres in the distance -// field -bool getCollisionSphereGradients(const distance_field::DistanceField* distance_field, - const std::vector& sphere_list, - const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient, - const CollisionType& type, double tolerance, bool subtract_radii, double maximum_value, - bool stop_at_first_collision); - -bool getCollisionSphereCollision(const distance_field::DistanceField* distance_field, - const std::vector& sphere_list, - const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value, - double tolerance); - -bool getCollisionSphereCollision(const distance_field::DistanceField* distance_field, - const std::vector& sphere_list, - const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value, - double tolerance, unsigned int num_coll, std::vector& colls); - -// forward declaration required for friending apparently -class BodyDecompositionVector; - -class BodyDecomposition -{ - friend class BodyDecompositionVector; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - BodyDecomposition(const shapes::ShapeConstPtr& shape, double resolution, double padding = 0.01); - - BodyDecomposition(const std::vector& shapes, const EigenSTL::vector_Isometry3d& poses, - double resolution, double padding); - - ~BodyDecomposition(); - - Eigen::Isometry3d relative_cylinder_pose_; - - void replaceCollisionSpheres(const std::vector& new_collision_spheres, - const Eigen::Isometry3d& new_relative_cylinder_pose) - { - // std::cerr << "Replacing " << collision_spheres_.size() << " with " << - // new_collision_spheres.size() << '\n'; - collision_spheres_ = new_collision_spheres; - relative_cylinder_pose_ = new_relative_cylinder_pose; - } - - const std::vector& getCollisionSpheres() const - { - return collision_spheres_; - } - - const std::vector& getSphereRadii() const - { - return sphere_radii_; - } - - const EigenSTL::vector_Vector3d& getCollisionPoints() const - { - return relative_collision_points_; - } - - const bodies::Body* getBody(unsigned int i) const - { - return bodies_.getBody(i); - } - - unsigned int getBodiesCount() - { - return bodies_.getCount(); - } - - Eigen::Isometry3d getRelativeCylinderPose() const - { - return relative_cylinder_pose_; - } - - const bodies::BoundingSphere& getRelativeBoundingSphere() const - { - return relative_bounding_sphere_; - } - -protected: - void init(const std::vector& shapes, const EigenSTL::vector_Isometry3d& poses, - double resolution, double padding); - -protected: - bodies::BodyVector bodies_; - - bodies::BoundingSphere relative_bounding_sphere_; - std::vector sphere_radii_; - std::vector collision_spheres_; - EigenSTL::vector_Vector3d relative_collision_points_; -}; - -class PosedBodySphereDecomposition -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PosedBodySphereDecomposition(const BodyDecompositionConstPtr& body_decomposition); - - const std::vector& getCollisionSpheres() const - { - return body_decomposition_->getCollisionSpheres(); - } - - const EigenSTL::vector_Vector3d& getSphereCenters() const - { - return sphere_centers_; - } - - const EigenSTL::vector_Vector3d& getCollisionPoints() const - { - return posed_collision_points_; - } - - const std::vector& getSphereRadii() const - { - return body_decomposition_->getSphereRadii(); - } - const Eigen::Vector3d& getBoundingSphereCenter() const - { - return posed_bounding_sphere_center_; - } - - double getBoundingSphereRadius() const - { - return body_decomposition_->getRelativeBoundingSphere().radius; - } - - // assumed to be in reference frame, updates the pose of the body, - // the collision spheres, and the posed collision points - void updatePose(const Eigen::Isometry3d& linkTransform); - -protected: - BodyDecompositionConstPtr body_decomposition_; - Eigen::Vector3d posed_bounding_sphere_center_; - EigenSTL::vector_Vector3d posed_collision_points_; - EigenSTL::vector_Vector3d sphere_centers_; -}; - -class PosedBodyPointDecomposition -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition); - - PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition, const Eigen::Isometry3d& pose); - - PosedBodyPointDecomposition(const std::shared_ptr& octree); - - const EigenSTL::vector_Vector3d& getCollisionPoints() const - { - return posed_collision_points_; - } - // the collision spheres, and the posed collision points - void updatePose(const Eigen::Isometry3d& linkTransform); - -protected: - BodyDecompositionConstPtr body_decomposition_; - EigenSTL::vector_Vector3d posed_collision_points_; -}; - -class PosedBodySphereDecompositionVector -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PosedBodySphereDecompositionVector() - : logger_(moveit::getLogger("moveit.core.posed_body_sphere_decomposition_vector")) - { - } - - const std::vector& getCollisionSpheres() const - { - return collision_spheres_; - } - - const EigenSTL::vector_Vector3d& getSphereCenters() const - { - return posed_collision_spheres_; - } - - const std::vector& getSphereRadii() const - { - return sphere_radii_; - } - - void addToVector(PosedBodySphereDecompositionPtr& bd) - { - sphere_index_map_[decomp_vector_.size()] = collision_spheres_.size(); - decomp_vector_.push_back(bd); - collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(), - bd->getCollisionSpheres().end()); - posed_collision_spheres_.insert(posed_collision_spheres_.end(), bd->getSphereCenters().begin(), - bd->getSphereCenters().end()); - sphere_radii_.insert(sphere_radii_.end(), bd->getSphereRadii().begin(), bd->getSphereRadii().end()); - } - - unsigned int getSize() const - { - return decomp_vector_.size(); - } - - PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const - { - if (i >= decomp_vector_.size()) - { - RCLCPP_INFO(logger_, "No body decomposition"); - return empty_ptr_; - } - return decomp_vector_[i]; - } - - void updatePose(unsigned int ind, const Eigen::Isometry3d& pose) - { - if (ind >= decomp_vector_.size()) - { - RCLCPP_WARN(logger_, "Can't update pose"); - return; - } - decomp_vector_[ind]->updatePose(pose); - for (unsigned int i = 0; i < decomp_vector_[ind]->getSphereCenters().size(); ++i) - { - posed_collision_spheres_[sphere_index_map_[ind] + i] = decomp_vector_[ind]->getSphereCenters()[i]; - } - } - -private: - rclcpp::Logger logger_; - PosedBodySphereDecompositionConstPtr empty_ptr_; - std::vector decomp_vector_; - std::vector collision_spheres_; - EigenSTL::vector_Vector3d posed_collision_spheres_; - std::vector sphere_radii_; - std::map sphere_index_map_; -}; - -class PosedBodyPointDecompositionVector -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("moveit.core.posed_body_point_decomposition_vector")) - { - } - - EigenSTL::vector_Vector3d getCollisionPoints() const - { - EigenSTL::vector_Vector3d ret_points; - for (const PosedBodyPointDecompositionPtr& decomp : decomp_vector_) - { - ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end()); - } - return ret_points; - } - - void addToVector(PosedBodyPointDecompositionPtr& bd) - { - decomp_vector_.push_back(bd); - } - - unsigned int getSize() const - { - return decomp_vector_.size(); - } - - PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const - { - if (i >= decomp_vector_.size()) - { - RCLCPP_INFO(logger_, "No body decomposition"); - return empty_ptr_; - } - return decomp_vector_[i]; - } - - void updatePose(unsigned int ind, const Eigen::Isometry3d& pose) - { - if (ind < decomp_vector_.size()) - { - decomp_vector_[ind]->updatePose(pose); - } - else - { - RCLCPP_WARN(logger_, "Can't update pose"); - return; - } - } - -protected: - rclcpp::Logger logger_; - -private: - PosedBodyPointDecompositionPtr empty_ptr_; - std::vector decomp_vector_; -}; - -struct ProximityInfo -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - std::string link_name; - std::string attached_object_name; - double proximity; - unsigned int sphere_index; - unsigned int att_index; - Eigen::Vector3d closest_point; - Eigen::Vector3d closest_gradient; -}; - -bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1, - const PosedBodySphereDecompositionConstPtr& p2); - -void getCollisionSphereMarkers(const std_msgs::msg::ColorRGBA& color, const std::string& frame_id, - const std::string& ns, const rclcpp::Duration& dur, - const std::vector& posed_decompositions, - visualization_msgs::msg::MarkerArray& arr); - -void getProximityGradientMarkers(const std::string& frame_id, const std::string& ns, const rclcpp::Duration& dur, - const std::vector& posed_decompositions, - const std::vector& posed_vector_decompositions, - const std::vector& gradients, visualization_msgs::msg::MarkerArray& arr); - -void getCollisionMarkers(const std::string& frame_id, const std::string& ns, const rclcpp::Duration& dur, - const std::vector& posed_decompositions, - const std::vector& posed_vector_decompositions, - const std::vector& gradients, visualization_msgs::msg::MarkerArray& arr); -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.hpp b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.hpp new file mode 100644 index 0000000000..f4e43fb40b --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.hpp @@ -0,0 +1,536 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, 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 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: E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +enum CollisionType +{ + NONE = 0, + SELF = 1, + INTRA = 2, + ENVIRONMENT = 3, +}; + +struct CollisionSphere +{ + CollisionSphere(const Eigen::Vector3d& rel, double radius) + { + relative_vec_ = rel; + radius_ = radius; + } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Eigen::Vector3d relative_vec_; + double radius_; +}; + +struct GradientInfo +{ + GradientInfo() : closest_distance(DBL_MAX), collision(false) + { + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + double closest_distance; + bool collision; + EigenSTL::vector_Vector3d sphere_locations; + std::vector distances; + EigenSTL::vector_Vector3d gradients; + std::vector types; + std::vector sphere_radii; + std::string joint_name; + + void clear() + { + closest_distance = DBL_MAX; + collision = false; + sphere_locations.clear(); + distances.clear(); + gradients.clear(); + sphere_radii.clear(); + joint_name.clear(); + } +}; + +// Defines ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(PosedDistanceField); +MOVEIT_CLASS_FORWARD(BodyDecomposition); +MOVEIT_CLASS_FORWARD(PosedBodySphereDecomposition); +MOVEIT_CLASS_FORWARD(PosedBodyPointDecomposition); +MOVEIT_CLASS_FORWARD(PosedBodySphereDecompositionVector); +MOVEIT_CLASS_FORWARD(PosedBodyPointDecompositionVector); + +class PosedDistanceField : public distance_field::PropagationDistanceField +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PosedDistanceField(const Eigen::Vector3d& size, const Eigen::Vector3d& origin, double resolution, double max_distance, + bool propagate_negative_distances = false) + : distance_field::PropagationDistanceField(size.x(), size.y(), size.z(), resolution, origin.x(), origin.y(), + origin.z(), max_distance, propagate_negative_distances) + , pose_(Eigen::Isometry3d::Identity()) + { + } + + void updatePose(const Eigen::Isometry3d& transform) + { + pose_ = transform; + } + + const Eigen::Isometry3d& getPose() const + { + return pose_; + } + + /** + * @brief Gets not only the distance to the nearest cell but the gradient + * direction. The point + * defined by the x, y and z values is transform into the local distance field + * coordinate system using + * the current pose. + * The gradient is computed as a function of the distances of near-by cells. + * An uninitialized + * distance is returned if the cell is not valid for gradient production + * purposes. + * @param x input x value of the query point + * @param y input y value of the query point + * @param z input z value of the query point + * @param gradient_x output argument with the x value of the gradient + * @param gradient_y output argument with the y value of the gradient + * @param gradient_z output argument with the z value of the gradient + * @param in_bounds true if the point is within the bounds of the distance + * field, false otherwise + */ + double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z, + bool& in_bounds) const + { + // Transpose of a rotation matrix equals its inverse, but computationally cheaper + Eigen::Vector3d rel_pos = pose_.linear().transpose() * Eigen::Vector3d(x, y, z); + double gx, gy, gz; + double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(), + gx, gy, gz, in_bounds); + Eigen::Vector3d grad = pose_ * Eigen::Vector3d(gx, gy, gz); + gradient_x = grad.x(); + gradient_y = grad.y(); + gradient_z = grad.z(); + return res; + } + + /* + * @brief determines a set of gradients of the given collision spheres in the + * distance field + * @param sphere_list vector of the spheres that approximate a links geometry + * @param sphere_centers vector of points which indicate the center of each + * sphere in sphere_list + * @param gradient output argument to be populated with the resulting gradient + * calculation + * @param tolerance + * @param subtract_radii distance to the sphere centers will be computed by + * subtracting the sphere radius from the nearest point + * @param maximum_value + * @param stop_at_first_collision when true the computation is terminated when + * the first collision is found + */ + bool getCollisionSphereGradients(const std::vector& sphere_list, + const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient, + const CollisionType& type, double tolerance, bool subtract_radii, + double maximum_value, bool stop_at_first_collision); + +protected: + Eigen::Isometry3d pose_; +}; + +// determines set of collision spheres given a posed body; this is BAD! +// Allocation errors will happen; change this function so it does not return +// that vector by value +std::vector determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relativeTransform); + +// determines a set of gradients of the given collision spheres in the distance +// field +bool getCollisionSphereGradients(const distance_field::DistanceField* distance_field, + const std::vector& sphere_list, + const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient, + const CollisionType& type, double tolerance, bool subtract_radii, double maximum_value, + bool stop_at_first_collision); + +bool getCollisionSphereCollision(const distance_field::DistanceField* distance_field, + const std::vector& sphere_list, + const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value, + double tolerance); + +bool getCollisionSphereCollision(const distance_field::DistanceField* distance_field, + const std::vector& sphere_list, + const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value, + double tolerance, unsigned int num_coll, std::vector& colls); + +// forward declaration required for friending apparently +class BodyDecompositionVector; + +class BodyDecomposition +{ + friend class BodyDecompositionVector; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + BodyDecomposition(const shapes::ShapeConstPtr& shape, double resolution, double padding = 0.01); + + BodyDecomposition(const std::vector& shapes, const EigenSTL::vector_Isometry3d& poses, + double resolution, double padding); + + ~BodyDecomposition(); + + Eigen::Isometry3d relative_cylinder_pose_; + + void replaceCollisionSpheres(const std::vector& new_collision_spheres, + const Eigen::Isometry3d& new_relative_cylinder_pose) + { + // std::cerr << "Replacing " << collision_spheres_.size() << " with " << + // new_collision_spheres.size() << '\n'; + collision_spheres_ = new_collision_spheres; + relative_cylinder_pose_ = new_relative_cylinder_pose; + } + + const std::vector& getCollisionSpheres() const + { + return collision_spheres_; + } + + const std::vector& getSphereRadii() const + { + return sphere_radii_; + } + + const EigenSTL::vector_Vector3d& getCollisionPoints() const + { + return relative_collision_points_; + } + + const bodies::Body* getBody(unsigned int i) const + { + return bodies_.getBody(i); + } + + unsigned int getBodiesCount() + { + return bodies_.getCount(); + } + + Eigen::Isometry3d getRelativeCylinderPose() const + { + return relative_cylinder_pose_; + } + + const bodies::BoundingSphere& getRelativeBoundingSphere() const + { + return relative_bounding_sphere_; + } + +protected: + void init(const std::vector& shapes, const EigenSTL::vector_Isometry3d& poses, + double resolution, double padding); + +protected: + bodies::BodyVector bodies_; + + bodies::BoundingSphere relative_bounding_sphere_; + std::vector sphere_radii_; + std::vector collision_spheres_; + EigenSTL::vector_Vector3d relative_collision_points_; +}; + +class PosedBodySphereDecomposition +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PosedBodySphereDecomposition(const BodyDecompositionConstPtr& body_decomposition); + + const std::vector& getCollisionSpheres() const + { + return body_decomposition_->getCollisionSpheres(); + } + + const EigenSTL::vector_Vector3d& getSphereCenters() const + { + return sphere_centers_; + } + + const EigenSTL::vector_Vector3d& getCollisionPoints() const + { + return posed_collision_points_; + } + + const std::vector& getSphereRadii() const + { + return body_decomposition_->getSphereRadii(); + } + const Eigen::Vector3d& getBoundingSphereCenter() const + { + return posed_bounding_sphere_center_; + } + + double getBoundingSphereRadius() const + { + return body_decomposition_->getRelativeBoundingSphere().radius; + } + + // assumed to be in reference frame, updates the pose of the body, + // the collision spheres, and the posed collision points + void updatePose(const Eigen::Isometry3d& linkTransform); + +protected: + BodyDecompositionConstPtr body_decomposition_; + Eigen::Vector3d posed_bounding_sphere_center_; + EigenSTL::vector_Vector3d posed_collision_points_; + EigenSTL::vector_Vector3d sphere_centers_; +}; + +class PosedBodyPointDecomposition +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition); + + PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition, const Eigen::Isometry3d& pose); + + PosedBodyPointDecomposition(const std::shared_ptr& octree); + + const EigenSTL::vector_Vector3d& getCollisionPoints() const + { + return posed_collision_points_; + } + // the collision spheres, and the posed collision points + void updatePose(const Eigen::Isometry3d& linkTransform); + +protected: + BodyDecompositionConstPtr body_decomposition_; + EigenSTL::vector_Vector3d posed_collision_points_; +}; + +class PosedBodySphereDecompositionVector +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PosedBodySphereDecompositionVector() + : logger_(moveit::getLogger("moveit.core.posed_body_sphere_decomposition_vector")) + { + } + + const std::vector& getCollisionSpheres() const + { + return collision_spheres_; + } + + const EigenSTL::vector_Vector3d& getSphereCenters() const + { + return posed_collision_spheres_; + } + + const std::vector& getSphereRadii() const + { + return sphere_radii_; + } + + void addToVector(PosedBodySphereDecompositionPtr& bd) + { + sphere_index_map_[decomp_vector_.size()] = collision_spheres_.size(); + decomp_vector_.push_back(bd); + collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(), + bd->getCollisionSpheres().end()); + posed_collision_spheres_.insert(posed_collision_spheres_.end(), bd->getSphereCenters().begin(), + bd->getSphereCenters().end()); + sphere_radii_.insert(sphere_radii_.end(), bd->getSphereRadii().begin(), bd->getSphereRadii().end()); + } + + unsigned int getSize() const + { + return decomp_vector_.size(); + } + + PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const + { + if (i >= decomp_vector_.size()) + { + RCLCPP_INFO(logger_, "No body decomposition"); + return empty_ptr_; + } + return decomp_vector_[i]; + } + + void updatePose(unsigned int ind, const Eigen::Isometry3d& pose) + { + if (ind >= decomp_vector_.size()) + { + RCLCPP_WARN(logger_, "Can't update pose"); + return; + } + decomp_vector_[ind]->updatePose(pose); + for (unsigned int i = 0; i < decomp_vector_[ind]->getSphereCenters().size(); ++i) + { + posed_collision_spheres_[sphere_index_map_[ind] + i] = decomp_vector_[ind]->getSphereCenters()[i]; + } + } + +private: + rclcpp::Logger logger_; + PosedBodySphereDecompositionConstPtr empty_ptr_; + std::vector decomp_vector_; + std::vector collision_spheres_; + EigenSTL::vector_Vector3d posed_collision_spheres_; + std::vector sphere_radii_; + std::map sphere_index_map_; +}; + +class PosedBodyPointDecompositionVector +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("moveit.core.posed_body_point_decomposition_vector")) + { + } + + EigenSTL::vector_Vector3d getCollisionPoints() const + { + EigenSTL::vector_Vector3d ret_points; + for (const PosedBodyPointDecompositionPtr& decomp : decomp_vector_) + { + ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end()); + } + return ret_points; + } + + void addToVector(PosedBodyPointDecompositionPtr& bd) + { + decomp_vector_.push_back(bd); + } + + unsigned int getSize() const + { + return decomp_vector_.size(); + } + + PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const + { + if (i >= decomp_vector_.size()) + { + RCLCPP_INFO(logger_, "No body decomposition"); + return empty_ptr_; + } + return decomp_vector_[i]; + } + + void updatePose(unsigned int ind, const Eigen::Isometry3d& pose) + { + if (ind < decomp_vector_.size()) + { + decomp_vector_[ind]->updatePose(pose); + } + else + { + RCLCPP_WARN(logger_, "Can't update pose"); + return; + } + } + +protected: + rclcpp::Logger logger_; + +private: + PosedBodyPointDecompositionPtr empty_ptr_; + std::vector decomp_vector_; +}; + +struct ProximityInfo +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::string link_name; + std::string attached_object_name; + double proximity; + unsigned int sphere_index; + unsigned int att_index; + Eigen::Vector3d closest_point; + Eigen::Vector3d closest_gradient; +}; + +bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1, + const PosedBodySphereDecompositionConstPtr& p2); + +void getCollisionSphereMarkers(const std_msgs::msg::ColorRGBA& color, const std::string& frame_id, + const std::string& ns, const rclcpp::Duration& dur, + const std::vector& posed_decompositions, + visualization_msgs::msg::MarkerArray& arr); + +void getProximityGradientMarkers(const std::string& frame_id, const std::string& ns, const rclcpp::Duration& dur, + const std::vector& posed_decompositions, + const std::vector& posed_vector_decompositions, + const std::vector& gradients, visualization_msgs::msg::MarkerArray& arr); + +void getCollisionMarkers(const std::string& frame_id, const std::string& ns, const rclcpp::Duration& dur, + const std::vector& posed_decompositions, + const std::vector& posed_vector_decompositions, + const std::vector& gradients, visualization_msgs::msg::MarkerArray& arr); +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h index f971f5a442..55e15faa04 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,276 +47,5 @@ /* Author: E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace collision_detection -{ -static const double DEFAULT_SIZE_X = 3.0; -static const double DEFAULT_SIZE_Y = 3.0; -static const double DEFAULT_SIZE_Z = 4.0; -static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD = false; -static const double DEFAULT_RESOLUTION = .02; -static const double DEFAULT_COLLISION_TOLERANCE = 0.0; -static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25; - -MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); // Defines CollisionEnvDistanceFieldPtr, ConstPtr, WeakPtr... etc - -class CollisionEnvDistanceField : public CollisionEnv -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions = - std::map>(), - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, - double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, - const std::map>& link_body_decompositions = - std::map>(), - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, - double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, - double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world); - - void initialize(const std::map>& link_body_decompositions, - const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field, - double resolution, double collision_tolerance, double max_propogation_distance); - - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state) const override; - - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const override; - - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void createCollisionModelMarker(const moveit::core::RobotState& state, - visualization_msgs::msg::MarkerArray& model_markers) const; - - virtual double distanceSelf(const moveit::core::RobotState& /* state */) const - { - return 0.0; - } - - virtual double distanceSelf(const moveit::core::RobotState& /* state */, - const collision_detection::AllowedCollisionMatrix& /* acm */) const - { - return 0.0; - } - - void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */, - const moveit::core::RobotState& /* state */) const override - { - RCLCPP_ERROR(logger_, "Not implemented"); - } - - DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const - { - return distance_field_cache_entry_; - } - - // void getSelfCollisionsGradients(const collision_detection::CollisionRequest - // &req, - // collision_detection::CollisionResult &res, - // const moveit::core::RobotState &state, - // const - // collision_detection::AllowedCollisionMatrix - // &acm) const; - - MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntryWorld); - struct DistanceFieldCacheEntryWorld - { - std::map> posed_body_point_decompositions_; - distance_field::DistanceFieldPtr distance_field_; - }; - - ~CollisionEnvDistanceField() override; - - void checkCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const; - - void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const override; - - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix& acm) const override; - - virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; - - void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, - const moveit::core::RobotState& state2) const override; - - virtual double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const - { - (void)state; - (void)verbose; - return 0.0; - } - - virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, - bool verbose = false) const - { - (void)state; - (void)acm; - (void)verbose; - return 0.0; - } - - void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */, - const moveit::core::RobotState& /* state */) const override - { - RCLCPP_ERROR(logger_, "Not implemented"); - } - - void setWorld(const WorldPtr& world) override; - - distance_field::DistanceFieldConstPtr getDistanceField() const - { - return distance_field_cache_entry_->distance_field_; - } - - collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const - { - return last_gsr_; - } - - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; - - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; - -protected: - bool getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const; - - bool getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const; - - bool getSelfCollisions(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - GroupStateRepresentationPtr& gsr) const; - - bool getIntraGroupCollisions(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, GroupStateRepresentationPtr& gsr) const; - - void checkSelfCollisionHelper(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr) const; - - void updateGroupStateRepresentationState(const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const; - - void generateCollisionCheckingStructures(const std::string& group_name, const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm, - GroupStateRepresentationPtr& gsr, bool generate_distance_field) const; - - DistanceFieldCacheEntryConstPtr - getDistanceFieldCacheEntry(const std::string& group_name, const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm) const; - - DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string& group_name, - const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix* acm, - bool generate_distance_field) const; - - void addLinkBodyDecompositions(double resolution); - - void addLinkBodyDecompositions(double resolution, - const std::map>& link_body_decompositions); - - PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls, - unsigned int ind) const; - - PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const; - - void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const; - - bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce, - const moveit::core::RobotState& state) const; - - bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr& dfce, - const collision_detection::AllowedCollisionMatrix& acm) const; - - void updatedPaddingOrScaling(const std::vector& /*links*/) override{}; - - DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld(); - - void updateDistanceObject(const std::string& id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr& dfce, - EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points); - - bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, - const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const; - - bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field, - GroupStateRepresentationPtr& gsr) const; - - void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); - - rclcpp::Logger logger_; - - Eigen::Vector3d size_; - Eigen::Vector3d origin_; - bool use_signed_distance_field_; - double resolution_; - double collision_tolerance_; - double max_propogation_distance_; - - std::vector link_body_decomposition_vector_; - std::map link_body_decomposition_index_map_; - - mutable std::mutex update_cache_lock_; - DistanceFieldCacheEntryPtr distance_field_cache_entry_; - std::map> in_group_update_map_; - std::map pregenerated_group_state_representation_map_; - - planning_scene::PlanningScenePtr planning_scene_; - - mutable std::mutex update_cache_lock_world_; - DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_; - GroupStateRepresentationPtr last_gsr_; - World::ObserverHandle observer_handle_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.hpp b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.hpp new file mode 100644 index 0000000000..7a9c53224c --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.hpp @@ -0,0 +1,310 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 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: E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace collision_detection +{ +static const double DEFAULT_SIZE_X = 3.0; +static const double DEFAULT_SIZE_Y = 3.0; +static const double DEFAULT_SIZE_Z = 4.0; +static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD = false; +static const double DEFAULT_RESOLUTION = .02; +static const double DEFAULT_COLLISION_TOLERANCE = 0.0; +static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25; + +MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); // Defines CollisionEnvDistanceFieldPtr, ConstPtr, WeakPtr... etc + +class CollisionEnvDistanceField : public CollisionEnv +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, + double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, + double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, + double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, + double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world); + + void initialize(const std::map>& link_body_decompositions, + const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field, + double resolution, double collision_tolerance, double max_propogation_distance); + + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& state) const override; + + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const override; + + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void createCollisionModelMarker(const moveit::core::RobotState& state, + visualization_msgs::msg::MarkerArray& model_markers) const; + + virtual double distanceSelf(const moveit::core::RobotState& /* state */) const + { + return 0.0; + } + + virtual double distanceSelf(const moveit::core::RobotState& /* state */, + const collision_detection::AllowedCollisionMatrix& /* acm */) const + { + return 0.0; + } + + void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */, + const moveit::core::RobotState& /* state */) const override + { + RCLCPP_ERROR(logger_, "Not implemented"); + } + + DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const + { + return distance_field_cache_entry_; + } + + // void getSelfCollisionsGradients(const collision_detection::CollisionRequest + // &req, + // collision_detection::CollisionResult &res, + // const moveit::core::RobotState &state, + // const + // collision_detection::AllowedCollisionMatrix + // &acm) const; + + MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntryWorld); + struct DistanceFieldCacheEntryWorld + { + std::map> posed_body_point_decompositions_; + distance_field::DistanceFieldPtr distance_field_; + }; + + ~CollisionEnvDistanceField() override; + + void checkCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix& acm) const override; + + virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override; + + void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1, + const moveit::core::RobotState& state2) const override; + + virtual double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const + { + (void)state; + (void)verbose; + return 0.0; + } + + virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + bool verbose = false) const + { + (void)state; + (void)acm; + (void)verbose; + return 0.0; + } + + void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */, + const moveit::core::RobotState& /* state */) const override + { + RCLCPP_ERROR(logger_, "Not implemented"); + } + + void setWorld(const WorldPtr& world) override; + + distance_field::DistanceFieldConstPtr getDistanceField() const + { + return distance_field_cache_entry_->distance_field_; + } + + collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const + { + return last_gsr_; + } + + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + +protected: + bool getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const; + + bool getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const; + + bool getSelfCollisions(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + GroupStateRepresentationPtr& gsr) const; + + bool getIntraGroupCollisions(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, GroupStateRepresentationPtr& gsr) const; + + void checkSelfCollisionHelper(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr) const; + + void updateGroupStateRepresentationState(const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void generateCollisionCheckingStructures(const std::string& group_name, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm, + GroupStateRepresentationPtr& gsr, bool generate_distance_field) const; + + DistanceFieldCacheEntryConstPtr + getDistanceFieldCacheEntry(const std::string& group_name, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm) const; + + DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string& group_name, + const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix* acm, + bool generate_distance_field) const; + + void addLinkBodyDecompositions(double resolution); + + void addLinkBodyDecompositions(double resolution, + const std::map>& link_body_decompositions); + + PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls, + unsigned int ind) const; + + PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const; + + void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce, + const moveit::core::RobotState& state) const; + + bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr& dfce, + const collision_detection::AllowedCollisionMatrix& acm) const; + + void updatedPaddingOrScaling(const std::vector& /*links*/) override{}; + + DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld(); + + void updateDistanceObject(const std::string& id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr& dfce, + EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points); + + bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res, + const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const; + + bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field, + GroupStateRepresentationPtr& gsr) const; + + void notifyObjectChange(const ObjectConstPtr& obj, World::Action action); + + rclcpp::Logger logger_; + + Eigen::Vector3d size_; + Eigen::Vector3d origin_; + bool use_signed_distance_field_; + double resolution_; + double collision_tolerance_; + double max_propogation_distance_; + + std::vector link_body_decomposition_vector_; + std::map link_body_decomposition_index_map_; + + mutable std::mutex update_cache_lock_; + DistanceFieldCacheEntryPtr distance_field_cache_entry_; + std::map> in_group_update_map_; + std::map pregenerated_group_state_representation_map_; + + planning_scene::PlanningScenePtr planning_scene_; + + mutable std::mutex update_cache_lock_world_; + DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_; + GroupStateRepresentationPtr last_gsr_; + World::ObserverHandle observer_handle_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h index 6e8dd80fd3..2374412165 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,117 +47,5 @@ /* Author: E. Gil Jones, Jens Petit */ #pragma once - -#include -#include -#include -#include - -namespace collision_detection -{ -/** \brief This hybrid collision environment combines FCL and a distance field. Both can be used to calculate - * collisions. */ -class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, - const std::map>& link_body_decompositions = - std::map>(), - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, - const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, - const std::map>& link_body_decompositions = - std::map>(), - double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, - const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), - bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, - double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, - double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, - double scale = 1.0); - - CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world); - - ~CollisionEnvHybrid() override - { - } - - void initializeRobotDistanceField(const std::map>& link_body_decompositions, - double size_x, double size_y, double size_z, bool use_signed_distance_field, - double resolution, double collision_tolerance, double max_propogation_distance) - { - cenv_distance_->initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), - Eigen::Vector3d(0, 0, 0), use_signed_distance_field, resolution, collision_tolerance, - max_propogation_distance); - } - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, - const moveit::core::RobotState& state) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const moveit::core::RobotState& state, - GroupStateRepresentationPtr& gsr) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res, const moveit::core::RobotState& state, - const collision_detection::AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - const CollisionEnvDistanceFieldConstPtr getCollisionRobotDistanceField() const - { - return cenv_distance_; - } - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; - - void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; - - void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, - const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, - GroupStateRepresentationPtr& gsr) const; - - void setWorld(const WorldPtr& world) override; - - void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; - - void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, - const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; - - const CollisionEnvDistanceFieldConstPtr getCollisionWorldDistanceField() const - { - return cenv_distance_; - } - -protected: - CollisionEnvDistanceFieldPtr cenv_distance_; -}; -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.hpp b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.hpp new file mode 100644 index 0000000000..ef799520a5 --- /dev/null +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.hpp @@ -0,0 +1,151 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 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: E. Gil Jones, Jens Petit */ + +#pragma once + +#include +#include +#include +#include + +namespace collision_detection +{ +/** \brief This hybrid collision environment combines FCL and a distance field. Both can be used to calculate + * collisions. */ +class CollisionEnvHybrid : public collision_detection::CollisionEnvFCL +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, + const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvHybrid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world, + const std::map>& link_body_decompositions = + std::map>(), + double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y, double size_z = DEFAULT_SIZE_Z, + const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0), + bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, + double resolution = DEFAULT_RESOLUTION, double collision_tolerance = DEFAULT_COLLISION_TOLERANCE, + double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0, + double scale = 1.0); + + CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world); + + ~CollisionEnvHybrid() override + { + } + + void initializeRobotDistanceField(const std::map>& link_body_decompositions, + double size_x, double size_y, double size_z, bool use_signed_distance_field, + double resolution, double collision_tolerance, double max_propogation_distance) + { + cenv_distance_->initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), + Eigen::Vector3d(0, 0, 0), use_signed_distance_field, resolution, collision_tolerance, + max_propogation_distance); + } + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, + const moveit::core::RobotState& state) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + GroupStateRepresentationPtr& gsr) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + void checkSelfCollisionDistanceField(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res, const moveit::core::RobotState& state, + const collision_detection::AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + const CollisionEnvDistanceFieldConstPtr getCollisionRobotDistanceField() const + { + return cenv_distance_; + } + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; + + void checkCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const; + + void checkRobotCollisionDistanceField(const CollisionRequest& req, CollisionResult& res, + const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm, + GroupStateRepresentationPtr& gsr) const; + + void setWorld(const WorldPtr& world) override; + + void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state, + const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const; + + const CollisionEnvDistanceFieldConstPtr getCollisionWorldDistanceField() const + { + return cenv_distance_; + } + +protected: + CollisionEnvDistanceFieldPtr cenv_distance_; +}; +} // namespace collision_detection diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp index 4402a4e44c..dae65836ac 100644 --- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp @@ -34,7 +34,7 @@ /* Author: E. Gil Jones */ -#include +#include #include #include #include diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 1754a74b4d..7b0df3a7ce 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -34,10 +34,10 @@ /* Author: E. Gil Jones */ -#include +#include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index 947ce31263..49e6517a79 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -34,17 +34,17 @@ /* Author: E. Gil Jones */ -#include +#include #include #include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp index 7f1686022b..b0afbd55fd 100644 --- a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp @@ -34,8 +34,8 @@ /* Author: E. Gil Jones, Jens Petit */ -#include -#include +#include +#include namespace collision_detection { diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp index de7af9f693..f2c69e9c01 100644 --- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp +++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp @@ -34,12 +34,12 @@ /** \author E. Gil Jones */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index 0552766463..629d68c425 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,235 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -/** - * \brief The constraint samplers namespace contains a number of - * methods for generating samples based on a constraint or set of - * constraints. - * - * It intended for use by any algorithm that requires a - * constraint-aware sampling strategy. - */ -namespace constraint_samplers -{ -MOVEIT_CLASS_FORWARD(ConstraintSampler); // Defines ConstraintSamplerPtr, ConstPtr, WeakPtr... etc - -/** - * \brief ConstraintSampler is an abstract base class that allows the - * sampling of a kinematic state for a particular group of a robot. - */ -class ConstraintSampler -{ -public: - /** \brief The default value associated with a sampling request. By default if a valid sample cannot be - produced in this many attempts, it returns with no sample */ - static const unsigned int DEFAULT_MAX_SAMPLING_ATTEMPTS = 2; - - /** - * \brief Constructor - * - * @param [in] scene The planning scene that will be used for constraint checking - * @param [in] group_name The group name associated with the - * constraint. Will be invalid if no group name is passed in or the - * joint model group cannot be found in the kinematic model - * - */ - ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name); - - virtual ~ConstraintSampler() - { - } - - /** - * \brief Function for configuring a constraint sampler given a Constraints message. - * - * @param [in] constr The constraints from which to construct a sampler - * - * @return True if the configuration is successful. If true, \ref isValid should also true. - * If false, \ref isValid should return false - */ - virtual bool configure(const moveit_msgs::msg::Constraints& constr) = 0; - - /** - * \brief Gets the group name set in the constructor - * - * @return The group name - */ - const std::string& getGroupName() const - { - return getJointModelGroup()->getName(); - } - - /** - * \brief Gets the joint model group - * - * - * @return The joint model group - */ - const moveit::core::JointModelGroup* getJointModelGroup() const - { - return jmg_; - } - - /** - * \brief Gets the planning scene - * - * - * @return The planning scene as a const ptr - */ - const planning_scene::PlanningSceneConstPtr& getPlanningScene() const - { - return scene_; - } - - /** - * \brief Return the names of the mobile frames whose pose is needed when sample() is called. - * - * Mobile frames mean frames other than the reference frame of the - * kinematic model. These frames may move when the kinematic state - * changes. Frame dependency can help determine an ordering from a - * set of constraint samplers - for more information see the derived - * class documentation for \ref UnionConstraintSampler. - * - * @return The list of names whose pose is needed - */ - const std::vector& getFrameDependency() const - { - return frame_depends_; - } - - /** - * \brief Gets the callback used to determine state validity during sampling. The sampler will attempt - * to satisfy this constraint if possible, but there is no guarantee. - */ - const moveit::core::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const - { - return group_state_validity_callback_; - } - - /** - * \brief Sets the callback used to determine the state validity during sampling. The sampler will attempt to satisfy - * this constraint if possible, but there is no guarantee. - * - * @param callback The callback to set - */ - void setGroupStateValidityCallback(const moveit::core::GroupStateValidityCallbackFn& callback) - { - group_state_validity_callback_ = callback; - } - - /** - * \brief Samples given the constraints, populating \e state. - * The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in - * as the maximum number of attempts to make to take a sample. - * - * @param state The state into which the values will be placed. Only values for the group are written. - * The same state is used as reference if needed. - * - * @return True if a sample was successfully taken, false otherwise - */ - bool sample(moveit::core::RobotState& state) - { - return sample(state, state, DEFAULT_MAX_SAMPLING_ATTEMPTS); - } - - /** - * \brief Samples given the constraints, populating \e state. - * This function allows the parameter max_attempts to be set. - * - * @param state The state into which the values will be placed. Only values for the group are written. - * The same state is used as reference if needed. - * @param [in] max_attempts The maximum number of times to attempt to draw a sample. If no sample has been drawn - * in this number of attempts, false will be returned. - * - * @return True if a sample was successfully taken, false otherwise - */ - bool sample(moveit::core::RobotState& state, unsigned int max_attempts) - { - return sample(state, state, max_attempts); - } - - /** - * \brief Samples given the constraints, populating \e state. - * The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in - * as the maximum number of attempts to make to take a sample. - * - * @param [out] state The state into which the values will be placed. Only values for the group are written. - * @param [in] reference_state Reference state that will be used to do transforms or perform other actions - * - * @return True if a sample was successfully taken, false otherwise - */ - bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state) - { - return sample(state, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS); - } - - /** - * \brief Samples given the constraints, populating \e state. - * This function allows the parameter max_attempts to be set. - * - * @param [out] state The state into which the values will be placed. Only values for the group are written. - * @param [in] reference_state Reference state that will be used to do transforms or perform other actions - * @param [in] max_attempts The maximum number of times to attempt to draw a sample. If no sample has been drawn in - * this number of attempts, false will be returned. - * - * @return True if a sample was successfully taken, false otherwise - */ - virtual bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, - unsigned int max_attempts) = 0; - - /** - * \brief Returns whether or not the constraint sampler is valid or not. - * To be valid, the joint model group must be available in the kinematic model and configure() must have successfully - * been called - * - * @return True if the sampler is valid, and otherwise false. - */ - bool isValid() const - { - return is_valid_; - } - - /** \brief Check if the sampler is set to verbose mode */ - bool getVerbose() const - { - return verbose_; - } - - /** \brief Enable/disable verbose mode for sampler */ - virtual void setVerbose(bool verbose) - { - verbose_ = verbose; - } - - /** - * \brief Get the name of the constraint sampler, for debugging purposes - * should be in CamelCase format. - * \return string of name - */ - virtual const std::string& getName() const = 0; - -protected: - /** - * \brief Clears all data from the constraint. - * - */ - virtual void clear(); - - bool is_valid_; ///< Holds the value for validity - - /// Holds the planning scene - planning_scene::PlanningSceneConstPtr scene_; - /// Holds the joint model group associated with this constraint - const moveit::core::JointModelGroup* const jmg_; - /// Holds the set of frames that must exist in the reference state to allow samples to be drawn - std::vector frame_depends_; - /// Holds the callback for state validity - moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_; - bool verbose_; ///< True if verbosity is on -}; -} // namespace constraint_samplers +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.hpp b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.hpp new file mode 100644 index 0000000000..32f58b1f3e --- /dev/null +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.hpp @@ -0,0 +1,269 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +/** + * \brief The constraint samplers namespace contains a number of + * methods for generating samples based on a constraint or set of + * constraints. + * + * It intended for use by any algorithm that requires a + * constraint-aware sampling strategy. + */ +namespace constraint_samplers +{ +MOVEIT_CLASS_FORWARD(ConstraintSampler); // Defines ConstraintSamplerPtr, ConstPtr, WeakPtr... etc + +/** + * \brief ConstraintSampler is an abstract base class that allows the + * sampling of a kinematic state for a particular group of a robot. + */ +class ConstraintSampler +{ +public: + /** \brief The default value associated with a sampling request. By default if a valid sample cannot be + produced in this many attempts, it returns with no sample */ + static const unsigned int DEFAULT_MAX_SAMPLING_ATTEMPTS = 2; + + /** + * \brief Constructor + * + * @param [in] scene The planning scene that will be used for constraint checking + * @param [in] group_name The group name associated with the + * constraint. Will be invalid if no group name is passed in or the + * joint model group cannot be found in the kinematic model + * + */ + ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name); + + virtual ~ConstraintSampler() + { + } + + /** + * \brief Function for configuring a constraint sampler given a Constraints message. + * + * @param [in] constr The constraints from which to construct a sampler + * + * @return True if the configuration is successful. If true, \ref isValid should also true. + * If false, \ref isValid should return false + */ + virtual bool configure(const moveit_msgs::msg::Constraints& constr) = 0; + + /** + * \brief Gets the group name set in the constructor + * + * @return The group name + */ + const std::string& getGroupName() const + { + return getJointModelGroup()->getName(); + } + + /** + * \brief Gets the joint model group + * + * + * @return The joint model group + */ + const moveit::core::JointModelGroup* getJointModelGroup() const + { + return jmg_; + } + + /** + * \brief Gets the planning scene + * + * + * @return The planning scene as a const ptr + */ + const planning_scene::PlanningSceneConstPtr& getPlanningScene() const + { + return scene_; + } + + /** + * \brief Return the names of the mobile frames whose pose is needed when sample() is called. + * + * Mobile frames mean frames other than the reference frame of the + * kinematic model. These frames may move when the kinematic state + * changes. Frame dependency can help determine an ordering from a + * set of constraint samplers - for more information see the derived + * class documentation for \ref UnionConstraintSampler. + * + * @return The list of names whose pose is needed + */ + const std::vector& getFrameDependency() const + { + return frame_depends_; + } + + /** + * \brief Gets the callback used to determine state validity during sampling. The sampler will attempt + * to satisfy this constraint if possible, but there is no guarantee. + */ + const moveit::core::GroupStateValidityCallbackFn& getGroupStateValidityCallback() const + { + return group_state_validity_callback_; + } + + /** + * \brief Sets the callback used to determine the state validity during sampling. The sampler will attempt to satisfy + * this constraint if possible, but there is no guarantee. + * + * @param callback The callback to set + */ + void setGroupStateValidityCallback(const moveit::core::GroupStateValidityCallbackFn& callback) + { + group_state_validity_callback_ = callback; + } + + /** + * \brief Samples given the constraints, populating \e state. + * The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in + * as the maximum number of attempts to make to take a sample. + * + * @param state The state into which the values will be placed. Only values for the group are written. + * The same state is used as reference if needed. + * + * @return True if a sample was successfully taken, false otherwise + */ + bool sample(moveit::core::RobotState& state) + { + return sample(state, state, DEFAULT_MAX_SAMPLING_ATTEMPTS); + } + + /** + * \brief Samples given the constraints, populating \e state. + * This function allows the parameter max_attempts to be set. + * + * @param state The state into which the values will be placed. Only values for the group are written. + * The same state is used as reference if needed. + * @param [in] max_attempts The maximum number of times to attempt to draw a sample. If no sample has been drawn + * in this number of attempts, false will be returned. + * + * @return True if a sample was successfully taken, false otherwise + */ + bool sample(moveit::core::RobotState& state, unsigned int max_attempts) + { + return sample(state, state, max_attempts); + } + + /** + * \brief Samples given the constraints, populating \e state. + * The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in + * as the maximum number of attempts to make to take a sample. + * + * @param [out] state The state into which the values will be placed. Only values for the group are written. + * @param [in] reference_state Reference state that will be used to do transforms or perform other actions + * + * @return True if a sample was successfully taken, false otherwise + */ + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state) + { + return sample(state, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS); + } + + /** + * \brief Samples given the constraints, populating \e state. + * This function allows the parameter max_attempts to be set. + * + * @param [out] state The state into which the values will be placed. Only values for the group are written. + * @param [in] reference_state Reference state that will be used to do transforms or perform other actions + * @param [in] max_attempts The maximum number of times to attempt to draw a sample. If no sample has been drawn in + * this number of attempts, false will be returned. + * + * @return True if a sample was successfully taken, false otherwise + */ + virtual bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, + unsigned int max_attempts) = 0; + + /** + * \brief Returns whether or not the constraint sampler is valid or not. + * To be valid, the joint model group must be available in the kinematic model and configure() must have successfully + * been called + * + * @return True if the sampler is valid, and otherwise false. + */ + bool isValid() const + { + return is_valid_; + } + + /** \brief Check if the sampler is set to verbose mode */ + bool getVerbose() const + { + return verbose_; + } + + /** \brief Enable/disable verbose mode for sampler */ + virtual void setVerbose(bool verbose) + { + verbose_ = verbose; + } + + /** + * \brief Get the name of the constraint sampler, for debugging purposes + * should be in CamelCase format. + * \return string of name + */ + virtual const std::string& getName() const = 0; + +protected: + /** + * \brief Clears all data from the constraint. + * + */ + virtual void clear(); + + bool is_valid_; ///< Holds the value for validity + + /// Holds the planning scene + planning_scene::PlanningSceneConstPtr scene_; + /// Holds the joint model group associated with this constraint + const moveit::core::JointModelGroup* const jmg_; + /// Holds the set of frames that must exist in the reference state to allow samples to be drawn + std::vector frame_depends_; + /// Holds the callback for state validity + moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_; + bool verbose_; ///< True if verbosity is on +}; +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h index a861facf54..f85741bcd7 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,30 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace constraint_samplers -{ -MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator); // Defines ConstraintSamplerAllocatorPtr, ConstPtr, WeakPtr... etc - -class ConstraintSamplerAllocator -{ -public: - ConstraintSamplerAllocator() - { - } - - virtual ~ConstraintSamplerAllocator() - { - } - - virtual ConstraintSamplerPtr alloc(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - const moveit_msgs::msg::Constraints& constr) = 0; - - virtual bool canService(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - const moveit_msgs::msg::Constraints& constr) const = 0; -}; -} // namespace constraint_samplers +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.hpp b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.hpp new file mode 100644 index 0000000000..0be8a86075 --- /dev/null +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.hpp @@ -0,0 +1,64 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include + +namespace constraint_samplers +{ +MOVEIT_CLASS_FORWARD(ConstraintSamplerAllocator); // Defines ConstraintSamplerAllocatorPtr, ConstPtr, WeakPtr... etc + +class ConstraintSamplerAllocator +{ +public: + ConstraintSamplerAllocator() + { + } + + virtual ~ConstraintSamplerAllocator() + { + } + + virtual ConstraintSamplerPtr alloc(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + const moveit_msgs::msg::Constraints& constr) = 0; + + virtual bool canService(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + const moveit_msgs::msg::Constraints& constr) const = 0; +}; +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h index afd036ddcf..3666bbdd61 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,114 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -namespace constraint_samplers -{ -MOVEIT_CLASS_FORWARD(ConstraintSamplerManager); // Defines ConstraintSamplerManagerPtr, ConstPtr, WeakPtr... etc - -/** - * \brief This class assists in the generation of a ConstraintSampler for a - * particular group from a moveit_msgs::msg::Constraints. - * - * It contains logic that will generate either a - * JointConstraintSampler, an IKConstraintSampler, or a - * UnionConstraintSampler depending on the contents of the Constraints - * message and the group in question. - * - */ -class ConstraintSamplerManager -{ -public: - /** - * \brief Empty constructor - * - */ - ConstraintSamplerManager() - { - } - /** - * \brief Allows the user to specify an alternate ConstraintSamplerAllocation - * - * @param sa The constraint sampler allocator that will be used - */ - void registerSamplerAllocator(const ConstraintSamplerAllocatorPtr& sa) - { - sampler_alloc_.push_back(sa); - } - /** - * \brief Selects among the potential sampler allocators. - * - * This function will iterate through the constraint sampler - * allocators, trying to find one that can service the constraints. - * The first one that can service the request will be called. If no - * allocators can service the Constraints, or there are no - * allocators, the selectDefaultSampler will be called. - * - * @param scene The planning scene that will be passed into the constraint sampler - * @param group_name The group name for which to allocate the constraint sampler - * @param constr The constraints - * - * @return An allocated ConstraintSamplerPtr, - * or an empty pointer if none could be allocated - */ - ConstraintSamplerPtr selectSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - const moveit_msgs::msg::Constraints& constr) const; - - /** - * \brief Default logic to select a ConstraintSampler given a - * constraints message. - * - * This function will generate a sampler using the joint_constraint, - * position_constraint, and orientation_constraint vectors from the - * Constraints argument. The type of constraint sampler that is - * produced depends on which constraint vectors have been populated. - * The following rules are applied: - * - * - If every joint in the group indicated by group_name is - * constrained by a valid joint constraint in the joint_constraints - * vector, a JointConstraintSampler with all bounded joints in - * returned. - * - If not every joint is constrained, but no position and - * orientation constraints are specified, or no valid - * IKConstraintSampler can be created, then a JointConstraintSampler - * with some unbounded joints is returned. - * - If position and orientation constraints are present and there - * is an IKSolver for the group, the function will attempt to - * create an IKConstraintSampler. - * - If there are multiple valid position/constraint pairings, the - * one with the smallest volume will be kept. - * - If no full pose is available, the function will attempt to create a position-only IKConstraintSampler. - * - Finally, the function will attempt to create an orientation-only IKConstraintSampler. - * - If there is a valid IKConstraintSampler, then if no valid joint constraints are present then an - *IKConstraintSampler will be returned. - * - If there are joint constraints, a UnionConstraintSampler with both the JointConstraintSampler and the - *IKConstraintSampler will be returned. - * - If there is no direct IK solver for the group, or no valid IKConstraintSampler could be generated, and there are - *subgroup IKSolvers, the function will attempt to generate a sampler from the various subgroup solvers. - * - It will attempt to determine which constraints act on the IK link for the sub-group IK solvers, and attempts to - *create ConstraintSampler functions by recursively calling \ref selectDefaultSampler for the sub-group. - * - If any samplers are valid, it adds them to a vector of type \ref ConstraintSamplerPtr. - * - Once it has iterated through each sub-group, if any samplers are valid, they are returned in a - *UnionConstraintSampler, along with a JointConstraintSampler if one exists. - * @param scene The planning scene that will be used to create the ConstraintSampler - * @param group_name The group name for which to create a sampler - * @param constr The set of constraints for which to create a sampler - * - * @return A valid \ref ConstraintSamplerPtr if one could be allocated, and otherwise an empty \ref - *ConstraintSamplerPtr - */ - static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr& scene, - const std::string& group_name, - const moveit_msgs::msg::Constraints& constr); - -private: - std::vector - sampler_alloc_; /**< \brief Holds the constraint sampler allocators, which will be tested in order */ -}; -} // namespace constraint_samplers +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.hpp b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.hpp new file mode 100644 index 0000000000..428a681430 --- /dev/null +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.hpp @@ -0,0 +1,148 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace constraint_samplers +{ +MOVEIT_CLASS_FORWARD(ConstraintSamplerManager); // Defines ConstraintSamplerManagerPtr, ConstPtr, WeakPtr... etc + +/** + * \brief This class assists in the generation of a ConstraintSampler for a + * particular group from a moveit_msgs::msg::Constraints. + * + * It contains logic that will generate either a + * JointConstraintSampler, an IKConstraintSampler, or a + * UnionConstraintSampler depending on the contents of the Constraints + * message and the group in question. + * + */ +class ConstraintSamplerManager +{ +public: + /** + * \brief Empty constructor + * + */ + ConstraintSamplerManager() + { + } + /** + * \brief Allows the user to specify an alternate ConstraintSamplerAllocation + * + * @param sa The constraint sampler allocator that will be used + */ + void registerSamplerAllocator(const ConstraintSamplerAllocatorPtr& sa) + { + sampler_alloc_.push_back(sa); + } + /** + * \brief Selects among the potential sampler allocators. + * + * This function will iterate through the constraint sampler + * allocators, trying to find one that can service the constraints. + * The first one that can service the request will be called. If no + * allocators can service the Constraints, or there are no + * allocators, the selectDefaultSampler will be called. + * + * @param scene The planning scene that will be passed into the constraint sampler + * @param group_name The group name for which to allocate the constraint sampler + * @param constr The constraints + * + * @return An allocated ConstraintSamplerPtr, + * or an empty pointer if none could be allocated + */ + ConstraintSamplerPtr selectSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + const moveit_msgs::msg::Constraints& constr) const; + + /** + * \brief Default logic to select a ConstraintSampler given a + * constraints message. + * + * This function will generate a sampler using the joint_constraint, + * position_constraint, and orientation_constraint vectors from the + * Constraints argument. The type of constraint sampler that is + * produced depends on which constraint vectors have been populated. + * The following rules are applied: + * + * - If every joint in the group indicated by group_name is + * constrained by a valid joint constraint in the joint_constraints + * vector, a JointConstraintSampler with all bounded joints in + * returned. + * - If not every joint is constrained, but no position and + * orientation constraints are specified, or no valid + * IKConstraintSampler can be created, then a JointConstraintSampler + * with some unbounded joints is returned. + * - If position and orientation constraints are present and there + * is an IKSolver for the group, the function will attempt to + * create an IKConstraintSampler. + * - If there are multiple valid position/constraint pairings, the + * one with the smallest volume will be kept. + * - If no full pose is available, the function will attempt to create a position-only IKConstraintSampler. + * - Finally, the function will attempt to create an orientation-only IKConstraintSampler. + * - If there is a valid IKConstraintSampler, then if no valid joint constraints are present then an + *IKConstraintSampler will be returned. + * - If there are joint constraints, a UnionConstraintSampler with both the JointConstraintSampler and the + *IKConstraintSampler will be returned. + * - If there is no direct IK solver for the group, or no valid IKConstraintSampler could be generated, and there are + *subgroup IKSolvers, the function will attempt to generate a sampler from the various subgroup solvers. + * - It will attempt to determine which constraints act on the IK link for the sub-group IK solvers, and attempts to + *create ConstraintSampler functions by recursively calling \ref selectDefaultSampler for the sub-group. + * - If any samplers are valid, it adds them to a vector of type \ref ConstraintSamplerPtr. + * - Once it has iterated through each sub-group, if any samplers are valid, they are returned in a + *UnionConstraintSampler, along with a JointConstraintSampler if one exists. + * @param scene The planning scene that will be used to create the ConstraintSampler + * @param group_name The group name for which to create a sampler + * @param constr The set of constraints for which to create a sampler + * + * @return A valid \ref ConstraintSamplerPtr if one could be allocated, and otherwise an empty \ref + *ConstraintSamplerPtr + */ + static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr& scene, + const std::string& group_name, + const moveit_msgs::msg::Constraints& constr); + +private: + std::vector + sampler_alloc_; /**< \brief Holds the constraint sampler allocators, which will be tested in order */ +}; +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index 77577b1a93..6291fe8b9b 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,24 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace constraint_samplers -{ -void visualizeDistribution(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state, - const std::string& link_name, unsigned int sample_count, - visualization_msgs::msg::MarkerArray& markers); - -void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, - const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, - const std::string& link_name, unsigned int sample_count, - visualization_msgs::msg::MarkerArray& markers); - -double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state); - -double countSamplesPerSecond(const moveit_msgs::msg::Constraints& constr, - const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); -} // namespace constraint_samplers +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.hpp b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.hpp new file mode 100644 index 0000000000..33c2808d41 --- /dev/null +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.hpp @@ -0,0 +1,58 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include + +namespace constraint_samplers +{ +void visualizeDistribution(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state, + const std::string& link_name, unsigned int sample_count, + visualization_msgs::msg::MarkerArray& markers); + +void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, + const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, + const std::string& link_name, unsigned int sample_count, + visualization_msgs::msg::MarkerArray& markers); + +double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const moveit::core::RobotState& reference_state); + +double countSamplesPerSecond(const moveit_msgs::msg::Constraints& constr, + const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index d1f307a040..88360bb35a 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,530 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace constraint_samplers -{ -MOVEIT_CLASS_FORWARD(JointConstraintSampler); // Defines JointConstraintSamplerPtr, ConstPtr, WeakPtr... etc - -/** - * \brief JointConstraintSampler is a class that allows the sampling - * of joints in a particular group of the robot, subject to a set of individual joint constraints. - * - * The set of individual joint constraint reduce the allowable bounds - * used in the sampling. Unconstrained values will be sampled within - * their limits. - * - */ -class JointConstraintSampler : public ConstraintSampler -{ -public: - /** - * Constructor - * - * @param [in] scene The planning scene used to check the constraint - * - * @param [in] group_name The group name associated with the - * constraint. Will be invalid if no group name is passed in or the - * joint model group cannot be found in the kinematic model - * - */ - JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) - : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator()) - { - } - - /** - * Constructor - * - * @param [in] scene The planning scene used to check the constraint - * - * @param [in] group_name The group name associated with the - * constraint. Will be invalid if no group name is passed in or the - * joint model group cannot be found in the kinematic model - * - * @param [in] seed The rng seed to be used - * - */ - JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - unsigned int seed) - : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator(seed)) - { - } - - /** - * \brief Configures a joint constraint given a Constraints message. - * - * If more than one constraint for a particular joint is specified, - * the most restrictive set of bounds will be used (highest minimum - * value, lowest maximum value). For the configuration to be - * successful, the following condition must be met, in addition to - * the conditions specified in \ref configure(const std::vector &jc) : - - * \li The Constraints message must contain one or more valid joint - * constraints (where validity is judged by the ability to configure - * a \ref JointConstraint) - * - * @param [in] constr The message containing the constraints - * - * @return True if the conditions are met, otherwise false - */ - bool configure(const moveit_msgs::msg::Constraints& constr) override; - - /** - * \brief Configures a joint constraint given a vector of constraints. - * - * If more than one constraint for a particular joint is specified, - * the most restrictive set of bounds will be used (highest minimum - * value, lowest_maximum value. For the configuration to be - * successful, the following conditions must be met: - - * \li The vector must contain one or more valid, enabled joint - * constraints - * - * \li At least one constraint must reference a joint in the - * indicated group. If no additional bounds exist for this group, - * then RobotState::setToRandomPositions() can be - * used to generate a sample independently from the - * constraint_samplers infrastructure. - * - * \li The constraints must allow a sampleable region for all - * joints, where the most restrictive minimum bound is less than the - * most restrictive maximum bound - * - * @param [in] jc The vector of joint constraints - * - * @return True if the conditions are met, otherwise false - */ - bool configure(const std::vector& jc); - - bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& ks, unsigned int max_attempts) override; - - /** - * \brief Gets the number of constrained joints - joints that have an - * additional bound beyond the joint limits. - * - * - * @return The number of constrained joints. - */ - std::size_t getConstrainedJointCount() const - { - return bounds_.size(); - } - - /** - * \brief Gets the number of unconstrained joints - joint that have - * no additional bound beyond the joint limits. - * - * @return The number of unconstrained joints. - */ - std::size_t getUnconstrainedJointCount() const - { - return unbounded_.size(); - } - - /** - * \brief Get the name of the constraint sampler, for debugging purposes - * should be in CamelCase format. - * \return string of name - */ - const std::string& getName() const override - { - static const std::string SAMPLER_NAME = "JointConstraintSampler"; - return SAMPLER_NAME; - } - -protected: - /// \brief An internal structure used for maintaining constraints on a particular joint - struct JointInfo - { - /** - * \brief Constructor - * - * @return - */ - JointInfo() - { - min_bound_ = -std::numeric_limits::max(); - max_bound_ = std::numeric_limits::max(); - } - - /** - * \brief Function that adjusts the joints only if they are more - * restrictive. This means that the min limit is higher than the - * current limit, or the max limit is lower than the current max - * limit. - * - * @param min The min limit for potential adjustment - * @param max The max limit for potential adjustment - */ - void potentiallyAdjustMinMaxBounds(double min, double max) - { - min_bound_ = std::max(min, min_bound_); - max_bound_ = std::min(max, max_bound_); - } - - double min_bound_; /**< The most restrictive min value of those set */ - double max_bound_; /**< The most restrictive max value of those set */ - std::size_t index_; /**< The index within the joint state vector for this joint */ - }; - - void clear() override; - - random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random number generator used to sample */ - std::vector bounds_; /**< \brief The bounds for any joint with bounds that are more restrictive than the - joint limits */ - - std::vector unbounded_; /**< \brief The joints that are not bounded except by joint - limits */ - std::vector uindex_; /**< \brief The index of the unbounded joints in the joint state vector */ - std::vector values_; /**< \brief Values associated with this group to avoid continuously reallocating */ -}; - -/** - * \brief A structure for potentially holding a position constraint - * and an orientation constraint for use during Ik Sampling - * - */ -struct IKSamplingPose -{ - /** - * \brief Empty constructor. - * - * @return - */ - IKSamplingPose(); - - /** - * \brief Constructor that takes a single pose constraint, doing a copy - * - * @param pc The pose constraint that will be copied into the internal variable - * - */ - IKSamplingPose(const kinematic_constraints::PositionConstraint& pc); - - /** - * \brief Constructor that takes a single orientation constraint, doing a copy - * - * @param oc The orientation constraint that will be copied into the internal variable - * - * @return - */ - IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc); - - /** - * \brief Constructor that takes both a position and an orientation - * constraint, copying both into the internal variables - * - * @param pc The pose constraint that will be copied into the internal variable - * @param oc The orientation constraint that will be copied into the internal variable - * - * @return - */ - IKSamplingPose(const kinematic_constraints::PositionConstraint& pc, - const kinematic_constraints::OrientationConstraint& oc); - - /** - * \brief Constructor that takes a pointer to a position constraint. - * - * @param pc Pointer for copying into internal variable - * - * @return - */ - IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc); - - /** - * \brief Constructor that takes a pointer to a orientation constraint. - * - * @param oc Pointer for copying into internal variable - * - * @return - */ - IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc); - - /** - * \brief Constructor that takes a pointer to both position and orientation constraints. - * - * @param pc Pointer for copying into internal variables - * @param oc Pointer for copying into internal variable - * - * @return - */ - IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc, - const kinematic_constraints::OrientationConstraintPtr& oc); - - kinematic_constraints::PositionConstraintPtr position_constraint_; /**< \brief Holds the position constraint for - sampling */ - kinematic_constraints::OrientationConstraintPtr - orientation_constraint_; /**< \brief Holds the orientation constraint for sampling */ -}; - -MOVEIT_CLASS_FORWARD(IKConstraintSampler); // Defines IKConstraintSamplerPtr, ConstPtr, WeakPtr... etc - -/** - * \brief A class that allows the sampling of IK constraints. - * - * An IK constraint can have a position constraint, and orientation - * constraint, or both. The constraint will attempt to sample a pose - * that adheres to the constraint, and then solves IK for that pose. - * - */ -class IKConstraintSampler : public ConstraintSampler -{ -public: - /** - * \brief Constructor - * - * @param [in] scene The planning scene used to check the constraint - * - * @param [in] group_name The group name associated with the - * constraint. Will be invalid if no group name is passed in or the - * joint model group cannot be found in the kinematic model - * - */ - IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) - : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator()) - { - } - - /** - * \brief Constructor - * - * @param [in] scene The planning scene used to check the constraint - * - * @param [in] group_name The group name associated with the - * constraint. Will be invalid if no group name is passed in or the - * joint model group cannot be found in the kinematic model - * - * @param [in] seed The rng seed to be used - * - */ - IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - unsigned int seed) - : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator(seed)) - { - } - - /** - * \brief Configures the IK constraint given a constraints message. - * - * If the constraints message contains both orientation constraints - * and positions constraints, the function iterates through each - * potential pair until it finds a pair of position orientation - * constraints that lead to valid configuration of kinematic - * constraints. It creates an IKSamplingPose from these and calls - * \ref configure(const IKSamplingPose &sp). If no pair leads to - * both having valid configuration, it will attempt to iterate - * through the position constraints in the Constraints message, - * calling \ref configure(const IKSamplingPose &sp) on the resulting - * IKSamplingPose. Finally, if no valid position constraints exist - * it will attempt the same procedure with the orientation - * constraints. If no valid position or orientation constraints - * exist, it will return false. For more information, see the docs - * for \ref configure(const IKSamplingPose &sp). - * - * @param constr The Constraint message - * - * @return True if some valid position and orientation constraints - * exist and the overloaded configuration function returns true. - * Otherwise, returns false. - */ - bool configure(const moveit_msgs::msg::Constraints& constr) override; - - /** - * \brief Configures the Constraint given a IKSamplingPose. - * - * - * This function performs the actual constraint configuration. It returns true if the following are true: - * \li The \ref IKSamplingPose has either a valid orientation or position constraint - * \li The position and orientation constraints are specified for the same link - * - * \li There is a valid IK solver instance for the indicated group. - * This will be only be the case if a group has a specific solver - * associated with it. For situations where the super-group doesn't - * have a solver, but all subgroups have solvers, then use the - * \ref ConstraintSamplerManager. - * - * \li The kinematic model has both the links associated with the IK - * solver's tip and base frames. - * - * \li The link specified in the constraints is the tip link of the IK solver - * - * @param [in] sp The variable that contains the position and orientation constraints - * - * @return True if all conditions are met and the group specified in - * the constructor is valid. Otherwise, false. - */ - bool configure(const IKSamplingPose& sp); - - /** - * \brief Gets the timeout argument passed to the IK solver - * - * - * @return The IK timeout - */ - double getIKTimeout() const - { - return ik_timeout_; - } - - /** - * \brief Sets the timeout argument passed to the IK solver - * - * @param timeout The timeout argument that will be used in future IK calls - */ - void setIKTimeout(double timeout) - { - ik_timeout_ = timeout; - } - - /** - * \brief Gets the position constraint associated with this sampler. - * - * - * @return The position constraint, or an empty shared_ptr if none has been specified - */ - const kinematic_constraints::PositionConstraintPtr& getPositionConstraint() const - { - return sampling_pose_.position_constraint_; - } - /** - * \brief Gets the orientation constraint associated with this sampler. - * - * - * @return The orientation constraint, or an empty shared_ptr if none has been specified - */ - const kinematic_constraints::OrientationConstraintPtr& getOrientationConstraint() const - { - return sampling_pose_.orientation_constraint_; - } - - /** - * \brief Gets the volume associated with the position and orientation constraints. - * - * This function computes the volume of the sampling constraint. - * The volume associated with the position constraint is either the - * product of the volume of all position constraint regions, or 1.0 - * otherwise. The volume associated with the orientation constraint - * is the product of all the axis tolerances, or 1.0 otherwise. If - * both are specified, the product of the volumes is returned. - - * @return Returns the sum of the volumes of all constraint regions - * associated with the position and orientation constraints. - */ - double getSamplingVolume() const; - - /** - * \brief Gets the link name associated with this sampler - * - * - * @return The associated link name - */ - const std::string& getLinkName() const; - - /** - * \brief Produces an IK sample. - * - * This function first calls the \ref samplePose function to produce - * a position and orientation in the constraint region. It then - * calls IK on that pose. If a pose that satisfies the constraints - * can be determined, and IK returns a solution for that pose, then - * the joint values associated with the joint group will be - * populated with the results of the IK, and the function will - * return true. The function will attempt to sample a pose up to - * max_attempts number of times, and then call IK on that value. If - * IK is not successful, it will repeat the pose sample and IK - * procedure max_attempt times. If in any iteration a valid pose - * cannot be sample within max_attempts time, it will return false. - * - * @param jsg The joint state group in question. Must match the group passed in the constructor or will return false. - * @param ks A reference state that will be used for transforming the IK poses - * @param max_attempts The number of attempts to both sample and try IK - * - * @return True if a valid sample pose was produced and valid IK found for that pose. Otherwise false. - */ - bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, - unsigned int max_attempts) override; - - /** - * \brief Returns a pose that falls within the constraint regions. - * - * If a position constraint is specified, then a position is - * produced by selecting a random region among the constraint - * regions and taking a sample in that region. If no region is - * valid the function returns false. If no position constraint is - * specified, a position is produced by assigning a random valid - * value to each group joint, performing forward kinematics, and - * taking the resulting pose. If an orientation constraint is - * specified, then an quaternion is produced by sampling a - * difference value within the axis tolerances and applying the - * difference rotation to the orientation constraint target. - * Otherwise, a random quaternion is produced. - * - * @param [out] pos The position component of the sample - * @param [out] quat The orientation component of the sample. It will always be a normalized quaternion. - * @param [in] ks The reference state used for performing transforms - * @param [in] max_attempts The maximum attempts to try to sample - applies only to the position constraint - * - * @return True if a sample was successfully produced, otherwise false - */ - bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks, - unsigned int max_attempts); - - /** - * \brief Get the name of the constraint sampler, for debugging purposes - * should be in CamelCase format. - * \return string of name - */ - const std::string& getName() const override - { - static const std::string SAMPLER_NAME = "IKConstraintSampler"; - return SAMPLER_NAME; - } - -protected: - void clear() override; - - /** - * \brief Performs checks and sets various internal values associated with the IK solver - * - * @return True if the IK solver exists and if it associated with the expected base frame and tip frames. Otherwise - *false. - */ - bool loadIKSolver(); - - /** - * \brief Actually calls IK on the given pose, generating a random seed state. - * - * @param ik_query The pose for solving IK, assumed to be for the tip frame in the base frame - * @param timeout The timeout for the IK search - * @param jsg The joint state group into which to place the solution - * @param use_as_seed If true, the state values in jsg are used as seed for the IK - * - * @return True if IK returns successfully with the timeout, and otherwise false. - */ - bool callIK(const geometry_msgs::msg::Pose& ik_query, - const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout, - moveit::core::RobotState& state, bool use_as_seed); - bool sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, - unsigned int max_attempts); - bool validate(moveit::core::RobotState& state) const; - - random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random generator used by the sampler */ - IKSamplingPose sampling_pose_; /**< \brief Holder for the pose used for sampling */ - kinematics::KinematicsBaseConstPtr kb_; /**< \brief Holds the kinematics solver */ - double ik_timeout_; /**< \brief Holds the timeout associated with IK */ - std::string ik_frame_; /**< \brief Holds the base from of the IK solver */ - bool transform_ik_; /**< \brief True if the frame associated with the kinematic model is different than the base frame - of the IK solver */ - bool need_eef_to_ik_tip_transform_; /**< \brief True if the tip frame of the inverse kinematic is different than the - frame of the end effector */ - Eigen::Isometry3d eef_to_ik_tip_transform_; /**< \brief Holds the transformation from end effector to IK tip frame */ -}; -} // namespace constraint_samplers +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.hpp b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.hpp new file mode 100644 index 0000000000..1f1df0f556 --- /dev/null +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.hpp @@ -0,0 +1,564 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace constraint_samplers +{ +MOVEIT_CLASS_FORWARD(JointConstraintSampler); // Defines JointConstraintSamplerPtr, ConstPtr, WeakPtr... etc + +/** + * \brief JointConstraintSampler is a class that allows the sampling + * of joints in a particular group of the robot, subject to a set of individual joint constraints. + * + * The set of individual joint constraint reduce the allowable bounds + * used in the sampling. Unconstrained values will be sampled within + * their limits. + * + */ +class JointConstraintSampler : public ConstraintSampler +{ +public: + /** + * Constructor + * + * @param [in] scene The planning scene used to check the constraint + * + * @param [in] group_name The group name associated with the + * constraint. Will be invalid if no group name is passed in or the + * joint model group cannot be found in the kinematic model + * + */ + JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) + : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator()) + { + } + + /** + * Constructor + * + * @param [in] scene The planning scene used to check the constraint + * + * @param [in] group_name The group name associated with the + * constraint. Will be invalid if no group name is passed in or the + * joint model group cannot be found in the kinematic model + * + * @param [in] seed The rng seed to be used + * + */ + JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + unsigned int seed) + : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator(seed)) + { + } + + /** + * \brief Configures a joint constraint given a Constraints message. + * + * If more than one constraint for a particular joint is specified, + * the most restrictive set of bounds will be used (highest minimum + * value, lowest maximum value). For the configuration to be + * successful, the following condition must be met, in addition to + * the conditions specified in \ref configure(const std::vector &jc) : + + * \li The Constraints message must contain one or more valid joint + * constraints (where validity is judged by the ability to configure + * a \ref JointConstraint) + * + * @param [in] constr The message containing the constraints + * + * @return True if the conditions are met, otherwise false + */ + bool configure(const moveit_msgs::msg::Constraints& constr) override; + + /** + * \brief Configures a joint constraint given a vector of constraints. + * + * If more than one constraint for a particular joint is specified, + * the most restrictive set of bounds will be used (highest minimum + * value, lowest_maximum value. For the configuration to be + * successful, the following conditions must be met: + + * \li The vector must contain one or more valid, enabled joint + * constraints + * + * \li At least one constraint must reference a joint in the + * indicated group. If no additional bounds exist for this group, + * then RobotState::setToRandomPositions() can be + * used to generate a sample independently from the + * constraint_samplers infrastructure. + * + * \li The constraints must allow a sampleable region for all + * joints, where the most restrictive minimum bound is less than the + * most restrictive maximum bound + * + * @param [in] jc The vector of joint constraints + * + * @return True if the conditions are met, otherwise false + */ + bool configure(const std::vector& jc); + + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& ks, unsigned int max_attempts) override; + + /** + * \brief Gets the number of constrained joints - joints that have an + * additional bound beyond the joint limits. + * + * + * @return The number of constrained joints. + */ + std::size_t getConstrainedJointCount() const + { + return bounds_.size(); + } + + /** + * \brief Gets the number of unconstrained joints - joint that have + * no additional bound beyond the joint limits. + * + * @return The number of unconstrained joints. + */ + std::size_t getUnconstrainedJointCount() const + { + return unbounded_.size(); + } + + /** + * \brief Get the name of the constraint sampler, for debugging purposes + * should be in CamelCase format. + * \return string of name + */ + const std::string& getName() const override + { + static const std::string SAMPLER_NAME = "JointConstraintSampler"; + return SAMPLER_NAME; + } + +protected: + /// \brief An internal structure used for maintaining constraints on a particular joint + struct JointInfo + { + /** + * \brief Constructor + * + * @return + */ + JointInfo() + { + min_bound_ = -std::numeric_limits::max(); + max_bound_ = std::numeric_limits::max(); + } + + /** + * \brief Function that adjusts the joints only if they are more + * restrictive. This means that the min limit is higher than the + * current limit, or the max limit is lower than the current max + * limit. + * + * @param min The min limit for potential adjustment + * @param max The max limit for potential adjustment + */ + void potentiallyAdjustMinMaxBounds(double min, double max) + { + min_bound_ = std::max(min, min_bound_); + max_bound_ = std::min(max, max_bound_); + } + + double min_bound_; /**< The most restrictive min value of those set */ + double max_bound_; /**< The most restrictive max value of those set */ + std::size_t index_; /**< The index within the joint state vector for this joint */ + }; + + void clear() override; + + random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random number generator used to sample */ + std::vector bounds_; /**< \brief The bounds for any joint with bounds that are more restrictive than the + joint limits */ + + std::vector unbounded_; /**< \brief The joints that are not bounded except by joint + limits */ + std::vector uindex_; /**< \brief The index of the unbounded joints in the joint state vector */ + std::vector values_; /**< \brief Values associated with this group to avoid continuously reallocating */ +}; + +/** + * \brief A structure for potentially holding a position constraint + * and an orientation constraint for use during Ik Sampling + * + */ +struct IKSamplingPose +{ + /** + * \brief Empty constructor. + * + * @return + */ + IKSamplingPose(); + + /** + * \brief Constructor that takes a single pose constraint, doing a copy + * + * @param pc The pose constraint that will be copied into the internal variable + * + */ + IKSamplingPose(const kinematic_constraints::PositionConstraint& pc); + + /** + * \brief Constructor that takes a single orientation constraint, doing a copy + * + * @param oc The orientation constraint that will be copied into the internal variable + * + * @return + */ + IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc); + + /** + * \brief Constructor that takes both a position and an orientation + * constraint, copying both into the internal variables + * + * @param pc The pose constraint that will be copied into the internal variable + * @param oc The orientation constraint that will be copied into the internal variable + * + * @return + */ + IKSamplingPose(const kinematic_constraints::PositionConstraint& pc, + const kinematic_constraints::OrientationConstraint& oc); + + /** + * \brief Constructor that takes a pointer to a position constraint. + * + * @param pc Pointer for copying into internal variable + * + * @return + */ + IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc); + + /** + * \brief Constructor that takes a pointer to a orientation constraint. + * + * @param oc Pointer for copying into internal variable + * + * @return + */ + IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc); + + /** + * \brief Constructor that takes a pointer to both position and orientation constraints. + * + * @param pc Pointer for copying into internal variables + * @param oc Pointer for copying into internal variable + * + * @return + */ + IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc, + const kinematic_constraints::OrientationConstraintPtr& oc); + + kinematic_constraints::PositionConstraintPtr position_constraint_; /**< \brief Holds the position constraint for + sampling */ + kinematic_constraints::OrientationConstraintPtr + orientation_constraint_; /**< \brief Holds the orientation constraint for sampling */ +}; + +MOVEIT_CLASS_FORWARD(IKConstraintSampler); // Defines IKConstraintSamplerPtr, ConstPtr, WeakPtr... etc + +/** + * \brief A class that allows the sampling of IK constraints. + * + * An IK constraint can have a position constraint, and orientation + * constraint, or both. The constraint will attempt to sample a pose + * that adheres to the constraint, and then solves IK for that pose. + * + */ +class IKConstraintSampler : public ConstraintSampler +{ +public: + /** + * \brief Constructor + * + * @param [in] scene The planning scene used to check the constraint + * + * @param [in] group_name The group name associated with the + * constraint. Will be invalid if no group name is passed in or the + * joint model group cannot be found in the kinematic model + * + */ + IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) + : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator()) + { + } + + /** + * \brief Constructor + * + * @param [in] scene The planning scene used to check the constraint + * + * @param [in] group_name The group name associated with the + * constraint. Will be invalid if no group name is passed in or the + * joint model group cannot be found in the kinematic model + * + * @param [in] seed The rng seed to be used + * + */ + IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + unsigned int seed) + : ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator(seed)) + { + } + + /** + * \brief Configures the IK constraint given a constraints message. + * + * If the constraints message contains both orientation constraints + * and positions constraints, the function iterates through each + * potential pair until it finds a pair of position orientation + * constraints that lead to valid configuration of kinematic + * constraints. It creates an IKSamplingPose from these and calls + * \ref configure(const IKSamplingPose &sp). If no pair leads to + * both having valid configuration, it will attempt to iterate + * through the position constraints in the Constraints message, + * calling \ref configure(const IKSamplingPose &sp) on the resulting + * IKSamplingPose. Finally, if no valid position constraints exist + * it will attempt the same procedure with the orientation + * constraints. If no valid position or orientation constraints + * exist, it will return false. For more information, see the docs + * for \ref configure(const IKSamplingPose &sp). + * + * @param constr The Constraint message + * + * @return True if some valid position and orientation constraints + * exist and the overloaded configuration function returns true. + * Otherwise, returns false. + */ + bool configure(const moveit_msgs::msg::Constraints& constr) override; + + /** + * \brief Configures the Constraint given a IKSamplingPose. + * + * + * This function performs the actual constraint configuration. It returns true if the following are true: + * \li The \ref IKSamplingPose has either a valid orientation or position constraint + * \li The position and orientation constraints are specified for the same link + * + * \li There is a valid IK solver instance for the indicated group. + * This will be only be the case if a group has a specific solver + * associated with it. For situations where the super-group doesn't + * have a solver, but all subgroups have solvers, then use the + * \ref ConstraintSamplerManager. + * + * \li The kinematic model has both the links associated with the IK + * solver's tip and base frames. + * + * \li The link specified in the constraints is the tip link of the IK solver + * + * @param [in] sp The variable that contains the position and orientation constraints + * + * @return True if all conditions are met and the group specified in + * the constructor is valid. Otherwise, false. + */ + bool configure(const IKSamplingPose& sp); + + /** + * \brief Gets the timeout argument passed to the IK solver + * + * + * @return The IK timeout + */ + double getIKTimeout() const + { + return ik_timeout_; + } + + /** + * \brief Sets the timeout argument passed to the IK solver + * + * @param timeout The timeout argument that will be used in future IK calls + */ + void setIKTimeout(double timeout) + { + ik_timeout_ = timeout; + } + + /** + * \brief Gets the position constraint associated with this sampler. + * + * + * @return The position constraint, or an empty shared_ptr if none has been specified + */ + const kinematic_constraints::PositionConstraintPtr& getPositionConstraint() const + { + return sampling_pose_.position_constraint_; + } + /** + * \brief Gets the orientation constraint associated with this sampler. + * + * + * @return The orientation constraint, or an empty shared_ptr if none has been specified + */ + const kinematic_constraints::OrientationConstraintPtr& getOrientationConstraint() const + { + return sampling_pose_.orientation_constraint_; + } + + /** + * \brief Gets the volume associated with the position and orientation constraints. + * + * This function computes the volume of the sampling constraint. + * The volume associated with the position constraint is either the + * product of the volume of all position constraint regions, or 1.0 + * otherwise. The volume associated with the orientation constraint + * is the product of all the axis tolerances, or 1.0 otherwise. If + * both are specified, the product of the volumes is returned. + + * @return Returns the sum of the volumes of all constraint regions + * associated with the position and orientation constraints. + */ + double getSamplingVolume() const; + + /** + * \brief Gets the link name associated with this sampler + * + * + * @return The associated link name + */ + const std::string& getLinkName() const; + + /** + * \brief Produces an IK sample. + * + * This function first calls the \ref samplePose function to produce + * a position and orientation in the constraint region. It then + * calls IK on that pose. If a pose that satisfies the constraints + * can be determined, and IK returns a solution for that pose, then + * the joint values associated with the joint group will be + * populated with the results of the IK, and the function will + * return true. The function will attempt to sample a pose up to + * max_attempts number of times, and then call IK on that value. If + * IK is not successful, it will repeat the pose sample and IK + * procedure max_attempt times. If in any iteration a valid pose + * cannot be sample within max_attempts time, it will return false. + * + * @param jsg The joint state group in question. Must match the group passed in the constructor or will return false. + * @param ks A reference state that will be used for transforming the IK poses + * @param max_attempts The number of attempts to both sample and try IK + * + * @return True if a valid sample pose was produced and valid IK found for that pose. Otherwise false. + */ + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, + unsigned int max_attempts) override; + + /** + * \brief Returns a pose that falls within the constraint regions. + * + * If a position constraint is specified, then a position is + * produced by selecting a random region among the constraint + * regions and taking a sample in that region. If no region is + * valid the function returns false. If no position constraint is + * specified, a position is produced by assigning a random valid + * value to each group joint, performing forward kinematics, and + * taking the resulting pose. If an orientation constraint is + * specified, then an quaternion is produced by sampling a + * difference value within the axis tolerances and applying the + * difference rotation to the orientation constraint target. + * Otherwise, a random quaternion is produced. + * + * @param [out] pos The position component of the sample + * @param [out] quat The orientation component of the sample. It will always be a normalized quaternion. + * @param [in] ks The reference state used for performing transforms + * @param [in] max_attempts The maximum attempts to try to sample - applies only to the position constraint + * + * @return True if a sample was successfully produced, otherwise false + */ + bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks, + unsigned int max_attempts); + + /** + * \brief Get the name of the constraint sampler, for debugging purposes + * should be in CamelCase format. + * \return string of name + */ + const std::string& getName() const override + { + static const std::string SAMPLER_NAME = "IKConstraintSampler"; + return SAMPLER_NAME; + } + +protected: + void clear() override; + + /** + * \brief Performs checks and sets various internal values associated with the IK solver + * + * @return True if the IK solver exists and if it associated with the expected base frame and tip frames. Otherwise + *false. + */ + bool loadIKSolver(); + + /** + * \brief Actually calls IK on the given pose, generating a random seed state. + * + * @param ik_query The pose for solving IK, assumed to be for the tip frame in the base frame + * @param timeout The timeout for the IK search + * @param jsg The joint state group into which to place the solution + * @param use_as_seed If true, the state values in jsg are used as seed for the IK + * + * @return True if IK returns successfully with the timeout, and otherwise false. + */ + bool callIK(const geometry_msgs::msg::Pose& ik_query, + const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout, + moveit::core::RobotState& state, bool use_as_seed); + bool sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, + unsigned int max_attempts); + bool validate(moveit::core::RobotState& state) const; + + random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random generator used by the sampler */ + IKSamplingPose sampling_pose_; /**< \brief Holder for the pose used for sampling */ + kinematics::KinematicsBaseConstPtr kb_; /**< \brief Holds the kinematics solver */ + double ik_timeout_; /**< \brief Holds the timeout associated with IK */ + std::string ik_frame_; /**< \brief Holds the base from of the IK solver */ + bool transform_ik_; /**< \brief True if the frame associated with the kinematic model is different than the base frame + of the IK solver */ + bool need_eef_to_ik_tip_transform_; /**< \brief True if the tip frame of the inverse kinematic is different than the + frame of the end effector */ + Eigen::Isometry3d eef_to_ik_tip_transform_; /**< \brief Holds the transformation from end effector to IK tip frame */ +}; +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index 319f2f9322..58aa8d0f8c 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,135 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include - -namespace constraint_samplers -{ -/** - * \brief This class exists as a union of constraint samplers. It - * contains a vector of constraint samplers, and will sample from each - * of them. - * - * When asked to sample it will call the samplers in a sorted order - * that samples more general groups - like a robot's whole body - - * before sampling more specific groups, such as a robot's arm. - * Member samplers can operate on all or part of a joint state group - * vector, with later samplers potentially overwriting previous - * samplers. - * - */ -class UnionConstraintSampler : public ConstraintSampler -{ -public: - /** - * \brief Constructor, which will re-order its internal list of - * samplers on construction. - * - * The samplers need not all refer to the same group, as long as all - * are part of the kinematic model. The sampler will sort the - * samplers based on a set of criteria - where A and B are two - * samplers being considered for swapping by a sort algorithm: - * - * \li If the set of links updated by the group of A are a proper - * subset of the set of links updated by the group of B, A and B are - * not swapped. If the updated links of B are a proper set of the - * updated links of A, A and B are swapped. - * - * \li Otherwise, the groups associated with A and B are either - * disjoint in terms of updated links or have an equivalent group. - * In this case, it is determined if any updated links in the group for A - * exist in the frame dependency of B, or vice-versa. - * - * \li If A depends on B, and B depends on A, a warning message is - * printed that circular dependencies are likely to lead to bad - * samples. A and B are not swapped. - * - * \li If one of the frame dependencies of B is a link updated by A, - * but not vice-versa, the samplers are swapped. - * - * \li If one of the frame dependencies of A is a link updated by B, - * but not vice-versa, the samplers are not swapped. - * - * \li If no dependency exists, the samplers are swapped according - * to alphabetical order. - * - * @param [in] scene The planning scene - * @param [in] group_name The group name is ignored, as each sampler already has a group name - * @param [in] samplers A vector of already configured samplers that will be applied for future samples - * - * @return - */ - UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - const std::vector& samplers); - - /** - * \brief Gets the sorted internal list of constraint samplers - * - * - * @return The sorted internal list of constraint samplers - */ - const std::vector& getSamplers() const - { - return samplers_; - } - - /** - * \brief No-op, as the union constraint sampler is for already - * configured samplers - * - * @param [in] constr Constraint message - * - * @return Always true - */ - bool configure(const moveit_msgs::msg::Constraints& /*constr*/) override - { - return true; - } - - /** - * \brief No-op, as the union constraint sampler can act on anything - * - * @param [in] constr Constraint message - * - * @return Always true - */ - virtual bool canService(const moveit_msgs::msg::Constraints& /*constr*/) const - { - return true; - } - - /** - * \brief Produces a sample from all configured samplers. - * - * This function will call each sampler in sorted order - * independently of the group associated with the sampler. The - * function will also operate independently of the joint state group - * passed in as an argument. If any sampler fails, the sample fails - * altogether. - * - * @param [in] state State where the group sample is written to - * @param [in] reference_state Reference kinematic state that will be passed through to samplers - * @param [in] max_attempts Max attempts, which will be passed through to samplers - * - * @return True if all individual samplers return true - */ - bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, - unsigned int max_attempts) override; - - /** - * \brief Get the name of the constraint sampler, for debugging purposes - * should be in CamelCase format. - * \return string of name - */ - const std::string& getName() const override - { - static const std::string SAMPLER_NAME = "UnionConstraintSampler"; - return SAMPLER_NAME; - } - -protected: - std::vector samplers_; /**< \brief Holder for sorted internal list of samplers*/ -}; -} // namespace constraint_samplers +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.hpp b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.hpp new file mode 100644 index 0000000000..0a0e1eea60 --- /dev/null +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.hpp @@ -0,0 +1,169 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include + +namespace constraint_samplers +{ +/** + * \brief This class exists as a union of constraint samplers. It + * contains a vector of constraint samplers, and will sample from each + * of them. + * + * When asked to sample it will call the samplers in a sorted order + * that samples more general groups - like a robot's whole body - + * before sampling more specific groups, such as a robot's arm. + * Member samplers can operate on all or part of a joint state group + * vector, with later samplers potentially overwriting previous + * samplers. + * + */ +class UnionConstraintSampler : public ConstraintSampler +{ +public: + /** + * \brief Constructor, which will re-order its internal list of + * samplers on construction. + * + * The samplers need not all refer to the same group, as long as all + * are part of the kinematic model. The sampler will sort the + * samplers based on a set of criteria - where A and B are two + * samplers being considered for swapping by a sort algorithm: + * + * \li If the set of links updated by the group of A are a proper + * subset of the set of links updated by the group of B, A and B are + * not swapped. If the updated links of B are a proper set of the + * updated links of A, A and B are swapped. + * + * \li Otherwise, the groups associated with A and B are either + * disjoint in terms of updated links or have an equivalent group. + * In this case, it is determined if any updated links in the group for A + * exist in the frame dependency of B, or vice-versa. + * + * \li If A depends on B, and B depends on A, a warning message is + * printed that circular dependencies are likely to lead to bad + * samples. A and B are not swapped. + * + * \li If one of the frame dependencies of B is a link updated by A, + * but not vice-versa, the samplers are swapped. + * + * \li If one of the frame dependencies of A is a link updated by B, + * but not vice-versa, the samplers are not swapped. + * + * \li If no dependency exists, the samplers are swapped according + * to alphabetical order. + * + * @param [in] scene The planning scene + * @param [in] group_name The group name is ignored, as each sampler already has a group name + * @param [in] samplers A vector of already configured samplers that will be applied for future samples + * + * @return + */ + UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + const std::vector& samplers); + + /** + * \brief Gets the sorted internal list of constraint samplers + * + * + * @return The sorted internal list of constraint samplers + */ + const std::vector& getSamplers() const + { + return samplers_; + } + + /** + * \brief No-op, as the union constraint sampler is for already + * configured samplers + * + * @param [in] constr Constraint message + * + * @return Always true + */ + bool configure(const moveit_msgs::msg::Constraints& /*constr*/) override + { + return true; + } + + /** + * \brief No-op, as the union constraint sampler can act on anything + * + * @param [in] constr Constraint message + * + * @return Always true + */ + virtual bool canService(const moveit_msgs::msg::Constraints& /*constr*/) const + { + return true; + } + + /** + * \brief Produces a sample from all configured samplers. + * + * This function will call each sampler in sorted order + * independently of the group associated with the sampler. The + * function will also operate independently of the joint state group + * passed in as an argument. If any sampler fails, the sample fails + * altogether. + * + * @param [in] state State where the group sample is written to + * @param [in] reference_state Reference kinematic state that will be passed through to samplers + * @param [in] max_attempts Max attempts, which will be passed through to samplers + * + * @return True if all individual samplers return true + */ + bool sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state, + unsigned int max_attempts) override; + + /** + * \brief Get the name of the constraint sampler, for debugging purposes + * should be in CamelCase format. + * \return string of name + */ + const std::string& getName() const override + { + static const std::string SAMPLER_NAME = "UnionConstraintSampler"; + return SAMPLER_NAME; + } + +protected: + std::vector samplers_; /**< \brief Holder for sorted internal list of samplers*/ +}; +} // namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index 0f855b14d4..ddc102251d 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index a3fea06024..9353d3015f 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 2aaec85145..f74141f4f3 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index a1869ccc3d..754e350768 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 13983d2ff5..320656ec4a 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index b35496a1a4..f521391b21 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -36,7 +36,7 @@ #include #include -#include "pr2_arm_ik.h" +#include "pr2_arm_ik.hpp" /**** List of angles (for reference) ******* th1 = shoulder/turret pan diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.hpp similarity index 100% rename from moveit_core/constraint_samplers/test/pr2_arm_ik.h rename to moveit_core/constraint_samplers/test/pr2_arm_ik.hpp diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index e32ee40eda..0bf50e5559 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -40,9 +40,9 @@ #include #include -#include +#include #include -#include "pr2_arm_kinematics_plugin.h" +#include "pr2_arm_kinematics_plugin.hpp" using namespace KDL; using namespace std; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.hpp similarity index 98% rename from moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h rename to moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.hpp index 6db3c908d4..ce483038b0 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.hpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include #include @@ -56,11 +56,11 @@ #include #endif -#include +#include #include -#include "pr2_arm_ik.h" +#include "pr2_arm_ik.hpp" namespace pr2_arm_kinematics { diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index cfd7cd1378..15aa8581b4 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -34,14 +34,14 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -52,7 +52,7 @@ #include #include -#include "pr2_arm_kinematics_plugin.h" +#include "pr2_arm_kinematics_plugin.hpp" class LoadPlanningModelsPr2 : public testing::Test { diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h index e74003a447..beedf3a8a5 100644 --- a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,179 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -/// Namespace for the base class of a MoveIt controller manager -namespace moveit_controller_manager -{ -/// The reported execution status -struct ExecutionStatus -{ - enum Value - { - UNKNOWN, - RUNNING, - SUCCEEDED, - PREEMPTED, - TIMED_OUT, - ABORTED, - FAILED - }; - - ExecutionStatus(Value value = UNKNOWN) : status_(value) - { - } - - operator Value() const - { - return status_; - } - - explicit operator bool() const - { - return status_ == SUCCEEDED; - } - - /// Convert the execution status to a string - std::string asString() const - { - switch (status_) - { - case RUNNING: - return "RUNNING"; - case SUCCEEDED: - return "SUCCEEDED"; - case PREEMPTED: - return "PREEMPTED"; - case TIMED_OUT: - return "TIMED_OUT"; - case ABORTED: - return "ABORTED"; - case FAILED: - return "FAILED"; - default: - return "UNKNOWN"; - } - } - -private: - Value status_; -}; - -MOVEIT_CLASS_FORWARD(MoveItControllerHandle); // Defines MoveItControllerHandlePtr, ConstPtr, WeakPtr... etc - -/** \brief MoveIt sends commands to a controller via a handle that satisfies this interface. */ -class MoveItControllerHandle -{ -public: - /** \brief Each controller has a name. The handle is initialized with that name */ - MoveItControllerHandle(const std::string& name) : name_(name) - { - } - - virtual ~MoveItControllerHandle() - { - } - - /** \brief Get the name of the controller this handle can send commands to */ - const std::string& getName() const - { - return name_; - } - - /** \brief Send a trajectory to the controller. - * - * The controller is expected to execute the trajectory, but this function call should not block. - * Blocking is achievable by calling waitForExecution(). - * Return false when the controller cannot accept the trajectory. */ - virtual bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) = 0; - - /** \brief Cancel the execution of any motion using this controller. - * - * Report false if canceling is not possible. - * If there is no execution in progress, this function is a no-op and returns true. */ - virtual bool cancelExecution() = 0; - - /** \brief Wait for the current execution to complete, or until the timeout is reached. - * - * Return true if the execution is complete (whether successful or not). - * Return false if timeout was reached. - * If timeout is -1 (default argument), wait until the execution is complete (no timeout). */ - virtual bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration::from_nanoseconds(-1)) = 0; - - /** \brief Return the execution status of the last trajectory sent to the controller. */ - virtual ExecutionStatus getLastExecutionStatus() = 0; - -protected: - std::string name_; -}; - -MOVEIT_CLASS_FORWARD(MoveItControllerManager); // Defines MoveItControllerManagerPtr, ConstPtr, WeakPtr... etc - -/** @brief MoveIt does not enforce how controllers are implemented. - To make your controllers usable by MoveIt, this interface needs to be implemented. - The main purpose of this interface is to expose the set of known controllers and - potentially to allow activating and deactivating them, if multiple controllers are available. - */ -class MoveItControllerManager -{ -public: - /** \brief Each controller known to MoveIt has a state. This - structure describes that controller's state. */ - struct ControllerState - { - ControllerState() : active_(false), default_(false) - { - } - - /** \brief A controller can be active or inactive. This means that MoveIt could activate the controller when - needed, and de-activate controllers that overlap (control the same set of joints) */ - bool active_; - - /** \brief It is often the case that multiple controllers could be used to execute a motion. Marking a controller as - default makes MoveIt prefer this controller when multiple options are available. */ - bool default_; - }; - - /** \brief Default constructor. This needs to have no arguments so that the plugin system can construct the object. */ - MoveItControllerManager() - { - } - - virtual ~MoveItControllerManager() - { - } - - virtual void initialize(const rclcpp::Node::SharedPtr& node) = 0; - - /** \brief Return a given named controller. */ - virtual MoveItControllerHandlePtr getControllerHandle(const std::string& name) = 0; - - /** \brief Get the list of known controller names. */ - virtual void getControllersList(std::vector& names) = 0; - - /** \brief Get the list of active controllers. - * - * If there is only one controller in the system, this will be active. When multiple controllers exist, - * and they operate on overlapping sets of joints, not all controllers should be active at the same time. */ - virtual void getActiveControllers(std::vector& names) = 0; - - /** \brief Report the joints a controller operates on, given the controller name. - * - * In order to decide which controller to use, it is necessary to reason about the joints a controller - * operates on. */ - virtual void getControllerJoints(const std::string& name, std::vector& joints) = 0; - - /** \brief Report the state of a controller, given its name. */ - virtual ControllerState getControllerState(const std::string& name) = 0; - - /** \brief Activate and deactivate controllers */ - virtual bool switchControllers(const std::vector& activate, - const std::vector& deactivate) = 0; -}; -} // namespace moveit_controller_manager +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.hpp b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.hpp new file mode 100644 index 0000000000..5dbda12f17 --- /dev/null +++ b/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.hpp @@ -0,0 +1,213 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +/// Namespace for the base class of a MoveIt controller manager +namespace moveit_controller_manager +{ +/// The reported execution status +struct ExecutionStatus +{ + enum Value + { + UNKNOWN, + RUNNING, + SUCCEEDED, + PREEMPTED, + TIMED_OUT, + ABORTED, + FAILED + }; + + ExecutionStatus(Value value = UNKNOWN) : status_(value) + { + } + + operator Value() const + { + return status_; + } + + explicit operator bool() const + { + return status_ == SUCCEEDED; + } + + /// Convert the execution status to a string + std::string asString() const + { + switch (status_) + { + case RUNNING: + return "RUNNING"; + case SUCCEEDED: + return "SUCCEEDED"; + case PREEMPTED: + return "PREEMPTED"; + case TIMED_OUT: + return "TIMED_OUT"; + case ABORTED: + return "ABORTED"; + case FAILED: + return "FAILED"; + default: + return "UNKNOWN"; + } + } + +private: + Value status_; +}; + +MOVEIT_CLASS_FORWARD(MoveItControllerHandle); // Defines MoveItControllerHandlePtr, ConstPtr, WeakPtr... etc + +/** \brief MoveIt sends commands to a controller via a handle that satisfies this interface. */ +class MoveItControllerHandle +{ +public: + /** \brief Each controller has a name. The handle is initialized with that name */ + MoveItControllerHandle(const std::string& name) : name_(name) + { + } + + virtual ~MoveItControllerHandle() + { + } + + /** \brief Get the name of the controller this handle can send commands to */ + const std::string& getName() const + { + return name_; + } + + /** \brief Send a trajectory to the controller. + * + * The controller is expected to execute the trajectory, but this function call should not block. + * Blocking is achievable by calling waitForExecution(). + * Return false when the controller cannot accept the trajectory. */ + virtual bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) = 0; + + /** \brief Cancel the execution of any motion using this controller. + * + * Report false if canceling is not possible. + * If there is no execution in progress, this function is a no-op and returns true. */ + virtual bool cancelExecution() = 0; + + /** \brief Wait for the current execution to complete, or until the timeout is reached. + * + * Return true if the execution is complete (whether successful or not). + * Return false if timeout was reached. + * If timeout is -1 (default argument), wait until the execution is complete (no timeout). */ + virtual bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration::from_nanoseconds(-1)) = 0; + + /** \brief Return the execution status of the last trajectory sent to the controller. */ + virtual ExecutionStatus getLastExecutionStatus() = 0; + +protected: + std::string name_; +}; + +MOVEIT_CLASS_FORWARD(MoveItControllerManager); // Defines MoveItControllerManagerPtr, ConstPtr, WeakPtr... etc + +/** @brief MoveIt does not enforce how controllers are implemented. + To make your controllers usable by MoveIt, this interface needs to be implemented. + The main purpose of this interface is to expose the set of known controllers and + potentially to allow activating and deactivating them, if multiple controllers are available. + */ +class MoveItControllerManager +{ +public: + /** \brief Each controller known to MoveIt has a state. This + structure describes that controller's state. */ + struct ControllerState + { + ControllerState() : active_(false), default_(false) + { + } + + /** \brief A controller can be active or inactive. This means that MoveIt could activate the controller when + needed, and de-activate controllers that overlap (control the same set of joints) */ + bool active_; + + /** \brief It is often the case that multiple controllers could be used to execute a motion. Marking a controller as + default makes MoveIt prefer this controller when multiple options are available. */ + bool default_; + }; + + /** \brief Default constructor. This needs to have no arguments so that the plugin system can construct the object. */ + MoveItControllerManager() + { + } + + virtual ~MoveItControllerManager() + { + } + + virtual void initialize(const rclcpp::Node::SharedPtr& node) = 0; + + /** \brief Return a given named controller. */ + virtual MoveItControllerHandlePtr getControllerHandle(const std::string& name) = 0; + + /** \brief Get the list of known controller names. */ + virtual void getControllersList(std::vector& names) = 0; + + /** \brief Get the list of active controllers. + * + * If there is only one controller in the system, this will be active. When multiple controllers exist, + * and they operate on overlapping sets of joints, not all controllers should be active at the same time. */ + virtual void getActiveControllers(std::vector& names) = 0; + + /** \brief Report the joints a controller operates on, given the controller name. + * + * In order to decide which controller to use, it is necessary to reason about the joints a controller + * operates on. */ + virtual void getControllerJoints(const std::string& name, std::vector& joints) = 0; + + /** \brief Report the state of a controller, given its name. */ + virtual ControllerState getControllerState(const std::string& name) = 0; + + /** \brief Activate and deactivate controllers */ + virtual bool switchControllers(const std::vector& activate, + const std::vector& deactivate) = 0; +}; +} // namespace moveit_controller_manager diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index cfbe2113d4..08756ce37d 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,582 +47,5 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace shapes -{ -MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc -} -namespace octomap -{ -class OcTree; -} - -/** - * \brief Namespace for holding classes that generate distance fields. - * - * Distance fields are dense 3D representations containing the - * distance to the nearest obstacles. - * - */ -namespace distance_field -{ -/// \brief The plane to visualize -enum PlaneVisualizationType -{ - XY_PLANE, - XZ_PLANE, - YZ_PLANE -}; - -MOVEIT_CLASS_FORWARD(DistanceField); // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc - -/** - * \brief DistanceField is an abstract base class for computing - * distances from sets of 3D obstacle points. The distance assigned to - * a freespace cell should be the distance to the closest obstacle - * cell. Cells that are obstacle cells should either be marked as zero - * distance, or may have a negative distance if a signed version of the - * distance field is being used and an obstacle point is internal to an - * obstacle volume. - * - * Inherited classes must contain methods for holding a dense set of 3D - * voxels as well as methods for computing the required distances. The - * distance field parent class doesn't hold the data or have any way to - * generate distances from that data. - * - */ -class DistanceField -{ -public: - /** - * \brief Constructor, where units are arbitrary but are assumed to - * be meters. - * - * @param [in] size_x The X dimension in meters of the volume to represent - * @param [in] size_y The Y dimension in meters of the volume to represent - * @param [in] size_z The Z dimension in meters of the volume to represent - * @param [in] resolution The resolution in meters of the volume - * @param [in] origin_x The minimum X point of the volume - * @param [in] origin_y The minimum Y point of the volume - * @param [in] origin_z The minimum Z point of the volume - */ - DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, - double origin_z); - - virtual ~DistanceField(); - - /** - * \brief Add a set of obstacle points to the distance field, - * updating distance values accordingly. The distance field may - * already contain obstacle cells. - * - * @param [in] points The set of obstacle points to add - */ - virtual void addPointsToField(const EigenSTL::vector_Vector3d& points) = 0; - - /** - * \brief Remove a set of obstacle points from the distance field, - * updating distance values accordingly. - * - * This function will invalidate the distance measurements - * associated with the obstacle points and recompute them. - * Depending on the implementation of the derived class and the - * proportion of the total occupied points being removed, it may be - * more efficient to use the \ref reset() function and add the - * remaining obstacle points to the grid again. - - * @param [in] points The set of obstacle points that will be set as free - */ - virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points) = 0; - - /** - * \brief This function will remove any obstacle points that are in - * the old point set but not the new point set, and add any obstacle - * points that are in the new block set but not the old block set. - * Any points that are in both sets are left unchanged. - * - * The primary use case for this function is in moving objects - - * calculating the set of points associated with an object, moving - * the object in space, and calculating the new set of points. If - * the object has moved substantially, such that the old object - * position does not overlap with the new object position, then it - * may be more efficient to call \ref removePointsFromField on the - * old points and \ref addPointsToField on the new points. If the - * object has moved only slightly, however, this function may offer a - * speed improvement. All points in the old_points set should have - * been previously added to the field in order for this function to - * act as intended - points that are in both the old_points set and - * the new_points set will not be added to the field, as they are - * assumed to already be obstacle points. - * - * @param [in] old_points The set of points that all should be obstacle cells in the distance field - * @param [in] new_points The set of points, all of which are intended to be obstacle points in the distance field - */ - virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points, - const EigenSTL::vector_Vector3d& new_points) = 0; - - /** - * @brief Get the points associated with a shape. - * This is mainly used when the external application needs to cache points. - * @param [in] shape The shape to find points for. - * @param [in] pose The pose of the shape. - * @param [out] points The points determined for this shape. - */ - bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points); - - /** - * \brief Adds the set of points corresponding to the shape at the - * given pose as obstacle points into the distance field. If the - * shape is an OcTree, the pose information is ignored and the - * OcTree is passed to the \ref addOcTreeToField function. - * - * This function uses the Body class in the geometric_shapes package - * to determine the set of obstacle points, with the exception of - * OcTrees as mentioned. A bounding sphere is computed given the - * shape; the bounding sphere is iterated through in 3D at the - * resolution of the distance_field, with each point tested for - * point inclusion. For more information about the behavior of - * bodies and poses please see the documentation for - * geometric_shapes. - * - * @param [in] shape The shape to add to the distance field - * @param [in] pose The pose of the shape - */ - void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); - - /** - * \brief Adds an octree to the distance field. Cells that are - * occupied in the octree that lie within the voxel grid are added - * to the distance field. The octree can represent either a larger - * or smaller volume than the distance field. If the resolution of - * the octree is less than or equal to the resolution of the - * distance field then the center of each leaf cell of the octree - * will be added to the distance field. If the resolution of the - * octree is greater than a 3D volume of the correct resolution will - * be added for each occupied leaf node. - * - * @param [in] octree The octree to add to the distance field - */ - void addOcTreeToField(const octomap::OcTree* octree); - - /** - * \brief Moves the shape in the distance field from the old pose to - * the new pose, removing points that are no longer obstacle points, - * and adding points that are now obstacle points at the new pose. - * This function will discretize both shapes, and call the - * \ref updatePointsInField function on the old and new point sets. - * - * It's important to note that this function has no semantic - * understanding of an object - this function may be called even if - * \ref addShapeToField was not previously called. Furthermore, - * points will be removed even if they have been added by multiple - * different sources - a cup resting on a table that is moved make - * take a chunk out of the top of the table. - * - * @param [in] shape The shape to move in the distance field - * @param [in] old_pose The old pose of the shape - * @param [in] new_pose The new pose of the shape - */ - void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose, - const Eigen::Isometry3d& new_pose); - - /** - * \brief All points corresponding to the shape are removed from the - * distance field. - * - * The points needs not have been added using \ref addShapeToField. - * - * @param [in] shape The shape to remove from the distance field - * @param [in] pose The pose of the shape to remove - */ - void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); - - /** - * \brief Resets all points in the distance field to an uninitialize - * value. - */ - virtual void reset() = 0; - - /** - * \brief Gets the distance to the closest obstacle at the given - * location. The particulars of this function are heavily dependent - * on the behavior of the derived class. - * - * The particular implementation may return a max distance value if - * a cell is far away from all obstacles. Values of 0.0 should - * represent that a cell is an obstacle, and some implementations - * may return a signed value representing distance to the surface - * when a cell is deep inside an obstacle volume. An implementation - * may also return some value to represent when a location is - * outside the represented volume. - * - * @param [in] x The cell's X value - * @param [in] y The cell's Y value - * @param [in] z The cell's Z value - * - * @return The distance to the closest obstacle cell - */ - virtual double getDistance(double x, double y, double z) const = 0; - - /** - * \brief Gets not only the distance to the nearest cell but the - * gradient direction. The gradient is computed as a function of - * the distances of near-by cells. An uninitialized distance is - * returned if the cell is not valid for gradient production - * purposes. The gradient is pointing out of the obstacle - thus to - * recover the closest obstacle point, the normalized gradient value - * is multiplied by the distance and subtracted from the cell's - * location, as shown below. - * - * A number of different cells will not have valid gradients. Any - * cell that is entirely surrounded by cells of the same distance - * will not have a valid gradient. Depending on the implementation - * of the distance field, such cells may be found far away from - * obstacles (if a distance is not computed for every cell), or deep - * within obstacles. Such points can be detected as having zero - * magnitude for the gradient. - * - * The closest cell to a given cell can be computed given the - * following formulation (this value will only be within the - * resolution parameter of the correct location), including a test - * for a non-zero gradient magnitude: - * - *\code - * Eigen::Vector3d grad; - * double dist = distance_field.getDistanceGradient(x,y,z,grad.x(),grad.y(),grad.z(),in_bounds); - * if(grad.norm() > 0) { - * double closest_point_x = x-(grad.x()/grad.norm())*dist; - * double closest_point_y = y-(grad.y()/grad.norm())*dist; - * double closest_point_z = z-(grad.z()/grad.norm())*dist; - * } - * \endcode - * - * @param [in] x The X location of the cell - * @param [in] y The X location of the cell - * @param [in] z The X location of the cell - * @param [out] gradient_x The X component of the gradient to the closest occupied cell - * @param [out] gradient_y The Y component of the gradient to the closest occupied cell - * @param [out] gradient_z The Z component of the gradient to the closest occupied cell - * - * @param [out] in_bounds Whether or not the (x,y,z) is valid for - * gradient purposes. Gradients are not valid at the boundary of - * the distance field (cells where one or more of the indexes are at - * 0 or at the maximum size). - * - * @return The distance to the closest occupied cell - */ - double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z, - bool& in_bounds) const; - /** - * \brief Gets the distance to the closest obstacle at the given - * integer cell location. The particulars of this function are - * heavily dependent on the behavior of the derived class. - * - * @param [in] x The X index of the cell - * @param [in] y The Y index of the cell - * @param [in] z The Z index of the cell - * - * @return The distance to the closest occupied cell - */ - virtual double getDistance(int x, int y, int z) const = 0; - - /** - * \brief Determines whether or not the cell associated with the - * supplied indices is valid for this distance field. - * - * @param [in] x The X index of the cell - * @param [in] y The Y index of the cell - * @param [in] z The Z index of the cell - * - * @return True if the cell is valid, otherwise false. - */ - virtual bool isCellValid(int x, int y, int z) const = 0; - - /** - * \brief Gets the number of cells along the X axis - * - * - * @return The number of cells along the X axis - */ - virtual int getXNumCells() const = 0; - - /** - * \brief Gets the number of cells along the Y axis - * - * - * @return The number of cells along the Y axis - */ - virtual int getYNumCells() const = 0; - - /** - * \brief Gets the number of cells along the Z axis - * - * - * @return The number of cells along the Z axis - */ - virtual int getZNumCells() const = 0; - - /** - * \brief Converts from an set of integer indices to a world - * location given the origin and resolution parameters. - * - * @param [in] x The integer X location - * @param [in] y The integer Y location - * @param [in] z The integer Z location - * @param [out] world_x The computed world X location - * @param [out] world_y The computed world X location - * @param [out] world_z The computed world X location - * @return Whether or not the transformation is successful. An implementation may or may not choose to return false - *if the indicated cell is not valid for this distance field. - */ - virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const = 0; - - /** - * \brief Converts from a world location to a set of integer - * indices. Should return false if the world location is not valid - * in the distance field, and should populate the index values in - * either case. - * - * @param [in] world_x The world X location - * @param [in] world_y The world Y location - * @param [in] world_z The world Z location - * @param [out] x The computed integer X location - * @param [out] y The computed integer X location - * @param [out] z The computed integer X location - * - * @return True if all the world values result in integer indices - * that pass a validity check; otherwise False. - */ - virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const = 0; - - /** - * \brief Writes the contents of the distance field to the supplied stream. - * - * @param [out] stream The stream to which to write the distance field contents. - * - * @return True if the writing is successful; otherwise, false. - */ - virtual bool writeToStream(std::ostream& stream) const = 0; - - /** - * \brief Reads, parameterizes, and populates the distance field - * based on the supplied stream. - * - * @param [in] stream The stream from which to read - * - * @return True if reading, parameterizing, and populating the - * distance field is successful; otherwise False. - */ - virtual bool readFromStream(std::istream& stream) = 0; - - /** - * \brief Get an iso-surface for visualization in rviz. The - * iso-surface shows every cell that has a distance in a given - * range in the distance field. The cells are added as a - * visualization_msgs::msg::Marker::CUBE_LIST in the namespace - * "distance_field". - * - * @param [in] min_distance Cells of less than this distance will not be added to the marker - * @param [in] max_distance Cells of greater than this distance will not be added to the marker - * @param [in] frame_id The frame to use as the header in the marker - * @param [in] stamp The stamp to use in the header of the marker - * @param [out] marker The marker that will contain the indicated cells. - */ - void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id, - const rclcpp::Time& stamp, visualization_msgs::msg::Marker& marker) const; - - /** - * \brief Populates the supplied marker array with a series of - * arrows representing gradients of cells that are within the - * supplied range in terms of distance. The markers will be - * visualization_msgs::msg::Marker::ARROW in the namespace - * "distance_field_gradient". - * - * @param [in] min_distance Cells of less than this distance will not be added to the marker - * @param [in] max_distance Cells of greater than this distance will not be added to the marker - * @param [in] frame_id The frame to use as the header in the marker - * @param [in] stamp The stamp to use in the header of the marker - * @param [out] marker_array The marker array to populate - */ - void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const rclcpp::Time& stamp, - visualization_msgs::msg::MarkerArray& marker_array) const; - - /** - * \brief Populates a marker with a slice of the distance field in a - * particular plane. All cells in the plane will be added to the - * field, with colors associated with their distance. - * - * @param [in] type Which plane to show in the marker - - * @param [in] length The length of the plane to show. If the type - * is XZ or XY, it's interpreted as the dimension along the X axis. - * If the type is YZ, it's interpreted along the Y axis. - - * @param [in] width The width of the plane to show. If the type is - * XZ or YZ, it's interpreted along the Z axis. If the type is XY, - * it's interpreted along the Y axis. - - * @param [in] height The height of the plane to show. If the type - * is XY, it's interpreted along the Z axis. If the type is XZ, - * it's interpreted along the Y axis. If the type is YZ, it's - * interpreted along the X axis. - - * @param [in] origin The minimum point along each axis to display - * @param [in] frame_id The frame to use as the header in the marker - * @param [in] stamp The stamp to use in the header of the marker - * @param [out] marker The marker that will contain the indicated cells. - */ - void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, - const Eigen::Vector3d& origin, const std::string& frame_id, const rclcpp::Time& stamp, - visualization_msgs::msg::Marker& marker) const; - /** - * \brief A function that populates the marker with three planes - - * one each along the XY, XZ, and YZ axes. For each of the planes, - * any column on that plane will be marked according to the minimum - * distance along that column. - * - * @param [in] frame_id The frame to use as the header in the marker - * @param [in] stamp The stamp to use in the header of the marker - * - * @param [in] max_distance A max distance for color calculation. - * Distances of this value or greater will show up as fully white in - * the marker. - * - * @param [out] marker The marker, which will be populated with a - * visualization_msgs::msg::Marker::CUBE_LIST . - */ - void getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_distance, - visualization_msgs::msg::Marker& marker) const; - - /** - * \brief Gets the distance field size along the X dimension in meters - * - * - * @return The size along the X dimension in meters - */ - double getSizeX() const - { - return size_x_; - } - - /** - * \brief Gets the distance field size along the Y dimension in meters - * - * - * @return The size along the Y dimension in meters - */ - double getSizeY() const - { - return size_y_; - } - - /** - * \brief Gets the distance field size along the Z dimension in meters - * - * - * @return The size along the Z dimension in meters - */ - double getSizeZ() const - { - return size_z_; - } - - /** - * \brief Gets the origin (minimum value) along the X dimension - * - * - * @return The X origin - */ - double getOriginX() const - { - return origin_x_; - } - - /** - * \brief Gets the origin (minimum value) along the Y dimension - * - * - * @return The Y origin - */ - double getOriginY() const - { - return origin_y_; - } - - /** - * \brief Gets the origin (minimum value) along the Z dimension - * - * - * @return The Z origin - */ - double getOriginZ() const - { - return origin_z_; - } - - /** - * \brief Gets the resolution of the distance field in meters - * - * - * @return The resolution of the distance field in meters - */ - double getResolution() const - { - return resolution_; - } - - /** - * \brief Gets a distance value for an invalid cell. - * - * - * @return The distance associated with an uninitialized cell - */ - virtual double getUninitializedDistance() const = 0; - -protected: - /** - * @brief Get the points associated with an octree. - * @param [in] octree The octree to find points for. - * @param [out] points The points determined for this octree. - */ - void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points); - - /** - * \brief Helper function that sets the point value and color given - * the distance. - * - * @param [in] xCell The x index of the cell - * @param [in] yCell The y index of the cell - * @param [in] zCell The z index of the cell - * @param [in] dist The distance of the cell - * @param [out] point World coordinates will be placed here - * - * @param [out] color A color will be assigned here that's only red if the distance is 0, and gets progressively - *whiter as the dist value approaches max_distance. - * - * @param [in] max_distance The distance past which all cells will be fully white - */ - void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point, - std_msgs::msg::ColorRGBA& color, double max_distance) const; - - double size_x_; /**< \brief X size of the distance field */ - double size_y_; /**< \brief Y size of the distance field */ - double size_z_; /**< \brief Z size of the distance field */ - double origin_x_; /**< \brief X origin of the distance field */ - double origin_y_; /**< \brief Y origin of the distance field */ - double origin_z_; /**< \brief Z origin of the distance field */ - double resolution_; /**< \brief Resolution of the distance field */ - int inv_twice_resolution_; /**< \brief Computed value 1.0/(2.0*resolution_) */ -}; - -} // namespace distance_field +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp b/moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp new file mode 100644 index 0000000000..535918c92d --- /dev/null +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp @@ -0,0 +1,616 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace shapes +{ +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc +} +namespace octomap +{ +class OcTree; +} + +/** + * \brief Namespace for holding classes that generate distance fields. + * + * Distance fields are dense 3D representations containing the + * distance to the nearest obstacles. + * + */ +namespace distance_field +{ +/// \brief The plane to visualize +enum PlaneVisualizationType +{ + XY_PLANE, + XZ_PLANE, + YZ_PLANE +}; + +MOVEIT_CLASS_FORWARD(DistanceField); // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc + +/** + * \brief DistanceField is an abstract base class for computing + * distances from sets of 3D obstacle points. The distance assigned to + * a freespace cell should be the distance to the closest obstacle + * cell. Cells that are obstacle cells should either be marked as zero + * distance, or may have a negative distance if a signed version of the + * distance field is being used and an obstacle point is internal to an + * obstacle volume. + * + * Inherited classes must contain methods for holding a dense set of 3D + * voxels as well as methods for computing the required distances. The + * distance field parent class doesn't hold the data or have any way to + * generate distances from that data. + * + */ +class DistanceField +{ +public: + /** + * \brief Constructor, where units are arbitrary but are assumed to + * be meters. + * + * @param [in] size_x The X dimension in meters of the volume to represent + * @param [in] size_y The Y dimension in meters of the volume to represent + * @param [in] size_z The Z dimension in meters of the volume to represent + * @param [in] resolution The resolution in meters of the volume + * @param [in] origin_x The minimum X point of the volume + * @param [in] origin_y The minimum Y point of the volume + * @param [in] origin_z The minimum Z point of the volume + */ + DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, + double origin_z); + + virtual ~DistanceField(); + + /** + * \brief Add a set of obstacle points to the distance field, + * updating distance values accordingly. The distance field may + * already contain obstacle cells. + * + * @param [in] points The set of obstacle points to add + */ + virtual void addPointsToField(const EigenSTL::vector_Vector3d& points) = 0; + + /** + * \brief Remove a set of obstacle points from the distance field, + * updating distance values accordingly. + * + * This function will invalidate the distance measurements + * associated with the obstacle points and recompute them. + * Depending on the implementation of the derived class and the + * proportion of the total occupied points being removed, it may be + * more efficient to use the \ref reset() function and add the + * remaining obstacle points to the grid again. + + * @param [in] points The set of obstacle points that will be set as free + */ + virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points) = 0; + + /** + * \brief This function will remove any obstacle points that are in + * the old point set but not the new point set, and add any obstacle + * points that are in the new block set but not the old block set. + * Any points that are in both sets are left unchanged. + * + * The primary use case for this function is in moving objects - + * calculating the set of points associated with an object, moving + * the object in space, and calculating the new set of points. If + * the object has moved substantially, such that the old object + * position does not overlap with the new object position, then it + * may be more efficient to call \ref removePointsFromField on the + * old points and \ref addPointsToField on the new points. If the + * object has moved only slightly, however, this function may offer a + * speed improvement. All points in the old_points set should have + * been previously added to the field in order for this function to + * act as intended - points that are in both the old_points set and + * the new_points set will not be added to the field, as they are + * assumed to already be obstacle points. + * + * @param [in] old_points The set of points that all should be obstacle cells in the distance field + * @param [in] new_points The set of points, all of which are intended to be obstacle points in the distance field + */ + virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points, + const EigenSTL::vector_Vector3d& new_points) = 0; + + /** + * @brief Get the points associated with a shape. + * This is mainly used when the external application needs to cache points. + * @param [in] shape The shape to find points for. + * @param [in] pose The pose of the shape. + * @param [out] points The points determined for this shape. + */ + bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points); + + /** + * \brief Adds the set of points corresponding to the shape at the + * given pose as obstacle points into the distance field. If the + * shape is an OcTree, the pose information is ignored and the + * OcTree is passed to the \ref addOcTreeToField function. + * + * This function uses the Body class in the geometric_shapes package + * to determine the set of obstacle points, with the exception of + * OcTrees as mentioned. A bounding sphere is computed given the + * shape; the bounding sphere is iterated through in 3D at the + * resolution of the distance_field, with each point tested for + * point inclusion. For more information about the behavior of + * bodies and poses please see the documentation for + * geometric_shapes. + * + * @param [in] shape The shape to add to the distance field + * @param [in] pose The pose of the shape + */ + void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); + + /** + * \brief Adds an octree to the distance field. Cells that are + * occupied in the octree that lie within the voxel grid are added + * to the distance field. The octree can represent either a larger + * or smaller volume than the distance field. If the resolution of + * the octree is less than or equal to the resolution of the + * distance field then the center of each leaf cell of the octree + * will be added to the distance field. If the resolution of the + * octree is greater than a 3D volume of the correct resolution will + * be added for each occupied leaf node. + * + * @param [in] octree The octree to add to the distance field + */ + void addOcTreeToField(const octomap::OcTree* octree); + + /** + * \brief Moves the shape in the distance field from the old pose to + * the new pose, removing points that are no longer obstacle points, + * and adding points that are now obstacle points at the new pose. + * This function will discretize both shapes, and call the + * \ref updatePointsInField function on the old and new point sets. + * + * It's important to note that this function has no semantic + * understanding of an object - this function may be called even if + * \ref addShapeToField was not previously called. Furthermore, + * points will be removed even if they have been added by multiple + * different sources - a cup resting on a table that is moved make + * take a chunk out of the top of the table. + * + * @param [in] shape The shape to move in the distance field + * @param [in] old_pose The old pose of the shape + * @param [in] new_pose The new pose of the shape + */ + void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose, + const Eigen::Isometry3d& new_pose); + + /** + * \brief All points corresponding to the shape are removed from the + * distance field. + * + * The points needs not have been added using \ref addShapeToField. + * + * @param [in] shape The shape to remove from the distance field + * @param [in] pose The pose of the shape to remove + */ + void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); + + /** + * \brief Resets all points in the distance field to an uninitialize + * value. + */ + virtual void reset() = 0; + + /** + * \brief Gets the distance to the closest obstacle at the given + * location. The particulars of this function are heavily dependent + * on the behavior of the derived class. + * + * The particular implementation may return a max distance value if + * a cell is far away from all obstacles. Values of 0.0 should + * represent that a cell is an obstacle, and some implementations + * may return a signed value representing distance to the surface + * when a cell is deep inside an obstacle volume. An implementation + * may also return some value to represent when a location is + * outside the represented volume. + * + * @param [in] x The cell's X value + * @param [in] y The cell's Y value + * @param [in] z The cell's Z value + * + * @return The distance to the closest obstacle cell + */ + virtual double getDistance(double x, double y, double z) const = 0; + + /** + * \brief Gets not only the distance to the nearest cell but the + * gradient direction. The gradient is computed as a function of + * the distances of near-by cells. An uninitialized distance is + * returned if the cell is not valid for gradient production + * purposes. The gradient is pointing out of the obstacle - thus to + * recover the closest obstacle point, the normalized gradient value + * is multiplied by the distance and subtracted from the cell's + * location, as shown below. + * + * A number of different cells will not have valid gradients. Any + * cell that is entirely surrounded by cells of the same distance + * will not have a valid gradient. Depending on the implementation + * of the distance field, such cells may be found far away from + * obstacles (if a distance is not computed for every cell), or deep + * within obstacles. Such points can be detected as having zero + * magnitude for the gradient. + * + * The closest cell to a given cell can be computed given the + * following formulation (this value will only be within the + * resolution parameter of the correct location), including a test + * for a non-zero gradient magnitude: + * + *\code + * Eigen::Vector3d grad; + * double dist = distance_field.getDistanceGradient(x,y,z,grad.x(),grad.y(),grad.z(),in_bounds); + * if(grad.norm() > 0) { + * double closest_point_x = x-(grad.x()/grad.norm())*dist; + * double closest_point_y = y-(grad.y()/grad.norm())*dist; + * double closest_point_z = z-(grad.z()/grad.norm())*dist; + * } + * \endcode + * + * @param [in] x The X location of the cell + * @param [in] y The X location of the cell + * @param [in] z The X location of the cell + * @param [out] gradient_x The X component of the gradient to the closest occupied cell + * @param [out] gradient_y The Y component of the gradient to the closest occupied cell + * @param [out] gradient_z The Z component of the gradient to the closest occupied cell + * + * @param [out] in_bounds Whether or not the (x,y,z) is valid for + * gradient purposes. Gradients are not valid at the boundary of + * the distance field (cells where one or more of the indexes are at + * 0 or at the maximum size). + * + * @return The distance to the closest occupied cell + */ + double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z, + bool& in_bounds) const; + /** + * \brief Gets the distance to the closest obstacle at the given + * integer cell location. The particulars of this function are + * heavily dependent on the behavior of the derived class. + * + * @param [in] x The X index of the cell + * @param [in] y The Y index of the cell + * @param [in] z The Z index of the cell + * + * @return The distance to the closest occupied cell + */ + virtual double getDistance(int x, int y, int z) const = 0; + + /** + * \brief Determines whether or not the cell associated with the + * supplied indices is valid for this distance field. + * + * @param [in] x The X index of the cell + * @param [in] y The Y index of the cell + * @param [in] z The Z index of the cell + * + * @return True if the cell is valid, otherwise false. + */ + virtual bool isCellValid(int x, int y, int z) const = 0; + + /** + * \brief Gets the number of cells along the X axis + * + * + * @return The number of cells along the X axis + */ + virtual int getXNumCells() const = 0; + + /** + * \brief Gets the number of cells along the Y axis + * + * + * @return The number of cells along the Y axis + */ + virtual int getYNumCells() const = 0; + + /** + * \brief Gets the number of cells along the Z axis + * + * + * @return The number of cells along the Z axis + */ + virtual int getZNumCells() const = 0; + + /** + * \brief Converts from an set of integer indices to a world + * location given the origin and resolution parameters. + * + * @param [in] x The integer X location + * @param [in] y The integer Y location + * @param [in] z The integer Z location + * @param [out] world_x The computed world X location + * @param [out] world_y The computed world X location + * @param [out] world_z The computed world X location + * @return Whether or not the transformation is successful. An implementation may or may not choose to return false + *if the indicated cell is not valid for this distance field. + */ + virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const = 0; + + /** + * \brief Converts from a world location to a set of integer + * indices. Should return false if the world location is not valid + * in the distance field, and should populate the index values in + * either case. + * + * @param [in] world_x The world X location + * @param [in] world_y The world Y location + * @param [in] world_z The world Z location + * @param [out] x The computed integer X location + * @param [out] y The computed integer X location + * @param [out] z The computed integer X location + * + * @return True if all the world values result in integer indices + * that pass a validity check; otherwise False. + */ + virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const = 0; + + /** + * \brief Writes the contents of the distance field to the supplied stream. + * + * @param [out] stream The stream to which to write the distance field contents. + * + * @return True if the writing is successful; otherwise, false. + */ + virtual bool writeToStream(std::ostream& stream) const = 0; + + /** + * \brief Reads, parameterizes, and populates the distance field + * based on the supplied stream. + * + * @param [in] stream The stream from which to read + * + * @return True if reading, parameterizing, and populating the + * distance field is successful; otherwise False. + */ + virtual bool readFromStream(std::istream& stream) = 0; + + /** + * \brief Get an iso-surface for visualization in rviz. The + * iso-surface shows every cell that has a distance in a given + * range in the distance field. The cells are added as a + * visualization_msgs::msg::Marker::CUBE_LIST in the namespace + * "distance_field". + * + * @param [in] min_distance Cells of less than this distance will not be added to the marker + * @param [in] max_distance Cells of greater than this distance will not be added to the marker + * @param [in] frame_id The frame to use as the header in the marker + * @param [in] stamp The stamp to use in the header of the marker + * @param [out] marker The marker that will contain the indicated cells. + */ + void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id, + const rclcpp::Time& stamp, visualization_msgs::msg::Marker& marker) const; + + /** + * \brief Populates the supplied marker array with a series of + * arrows representing gradients of cells that are within the + * supplied range in terms of distance. The markers will be + * visualization_msgs::msg::Marker::ARROW in the namespace + * "distance_field_gradient". + * + * @param [in] min_distance Cells of less than this distance will not be added to the marker + * @param [in] max_distance Cells of greater than this distance will not be added to the marker + * @param [in] frame_id The frame to use as the header in the marker + * @param [in] stamp The stamp to use in the header of the marker + * @param [out] marker_array The marker array to populate + */ + void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const rclcpp::Time& stamp, + visualization_msgs::msg::MarkerArray& marker_array) const; + + /** + * \brief Populates a marker with a slice of the distance field in a + * particular plane. All cells in the plane will be added to the + * field, with colors associated with their distance. + * + * @param [in] type Which plane to show in the marker + + * @param [in] length The length of the plane to show. If the type + * is XZ or XY, it's interpreted as the dimension along the X axis. + * If the type is YZ, it's interpreted along the Y axis. + + * @param [in] width The width of the plane to show. If the type is + * XZ or YZ, it's interpreted along the Z axis. If the type is XY, + * it's interpreted along the Y axis. + + * @param [in] height The height of the plane to show. If the type + * is XY, it's interpreted along the Z axis. If the type is XZ, + * it's interpreted along the Y axis. If the type is YZ, it's + * interpreted along the X axis. + + * @param [in] origin The minimum point along each axis to display + * @param [in] frame_id The frame to use as the header in the marker + * @param [in] stamp The stamp to use in the header of the marker + * @param [out] marker The marker that will contain the indicated cells. + */ + void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, + const Eigen::Vector3d& origin, const std::string& frame_id, const rclcpp::Time& stamp, + visualization_msgs::msg::Marker& marker) const; + /** + * \brief A function that populates the marker with three planes - + * one each along the XY, XZ, and YZ axes. For each of the planes, + * any column on that plane will be marked according to the minimum + * distance along that column. + * + * @param [in] frame_id The frame to use as the header in the marker + * @param [in] stamp The stamp to use in the header of the marker + * + * @param [in] max_distance A max distance for color calculation. + * Distances of this value or greater will show up as fully white in + * the marker. + * + * @param [out] marker The marker, which will be populated with a + * visualization_msgs::msg::Marker::CUBE_LIST . + */ + void getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_distance, + visualization_msgs::msg::Marker& marker) const; + + /** + * \brief Gets the distance field size along the X dimension in meters + * + * + * @return The size along the X dimension in meters + */ + double getSizeX() const + { + return size_x_; + } + + /** + * \brief Gets the distance field size along the Y dimension in meters + * + * + * @return The size along the Y dimension in meters + */ + double getSizeY() const + { + return size_y_; + } + + /** + * \brief Gets the distance field size along the Z dimension in meters + * + * + * @return The size along the Z dimension in meters + */ + double getSizeZ() const + { + return size_z_; + } + + /** + * \brief Gets the origin (minimum value) along the X dimension + * + * + * @return The X origin + */ + double getOriginX() const + { + return origin_x_; + } + + /** + * \brief Gets the origin (minimum value) along the Y dimension + * + * + * @return The Y origin + */ + double getOriginY() const + { + return origin_y_; + } + + /** + * \brief Gets the origin (minimum value) along the Z dimension + * + * + * @return The Z origin + */ + double getOriginZ() const + { + return origin_z_; + } + + /** + * \brief Gets the resolution of the distance field in meters + * + * + * @return The resolution of the distance field in meters + */ + double getResolution() const + { + return resolution_; + } + + /** + * \brief Gets a distance value for an invalid cell. + * + * + * @return The distance associated with an uninitialized cell + */ + virtual double getUninitializedDistance() const = 0; + +protected: + /** + * @brief Get the points associated with an octree. + * @param [in] octree The octree to find points for. + * @param [out] points The points determined for this octree. + */ + void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points); + + /** + * \brief Helper function that sets the point value and color given + * the distance. + * + * @param [in] xCell The x index of the cell + * @param [in] yCell The y index of the cell + * @param [in] zCell The z index of the cell + * @param [in] dist The distance of the cell + * @param [out] point World coordinates will be placed here + * + * @param [out] color A color will be assigned here that's only red if the distance is 0, and gets progressively + *whiter as the dist value approaches max_distance. + * + * @param [in] max_distance The distance past which all cells will be fully white + */ + void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point, + std_msgs::msg::ColorRGBA& color, double max_distance) const; + + double size_x_; /**< \brief X size of the distance field */ + double size_y_; /**< \brief Y size of the distance field */ + double size_z_; /**< \brief Z size of the distance field */ + double origin_x_; /**< \brief X origin of the distance field */ + double origin_y_; /**< \brief Y origin of the distance field */ + double origin_z_; /**< \brief Z origin of the distance field */ + double resolution_; /**< \brief Resolution of the distance field */ + int inv_twice_resolution_; /**< \brief Computed value 1.0/(2.0*resolution_) */ +}; + +} // namespace distance_field diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h index 5af220a81e..a5f7d1b2b0 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,21 +47,5 @@ /* Author: Acorn Pooley */ #pragma once - -#include -#include - -namespace distance_field -{ -/** - * \brief Find all points on a regular grid that are internal to the body, - * assuming the body is a convex shape. If the body is not convex then its - * convex hull is used. - * - * @param [in] body The body to discretize - * @param [in] resolution The resolution at which to test - * @param [out] points The points internal to the body are appended to this - * vector. - */ -void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); -} // namespace distance_field +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.hpp b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.hpp new file mode 100644 index 0000000000..5af220a81e --- /dev/null +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.hpp @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Acorn Pooley */ + +#pragma once + +#include +#include + +namespace distance_field +{ +/** + * \brief Find all points on a regular grid that are internal to the body, + * assuming the body is a convex shape. If the body is not convex then its + * convex hull is used. + * + * @param [in] body The body to discretize + * @param [in] resolution The resolution at which to test + * @param [out] points The points internal to the body are appended to this + * vector. + */ +void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); +} // namespace distance_field diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index c9c1a6d78d..b35d5dae45 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,574 +47,5 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace EigenSTL -{ -typedef std::vector> vector_Vector3i; -} - -namespace distance_field -{ -/** - * \brief Struct for sorting type Eigen::Vector3i for use in sorted - * std containers. Sorts in z order, then y order, then x order. - */ -struct CompareEigenVector3i -{ - bool operator()(const Eigen::Vector3i& loc_1, const Eigen::Vector3i& loc_2) const - { - if (loc_1.z() != loc_2.z()) - { - return (loc_1.z() < loc_2.z()); - } - else if (loc_1.y() != loc_2.y()) - { - return (loc_1.y() < loc_2.y()); - } - else if (loc_1.x() != loc_2.x()) - { - return (loc_1.x() < loc_2.x()); - } - return false; - } -}; - -/** - * \brief Structure that holds voxel information for the - * DistanceField. Will be used in VoxelGrid. - */ -struct PropDistanceFieldVoxel -{ - /** - * \brief Constructor. All fields left uninitialized. - * - * - */ - PropDistanceFieldVoxel(); - - /** - * \brief Constructor. Sets values for distance_sq_ and - * negative_distance_square_, and sets all remaining internal values - * to uninitialized. These should be integers values which - * represent the distance in cells squared. - * - * @param [in] distance_sq_positive Value to which to initialize - * distance_sq_ for distance to closest obstalce - * - * @param [in] distance_sq_negative Value to which to initialize - * distance_sq_negative_ for distance to nearest non-obstacle cell - * - */ - PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative); - - int distance_square_; /**< \brief Distance in cells to the closest obstacle, squared */ - int negative_distance_square_; /**< \brief Distance in cells to the nearest unoccupied cell, squared */ - Eigen::Vector3i closest_point_; /**< \brief Closest occupied cell */ - Eigen::Vector3i closest_negative_point_; /**< \brief Closest unoccupied cell */ - int update_direction_; /**< \brief Direction from which this voxel was updated for occupied distance propagation */ - int negative_update_direction_; /**< \brief Direction from which this voxel was updated for negative distance - propagation*/ - - static const int UNINITIALIZED = -1; /**< \brief Value that represents an uninitialized voxel */ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * \brief A DistanceField implementation that uses a vector - * propagation method. Distances propagate outward from occupied - * cells, or inwards from unoccupied cells if negative distances are - * to be computed, which is optional. Outward and inward propagation - * only occur to a desired maximum distance - cells that are more than - * this maximum distance from the nearest cell will have maximum - * distance measurements. - * - * This class uses a \ref VoxelGrid to hold all data. One important - * decision that must be made on construction is whether or not to - * create a signed version of the distance field. If the distance - * field is unsigned, it means that the minimum obstacle distance is - * 0, a value that will be assigned to all obstacle cells. Gradient - * queries for obstacle cells will not give useful information, as the - * gradient at an obstacle cell will point to the cell itself. If - * this behavior is acceptable, then the performance of this mode will - * be more efficient, as no propagation will occur for obstacle cells. - * The other option is to calculate signed distances. In this case, - * negative distances up to the maximum distance are calculated for - * obstacle volumes. This distance encodes the distance of an - * obstacle cell to the nearest unoccupied obstacle voxel. Furthmore, - * gradients pointing out of the volume will be produced. Depending - * on the data, calculating this data can significantly impact the - * time it takes to add and remove obstacle cells. - */ -class PropagationDistanceField : public DistanceField -{ -public: - /** - * \brief Constructor that initializes entire distance field to - * empty - all cells will be assigned maximum distance values. All - * units are arbitrary but are assumed for documentation purposes to - * represent meters. - * - * @param [in] size_x The X dimension in meters of the volume to represent - * @param [in] size_y The Y dimension in meters of the volume to represent - * @param [in] size_z The Z dimension in meters of the volume to represent - * @param [in] resolution The resolution in meters of the volume - * @param [in] origin_x The minimum X point of the volume - * @param [in] origin_y The minimum Y point of the volume - * @param [in] origin_z The minimum Z point of the volume - - * @param [in] max_distance The maximum distance to which to - * propagate distance values. Cells that are greater than this - * distance will be assigned the maximum distance value. - * - * @param [in] propagate_negative_distances Whether or not to - * propagate negative distances. If false, no propagation occurs, - * and all obstacle cells will be assigned zero distance. See the - * \ref PropagationDistanceField description for more information on - * the implications of this. - * - */ - PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, - double origin_y, double origin_z, double max_distance, - bool propagate_negative_distances = false); - - /** - * \brief Constructor based on an OcTree and bounding box - * information. A distance field will be constructed with - * dimensions based on the supplied bounding box at the resolution - * of the OcTree. All octree obstacle cells will be added to the - * resulting distance field using the \ref DistanceField::addOcTreeToField - * function. - * - * @param [in] octree The OcTree from which to construct the distance field - * @param [in] bbx_min The minimum world coordinates of the bounding box - * @param [in] bbx_max The maximum world coordinates of the bounding box - * - * @param [in] max_distance The maximum distance to which to - * propagate distance values. Cells that are greater than this - * distance will be assigned the maximum distance value. - * - * @param [in] propagate_negative_distances Whether or not to - * propagate negative distances. If false, no propagation occurs, - * and all obstacle cells will be assigned zero distance. See the - * \ref PropagationDistanceField description for more information on - * the implications of this. - */ - PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min, - const octomap::point3d& bbx_max, double max_distance, - bool propagate_negative_distances = false); - - /** - * \brief Constructor that takes an istream and reads the contents - * of a saved distance field, adding all obstacle points and running - * propagation given the arguments for max_distance and - * propagate_negative_distances. Calls the function - * \ref readFromStream. - * - * @param [in] stream The stream from which to read the data - * - * @param [in] max_distance The maximum distance to which to - * propagate distance values. Cells that are greater than this - * distance will be assigned the maximum distance value. - * - * @param [in] propagate_negative_distances Whether or not to - * propagate negative distances. If false, no propagation occurs, - * and all obstacle cells will be assigned zero distance. See the - * \ref PropagationDistanceField description for more information on - * the implications of this. - * - * @return - */ - PropagationDistanceField(std::istream& stream, double max_distance, bool propagate_negative_distances = false); - /** - * \brief Empty destructor - * - * - */ - ~PropagationDistanceField() override - { - } - - /** - * \brief Add a set of obstacle points to the distance field, - * updating distance values accordingly. The distance field may - * already contain obstacle cells. - * - * The function first checks that each location represents a valid - * point - only valid points will be added. It takes the vector of - * valid points and performs positive propagation on them. If the - * class has been set up to propagate negative distance, those will - * also be propagated. - * - * @param [in] points The set of obstacle points to add - */ - void addPointsToField(const EigenSTL::vector_Vector3d& points) override; - - /** - * \brief Remove a set of obstacle points from the distance field, - * updating distance values accordingly. - * - * This function is relatively less efficient than adding points to - * the field in terms of positive distances - adding a given number - * of points will be less comptationally expensive than removing the - * same number of points. This is due to the nature of the - * propagation algorithm - when removing sets of cells, we must - * search outward from the freed cells and then propagate inward. - * Negative distances can be propagated more efficiently, as - * propagation can occur outward from newly freed cells without - * requiring a search step. If the set of occupied points that - * remain after removal is small it may be more efficient to call - * \ref reset and then to add the remaining points rather than - * removing a set of points. - * - * @param [in] points The set of obstacle points that will be set as free - */ - void removePointsFromField(const EigenSTL::vector_Vector3d& points) override; - - /** - * \brief This function will remove any obstacle points that are in - * the old point set but not the new point set, and add any obstacle - * points that are in the new block set but not the old block set. - * Any points that are in both sets are left unchanged. For more - * information see \ref DistanceField::updatePointsInField. - * - * The implementation of this function finds the set of points that - * are in the old_points and not the new_points, and the in the - * new_points and not the old_points using std::set_difference. It - * then calls a removal function on the former set, and an addition - * function on the latter set. - * - * If there is no overlap between the old_points and the new_points - * it is more efficient to first call \ref removePointsFromField on - * the old_points and then \ref addPointsToField on the new points - - * this does not require computing set differences. - * - * @param [in] old_points The set of points that all should be obstacle cells in the distance field - * @param [in] new_points The set of points, all of which are intended to be obstacle points in the distance field - * - */ - void updatePointsInField(const EigenSTL::vector_Vector3d& old_points, - const EigenSTL::vector_Vector3d& new_points) override; - - /** - * \brief Resets the entire distance field to max_distance for - * positive values and zero for negative values. - * - */ - void reset() override; - - /** - * \brief Get the distance value associated with the cell indicated - * by the world coordinate. If the cell is invalid, max_distance - * will be returned. If running without negative distances, all - * obstacle cells will have zero distance. If running with negative - * distances, the distance will be between -max_distance and - * max_distance, with no values having a 0 distance. - * - * - * @param [in] x The X location of the cell - * @param [in] y The X location of the cell - * @param [in] z The X location of the cell - * - * @return The distance value - */ - double getDistance(double x, double y, double z) const override; - - /** - * \brief Get the distance value associated with the cell indicated - * by the index coordinates. If the cell is invalid, max_distance - * will be returned. If running without negative distances, all - * obstacle cells will have zero distance. If running with negative - * distances, the distance will be between -max_distance and - * max_distance, with no values having a 0 distance. - * - * - * @param [in] x The integer X location - * @param [in] y The integer Y location - * @param [in] z The integer Z location - * - * @return The distance value for the cell - */ - double getDistance(int x, int y, int z) const override; - - bool isCellValid(int x, int y, int z) const override; - int getXNumCells() const override; - int getYNumCells() const override; - int getZNumCells() const override; - bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const override; - bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const override; - - /** - * \brief Writes the contents of the distance field to the supplied stream. - * - * This function writes the resolution, size, and origin parameters - * to the file in ASCII. It then writes the occupancy data only in - * bit form (with values or 1 representing occupancy, and 0 - * representing empty space). It further runs Zlib compression on - * the binary data before actually writing to disk. The - * max_distance and propagate_negative_distances values are not - * written to file, and the distances themselves will need to be - * recreated on load. - * - * @param [out] stream The stream to which to write the distance field contents. - * - * @return True - */ - bool writeToStream(std::ostream& stream) const override; - - /** - * \brief Reads, parameterizes, and populates the distance field - * based on the supplied stream. - * - * This function assumes that the file begins with ASCII data, and - * that the binary data has been written in bit formulation and - * compressed using Zlib. The function will reinitialize all data - * members based on the data in the file, using preset values for - * max_distance_ and propagate_negative_distances_. All occupied - * cells will be added to the distance field. - * - * @param [in] stream The stream from which to read - * - * @return True if reading, parameterizing, and populating the - * distance field is successful; otherwise False. - */ - bool readFromStream(std::istream& stream) override; - - // passthrough docs to DistanceField - double getUninitializedDistance() const override - { - return max_distance_; - } - - /** - * \brief Gets full cell data given an index. - * - * x,y,z MUST be valid or data corruption (SEGFAULTS) will occur. - * - * @param [in] x The integer X location - * @param [in] y The integer Y location - * @param [in] z The integer Z location - * - * @return The data in the indicated cell. - */ - const PropDistanceFieldVoxel& getCell(int x, int y, int z) const - { - return voxel_grid_->getCell(x, y, z); - } - - /** - * \brief Gets nearest surface cell and returns distance to it. - * - * x,y,z MUST be valid or data corruption (SEGFAULTS) will occur. - * - * @param [in] x The integer X location of the starting cell - * @param [in] y The integer Y location of the starting cell - * @param [in] z The integer Z location of the starting cell - * @param [out] dist if starting cell is inside, the negative distance to the nearest outside cell - * if starting cell is outside, the positive distance to the nearest inside cell - * if nearby cell is unknown, zero - * @param [out] pos the position of the nearest cell - * - * - * @return If starting cell is inside, the nearest outside cell - * If starting cell is outside, the nearst inside cell - * If nearest cell is unknown, return nullptr - */ - const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const - { - const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z); - if (cell->distance_square_ > 0) - { - dist = sqrt_table_[cell->distance_square_]; - pos = cell->closest_point_; - const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); - return ncell == cell ? nullptr : ncell; - } - if (cell->negative_distance_square_ > 0) - { - dist = -sqrt_table_[cell->negative_distance_square_]; - pos = cell->closest_negative_point_; - const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); - return ncell == cell ? nullptr : ncell; - } - dist = 0.0; - pos.x() = x; - pos.y() = y; - pos.z() = z; - return nullptr; - } - - /** - * \brief Gets the maximum distance squared value. - * - * Produced by taking the ceiling of the maximum distance divided by - * the resolution, and then squaring that value. - * - * @return The maximum distance squared. - */ - int getMaximumDistanceSquared() const - { - return max_distance_sq_; - } - -private: - /** Typedef for set of integer indices */ - typedef std::set> VoxelSet; - /** - * \brief Initializes the field, resetting the voxel grid and - * building a sqrt lookup table for efficiency based on - * max_distance_. - * - */ - void initialize(); - - /** - * \brief Adds a valid set of integer points to the voxel grid - * - * @param voxel_points Valid set of voxel points for addition - */ - void addNewObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points); - - /** - * \brief Removes a valid set of integer points from the voxel grid - * - * @param voxel_points Valid set of voxel points for removal - */ - void removeObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points); - - /** - * \brief Propagates outward to the maximum distance given the - * contents of the \ref bucket_queue_, and clears the \ref - * bucket_queue_. - * - */ - void propagatePositive(); - - /** - * \brief Propagates inward to a maximum distance given the contents - * of the \ref negative_bucket_queue_, and clears the \ref - * negative_bucket_queue_. - * - */ - void propagateNegative(); - - /** - * \brief Determines distance based on actual voxel data - * - * @param object Actual voxel data - * - * @return The distance reported by the cell - */ - virtual double getDistance(const PropDistanceFieldVoxel& object) const; - - /** - * \brief Helper function to get a single number in a 27 connected - * 3D voxel grid given dx, dy, and dz values. - * - * @param dx The change in the X direction - * @param dy The change in the X direction - * @param dz The change in the Z direction - * - * @return Single number 0-26 representing direction - */ - int getDirectionNumber(int dx, int dy, int dz) const; - - /** - * \brief Helper function that gets change values given single - * number representing update direction. - * - * @param directionNumber Direction number 0-26 - * - * @return Integer changes - */ - Eigen::Vector3i getLocationDifference(int directionNumber) const; - - /** - * \brief Helper function for computing location and neighborhood - * information in 27 connected voxel grid. - * - */ - void initNeighborhoods(); - - /** - * \brief Debug function that prints all voxels in a set to ROS_DEBUG_NAMED - * - * @param set Voxel set to print - */ - void print(const VoxelSet& set); - - /** - * \brief Debug function that prints all points in a vector to ROS_DEBUG_NAMED - * - * @param points Points to print - */ - void print(const EigenSTL::vector_Vector3d& points); - - bool propagate_negative_; /**< \brief Whether or not to propagate negative distances */ - - VoxelGrid::Ptr voxel_grid_; /**< \brief Actual container for distance data */ - - /// \brief Structure used to hold propagation frontier - std::vector bucket_queue_; /**< \brief Data member that holds points from which to - propagate, where each vector holds points that are a - particular integer distance from the closest obstacle - points*/ - - std::vector negative_bucket_queue_; /**< \brief Data member that holds points from - which to propagate in the negative, where each - vector holds points that are a particular - integer distance from the closest unoccupied - points*/ - - double max_distance_; /**< \brief Holds maximum distance */ - int max_distance_sq_; /**< \brief Holds maximum distance squared in cells */ - - std::vector sqrt_table_; /**< \brief Precomputed square root table for faster distance lookups */ - - /** - * \brief Holds information on neighbor direction, with 27 different - * directions. Shows where to propagate given an integer distance - * and an update direction. - * - * [0] - for expansion of d=0 - * [1] - for expansion of d>=1 - * Under this, we have the 27 directions - * Then, a list of neighborhoods for each direction - * - */ - - std::vector> neighborhoods_; - - EigenSTL::vector_Vector3i direction_number_to_direction_; /**< \brief Holds conversion from direction number to - integer changes */ -}; - -////////////////////////// inline functions follow //////////////////////////////////////// - -inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared) - : distance_square_(distance_square), negative_distance_square_(negative_distance_squared) -{ - closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED; - closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED; - closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED; - closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED; - closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED; - closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED; -} - -inline PropDistanceFieldVoxel::PropDistanceFieldVoxel() -{ -} - -inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const -{ - return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_]; -} -} // namespace distance_field +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.hpp b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.hpp new file mode 100644 index 0000000000..96cb39eb51 --- /dev/null +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.hpp @@ -0,0 +1,608 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan, Ken Anderson */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace EigenSTL +{ +typedef std::vector> vector_Vector3i; +} + +namespace distance_field +{ +/** + * \brief Struct for sorting type Eigen::Vector3i for use in sorted + * std containers. Sorts in z order, then y order, then x order. + */ +struct CompareEigenVector3i +{ + bool operator()(const Eigen::Vector3i& loc_1, const Eigen::Vector3i& loc_2) const + { + if (loc_1.z() != loc_2.z()) + { + return (loc_1.z() < loc_2.z()); + } + else if (loc_1.y() != loc_2.y()) + { + return (loc_1.y() < loc_2.y()); + } + else if (loc_1.x() != loc_2.x()) + { + return (loc_1.x() < loc_2.x()); + } + return false; + } +}; + +/** + * \brief Structure that holds voxel information for the + * DistanceField. Will be used in VoxelGrid. + */ +struct PropDistanceFieldVoxel +{ + /** + * \brief Constructor. All fields left uninitialized. + * + * + */ + PropDistanceFieldVoxel(); + + /** + * \brief Constructor. Sets values for distance_sq_ and + * negative_distance_square_, and sets all remaining internal values + * to uninitialized. These should be integers values which + * represent the distance in cells squared. + * + * @param [in] distance_sq_positive Value to which to initialize + * distance_sq_ for distance to closest obstalce + * + * @param [in] distance_sq_negative Value to which to initialize + * distance_sq_negative_ for distance to nearest non-obstacle cell + * + */ + PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative); + + int distance_square_; /**< \brief Distance in cells to the closest obstacle, squared */ + int negative_distance_square_; /**< \brief Distance in cells to the nearest unoccupied cell, squared */ + Eigen::Vector3i closest_point_; /**< \brief Closest occupied cell */ + Eigen::Vector3i closest_negative_point_; /**< \brief Closest unoccupied cell */ + int update_direction_; /**< \brief Direction from which this voxel was updated for occupied distance propagation */ + int negative_update_direction_; /**< \brief Direction from which this voxel was updated for negative distance + propagation*/ + + static const int UNINITIALIZED = -1; /**< \brief Value that represents an uninitialized voxel */ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/** + * \brief A DistanceField implementation that uses a vector + * propagation method. Distances propagate outward from occupied + * cells, or inwards from unoccupied cells if negative distances are + * to be computed, which is optional. Outward and inward propagation + * only occur to a desired maximum distance - cells that are more than + * this maximum distance from the nearest cell will have maximum + * distance measurements. + * + * This class uses a \ref VoxelGrid to hold all data. One important + * decision that must be made on construction is whether or not to + * create a signed version of the distance field. If the distance + * field is unsigned, it means that the minimum obstacle distance is + * 0, a value that will be assigned to all obstacle cells. Gradient + * queries for obstacle cells will not give useful information, as the + * gradient at an obstacle cell will point to the cell itself. If + * this behavior is acceptable, then the performance of this mode will + * be more efficient, as no propagation will occur for obstacle cells. + * The other option is to calculate signed distances. In this case, + * negative distances up to the maximum distance are calculated for + * obstacle volumes. This distance encodes the distance of an + * obstacle cell to the nearest unoccupied obstacle voxel. Furthmore, + * gradients pointing out of the volume will be produced. Depending + * on the data, calculating this data can significantly impact the + * time it takes to add and remove obstacle cells. + */ +class PropagationDistanceField : public DistanceField +{ +public: + /** + * \brief Constructor that initializes entire distance field to + * empty - all cells will be assigned maximum distance values. All + * units are arbitrary but are assumed for documentation purposes to + * represent meters. + * + * @param [in] size_x The X dimension in meters of the volume to represent + * @param [in] size_y The Y dimension in meters of the volume to represent + * @param [in] size_z The Z dimension in meters of the volume to represent + * @param [in] resolution The resolution in meters of the volume + * @param [in] origin_x The minimum X point of the volume + * @param [in] origin_y The minimum Y point of the volume + * @param [in] origin_z The minimum Z point of the volume + + * @param [in] max_distance The maximum distance to which to + * propagate distance values. Cells that are greater than this + * distance will be assigned the maximum distance value. + * + * @param [in] propagate_negative_distances Whether or not to + * propagate negative distances. If false, no propagation occurs, + * and all obstacle cells will be assigned zero distance. See the + * \ref PropagationDistanceField description for more information on + * the implications of this. + * + */ + PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, + double origin_y, double origin_z, double max_distance, + bool propagate_negative_distances = false); + + /** + * \brief Constructor based on an OcTree and bounding box + * information. A distance field will be constructed with + * dimensions based on the supplied bounding box at the resolution + * of the OcTree. All octree obstacle cells will be added to the + * resulting distance field using the \ref DistanceField::addOcTreeToField + * function. + * + * @param [in] octree The OcTree from which to construct the distance field + * @param [in] bbx_min The minimum world coordinates of the bounding box + * @param [in] bbx_max The maximum world coordinates of the bounding box + * + * @param [in] max_distance The maximum distance to which to + * propagate distance values. Cells that are greater than this + * distance will be assigned the maximum distance value. + * + * @param [in] propagate_negative_distances Whether or not to + * propagate negative distances. If false, no propagation occurs, + * and all obstacle cells will be assigned zero distance. See the + * \ref PropagationDistanceField description for more information on + * the implications of this. + */ + PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min, + const octomap::point3d& bbx_max, double max_distance, + bool propagate_negative_distances = false); + + /** + * \brief Constructor that takes an istream and reads the contents + * of a saved distance field, adding all obstacle points and running + * propagation given the arguments for max_distance and + * propagate_negative_distances. Calls the function + * \ref readFromStream. + * + * @param [in] stream The stream from which to read the data + * + * @param [in] max_distance The maximum distance to which to + * propagate distance values. Cells that are greater than this + * distance will be assigned the maximum distance value. + * + * @param [in] propagate_negative_distances Whether or not to + * propagate negative distances. If false, no propagation occurs, + * and all obstacle cells will be assigned zero distance. See the + * \ref PropagationDistanceField description for more information on + * the implications of this. + * + * @return + */ + PropagationDistanceField(std::istream& stream, double max_distance, bool propagate_negative_distances = false); + /** + * \brief Empty destructor + * + * + */ + ~PropagationDistanceField() override + { + } + + /** + * \brief Add a set of obstacle points to the distance field, + * updating distance values accordingly. The distance field may + * already contain obstacle cells. + * + * The function first checks that each location represents a valid + * point - only valid points will be added. It takes the vector of + * valid points and performs positive propagation on them. If the + * class has been set up to propagate negative distance, those will + * also be propagated. + * + * @param [in] points The set of obstacle points to add + */ + void addPointsToField(const EigenSTL::vector_Vector3d& points) override; + + /** + * \brief Remove a set of obstacle points from the distance field, + * updating distance values accordingly. + * + * This function is relatively less efficient than adding points to + * the field in terms of positive distances - adding a given number + * of points will be less comptationally expensive than removing the + * same number of points. This is due to the nature of the + * propagation algorithm - when removing sets of cells, we must + * search outward from the freed cells and then propagate inward. + * Negative distances can be propagated more efficiently, as + * propagation can occur outward from newly freed cells without + * requiring a search step. If the set of occupied points that + * remain after removal is small it may be more efficient to call + * \ref reset and then to add the remaining points rather than + * removing a set of points. + * + * @param [in] points The set of obstacle points that will be set as free + */ + void removePointsFromField(const EigenSTL::vector_Vector3d& points) override; + + /** + * \brief This function will remove any obstacle points that are in + * the old point set but not the new point set, and add any obstacle + * points that are in the new block set but not the old block set. + * Any points that are in both sets are left unchanged. For more + * information see \ref DistanceField::updatePointsInField. + * + * The implementation of this function finds the set of points that + * are in the old_points and not the new_points, and the in the + * new_points and not the old_points using std::set_difference. It + * then calls a removal function on the former set, and an addition + * function on the latter set. + * + * If there is no overlap between the old_points and the new_points + * it is more efficient to first call \ref removePointsFromField on + * the old_points and then \ref addPointsToField on the new points - + * this does not require computing set differences. + * + * @param [in] old_points The set of points that all should be obstacle cells in the distance field + * @param [in] new_points The set of points, all of which are intended to be obstacle points in the distance field + * + */ + void updatePointsInField(const EigenSTL::vector_Vector3d& old_points, + const EigenSTL::vector_Vector3d& new_points) override; + + /** + * \brief Resets the entire distance field to max_distance for + * positive values and zero for negative values. + * + */ + void reset() override; + + /** + * \brief Get the distance value associated with the cell indicated + * by the world coordinate. If the cell is invalid, max_distance + * will be returned. If running without negative distances, all + * obstacle cells will have zero distance. If running with negative + * distances, the distance will be between -max_distance and + * max_distance, with no values having a 0 distance. + * + * + * @param [in] x The X location of the cell + * @param [in] y The X location of the cell + * @param [in] z The X location of the cell + * + * @return The distance value + */ + double getDistance(double x, double y, double z) const override; + + /** + * \brief Get the distance value associated with the cell indicated + * by the index coordinates. If the cell is invalid, max_distance + * will be returned. If running without negative distances, all + * obstacle cells will have zero distance. If running with negative + * distances, the distance will be between -max_distance and + * max_distance, with no values having a 0 distance. + * + * + * @param [in] x The integer X location + * @param [in] y The integer Y location + * @param [in] z The integer Z location + * + * @return The distance value for the cell + */ + double getDistance(int x, int y, int z) const override; + + bool isCellValid(int x, int y, int z) const override; + int getXNumCells() const override; + int getYNumCells() const override; + int getZNumCells() const override; + bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const override; + bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const override; + + /** + * \brief Writes the contents of the distance field to the supplied stream. + * + * This function writes the resolution, size, and origin parameters + * to the file in ASCII. It then writes the occupancy data only in + * bit form (with values or 1 representing occupancy, and 0 + * representing empty space). It further runs Zlib compression on + * the binary data before actually writing to disk. The + * max_distance and propagate_negative_distances values are not + * written to file, and the distances themselves will need to be + * recreated on load. + * + * @param [out] stream The stream to which to write the distance field contents. + * + * @return True + */ + bool writeToStream(std::ostream& stream) const override; + + /** + * \brief Reads, parameterizes, and populates the distance field + * based on the supplied stream. + * + * This function assumes that the file begins with ASCII data, and + * that the binary data has been written in bit formulation and + * compressed using Zlib. The function will reinitialize all data + * members based on the data in the file, using preset values for + * max_distance_ and propagate_negative_distances_. All occupied + * cells will be added to the distance field. + * + * @param [in] stream The stream from which to read + * + * @return True if reading, parameterizing, and populating the + * distance field is successful; otherwise False. + */ + bool readFromStream(std::istream& stream) override; + + // passthrough docs to DistanceField + double getUninitializedDistance() const override + { + return max_distance_; + } + + /** + * \brief Gets full cell data given an index. + * + * x,y,z MUST be valid or data corruption (SEGFAULTS) will occur. + * + * @param [in] x The integer X location + * @param [in] y The integer Y location + * @param [in] z The integer Z location + * + * @return The data in the indicated cell. + */ + const PropDistanceFieldVoxel& getCell(int x, int y, int z) const + { + return voxel_grid_->getCell(x, y, z); + } + + /** + * \brief Gets nearest surface cell and returns distance to it. + * + * x,y,z MUST be valid or data corruption (SEGFAULTS) will occur. + * + * @param [in] x The integer X location of the starting cell + * @param [in] y The integer Y location of the starting cell + * @param [in] z The integer Z location of the starting cell + * @param [out] dist if starting cell is inside, the negative distance to the nearest outside cell + * if starting cell is outside, the positive distance to the nearest inside cell + * if nearby cell is unknown, zero + * @param [out] pos the position of the nearest cell + * + * + * @return If starting cell is inside, the nearest outside cell + * If starting cell is outside, the nearst inside cell + * If nearest cell is unknown, return nullptr + */ + const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const + { + const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z); + if (cell->distance_square_ > 0) + { + dist = sqrt_table_[cell->distance_square_]; + pos = cell->closest_point_; + const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); + return ncell == cell ? nullptr : ncell; + } + if (cell->negative_distance_square_ > 0) + { + dist = -sqrt_table_[cell->negative_distance_square_]; + pos = cell->closest_negative_point_; + const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z()); + return ncell == cell ? nullptr : ncell; + } + dist = 0.0; + pos.x() = x; + pos.y() = y; + pos.z() = z; + return nullptr; + } + + /** + * \brief Gets the maximum distance squared value. + * + * Produced by taking the ceiling of the maximum distance divided by + * the resolution, and then squaring that value. + * + * @return The maximum distance squared. + */ + int getMaximumDistanceSquared() const + { + return max_distance_sq_; + } + +private: + /** Typedef for set of integer indices */ + typedef std::set> VoxelSet; + /** + * \brief Initializes the field, resetting the voxel grid and + * building a sqrt lookup table for efficiency based on + * max_distance_. + * + */ + void initialize(); + + /** + * \brief Adds a valid set of integer points to the voxel grid + * + * @param voxel_points Valid set of voxel points for addition + */ + void addNewObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points); + + /** + * \brief Removes a valid set of integer points from the voxel grid + * + * @param voxel_points Valid set of voxel points for removal + */ + void removeObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points); + + /** + * \brief Propagates outward to the maximum distance given the + * contents of the \ref bucket_queue_, and clears the \ref + * bucket_queue_. + * + */ + void propagatePositive(); + + /** + * \brief Propagates inward to a maximum distance given the contents + * of the \ref negative_bucket_queue_, and clears the \ref + * negative_bucket_queue_. + * + */ + void propagateNegative(); + + /** + * \brief Determines distance based on actual voxel data + * + * @param object Actual voxel data + * + * @return The distance reported by the cell + */ + virtual double getDistance(const PropDistanceFieldVoxel& object) const; + + /** + * \brief Helper function to get a single number in a 27 connected + * 3D voxel grid given dx, dy, and dz values. + * + * @param dx The change in the X direction + * @param dy The change in the X direction + * @param dz The change in the Z direction + * + * @return Single number 0-26 representing direction + */ + int getDirectionNumber(int dx, int dy, int dz) const; + + /** + * \brief Helper function that gets change values given single + * number representing update direction. + * + * @param directionNumber Direction number 0-26 + * + * @return Integer changes + */ + Eigen::Vector3i getLocationDifference(int directionNumber) const; + + /** + * \brief Helper function for computing location and neighborhood + * information in 27 connected voxel grid. + * + */ + void initNeighborhoods(); + + /** + * \brief Debug function that prints all voxels in a set to ROS_DEBUG_NAMED + * + * @param set Voxel set to print + */ + void print(const VoxelSet& set); + + /** + * \brief Debug function that prints all points in a vector to ROS_DEBUG_NAMED + * + * @param points Points to print + */ + void print(const EigenSTL::vector_Vector3d& points); + + bool propagate_negative_; /**< \brief Whether or not to propagate negative distances */ + + VoxelGrid::Ptr voxel_grid_; /**< \brief Actual container for distance data */ + + /// \brief Structure used to hold propagation frontier + std::vector bucket_queue_; /**< \brief Data member that holds points from which to + propagate, where each vector holds points that are a + particular integer distance from the closest obstacle + points*/ + + std::vector negative_bucket_queue_; /**< \brief Data member that holds points from + which to propagate in the negative, where each + vector holds points that are a particular + integer distance from the closest unoccupied + points*/ + + double max_distance_; /**< \brief Holds maximum distance */ + int max_distance_sq_; /**< \brief Holds maximum distance squared in cells */ + + std::vector sqrt_table_; /**< \brief Precomputed square root table for faster distance lookups */ + + /** + * \brief Holds information on neighbor direction, with 27 different + * directions. Shows where to propagate given an integer distance + * and an update direction. + * + * [0] - for expansion of d=0 + * [1] - for expansion of d>=1 + * Under this, we have the 27 directions + * Then, a list of neighborhoods for each direction + * + */ + + std::vector> neighborhoods_; + + EigenSTL::vector_Vector3i direction_number_to_direction_; /**< \brief Holds conversion from direction number to + integer changes */ +}; + +////////////////////////// inline functions follow //////////////////////////////////////// + +inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared) + : distance_square_(distance_square), negative_distance_square_(negative_distance_squared) +{ + closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED; + closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED; + closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED; + closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED; + closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED; + closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED; +} + +inline PropDistanceFieldVoxel::PropDistanceFieldVoxel() +{ +} + +inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const +{ + return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_]; +} +} // namespace distance_field diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h index c441333d18..3fffa6cb19 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,537 +47,5 @@ /* Author: Mrinal Kalakrishnan, Acorn Pooley */ #pragma once - -#include -#include -#include -#include - -namespace distance_field -{ -/// \brief Specifies dimension of different axes -enum Dimension -{ - DIM_X = 0, - DIM_Y = 1, - DIM_Z = 2 -}; - -/** - * \brief VoxelGrid holds a dense 3D, axis-aligned set of data at a - * given resolution, where the data is supplied as a template - * parameter. - * - */ -template -class VoxelGrid -{ -public: - MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid); - - /** - * \brief Constructor for the VoxelGrid. - * - * Constructs a dense representation of a 3D, axis-aligned volume at - * a given resolution. The volume can be represented in any - * consistent set of units, but for the sake of documentation we - * assume that the units are meters. The size of the the volume is - * given along each of the X, Y, and Z axes. The volume begins at - * the minimum point in each dimension, as specified by the origin - * parameters. The data structure will remain uninitialized until - * the \ref VoxelGrid::reset function is called. - * - * @param [in] size_x Size of the X axis in meters - * @param [in] size_y Size of the Y axis in meters - * @param [in] size_z Size of the Z axis in meters - * - * @param [in] resolution Resolution of a single cell in meters - * - * @param [in] origin_x Minimum point along the X axis of the volume - * @param [in] origin_y Minimum point along the Y axis of the volume - * @param [in] origin_z Minimum point along the Z axis of the volume - * - * @param [in] default_object An object that will be returned for any - * future queries that are not valid - */ - VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, - double origin_z, T default_object); - virtual ~VoxelGrid(); - - /** - * \brief Default constructor for the VoxelGrid. - * - * This is only useful if resize() is called after construction to set the - * size and resolution of the VoxelGrid. - */ - VoxelGrid(); - - /** - * \brief Resize the VoxelGrid. - * - * This discards all the data in the voxel grid and reinitializes - * it with a new size and resolution. This is mainly useful if the size or - * resolution is not known until after the voxelgrid is constructed. - * - * @param [in] size_x Size of the X axis in meters - * @param [in] size_y Size of the Y axis in meters - * @param [in] size_z Size of the Z axis in meters - * - * @param [in] resolution Resolution of a single cell in meters - * - * @param [in] origin_x Minimum point along the X axis of the volume - * @param [in] origin_y Minimum point along the Y axis of the volume - * @param [in] origin_z Minimum point along the Z axis of the volume - */ - void resize(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, - double origin_z, T default_object); - - /** - * \brief Operator that gets the value of the given location (x, y, - * z) given the discretization of the volume. The location - * represents a location in the original coordinate frame used to - * construct the voxel grid. - * - * @param [in] x X component of the desired location - * @param [in] y Y component of the desired location - * @param [in] z Z component of the desired location - * - * @return The data stored at that location, or a default value - * supplied in the constructor if the location is not valid. - */ - const T& operator()(double x, double y, double z) const; - const T& operator()(const Eigen::Vector3d& pos) const; - - /** - * \brief Gives the value of the given location (x,y,z) in the - * discretized voxel grid space. - * - * The address here is in the discretized space of the voxel grid, - * where the cell indicated by the constructor arguments (origin_x, - * origin_y, origin_z) is cell (0,0,0), and the cell indicated by - * (origin_x+x_size, origin_y+y_size, origin_z+z_size) will be - * (size_x/resolution, size_y/resolution, size_z/resolution). - * - * @param [in] x The X index of the desired cell - * @param [in] y The Y index of the desired cell - * @param [in] z The Z index of the desired cell - * - * @return The data in the indicated cell. If x,y,z is invalid then - * corruption and/or SEGFAULTS will occur. - */ - T& getCell(int x, int y, int z); - T& getCell(const Eigen::Vector3i& pos); - const T& getCell(int x, int y, int z) const; - const T& getCell(const Eigen::Vector3i& pos) const; - - /** - * \brief Sets the value of the given location (x,y,z) in the - * discretized voxel grid space to supplied value. - * - * The address here is in the discretized space of the voxel grid, - * where the cell indicated by the constructor arguments (origin_x, - * origin_y, origin_z) is cell (0,0,0), and the cell indicated by - * (origin_x+x_size, origin_y+y_size, origin_z+z_size) will be - * (size_x/resolution, size_y/resolution, size_z/resolution). - * - * If the arguments do not indicate a valid cell, corruption and/or SEGFAULTS - * will occur. - * - * @param [in] x The X index of the desired cell - * @param [in] y The Y index of the desired cell - * @param [in] z The Z index of the desired cell - * @param [out] obj The data to place into the given cell - */ - void setCell(int x, int y, int z, const T& obj); - void setCell(const Eigen::Vector3i& pos, const T& obj); - - /** - * \brief Sets every cell in the voxel grid to the supplied data - * - * @param [in] initial The template variable to which to set the data - */ - void reset(const T& initial); - - /** - * \brief Gets the size in arbitrary units of the indicated dimension - * - * @param [in] dim The dimension for the query - * - * @return The size in meters - */ - double getSize(Dimension dim) const; - - /** - * \brief Gets the resolution in arbitrary consistent units - * - * @return The resolution in meters - */ - double getResolution() const; - - /** - * \brief Gets the origin (minimum point) of the indicated dimension - * - * @param [in] dim The dimension for the query - * - * @return The indicated axis origin - */ - double getOrigin(Dimension dim) const; - - /** - * \brief Gets the number of cells in the indicated dimension - * - * @param [in] dim The dimension for the query - * - * @return The number of cells for the indicated dimension - */ - int getNumCells(Dimension dim) const; - - /** - * \brief Converts grid coordinates to world coordinates. - */ - /** - * \brief Converts from an set of integer indices to a world - * location given the origin and resolution parameters. There is no - * check whether or not the cell or world locations lie within the - * represented region. - * - * @param [in] x The integer X location - * @param [in] y The integer Y location - * @param [in] z The integer Z location - * @param [out] world_x The computed world X location - * @param [out] world_y The computed world X location - * @param [out] world_z The computed world X location - * - * @return True, as there is no check that the integer locations are valid - */ - void gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const; - void gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const; - - /** - * \brief Converts from a world location to a set of integer - * indices. Does check whether or not the cell being returned is - * valid. The returned indices will be computed even if they are - * invalid. - * - * @param [in] world_x The world X location - * @param [in] world_y The world Y location - * @param [in] world_z The world Z location - * @param [out] x The computed integer X location - * @param [out] y The computed integer X location - * @param [out] z The computed integer X location - * - * @return True if all the world values result in integer indices - * that pass a validity check; otherwise False. - */ - bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const; - bool worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const; - - /** - * \brief Checks if the given cell in integer coordinates is within the voxel grid - * - * @param [in] x The integer X location - * @param [in] y The integer Y location - * @param [in] z The integer Z location - * - * @return True if the cell lies within the voxel grid; otherwise False. - */ - bool isCellValid(int x, int y, int z) const; - bool isCellValid(const Eigen::Vector3i& pos) const; - - /** - * \brief Checks if the indicated index is valid along a particular dimension. - * - * @param [in] dim The dimension for the query - * @param [in] cell The index along that dimension - * - * @return True if the cell is valid along that dimension; otherwise False. - */ - bool isCellValid(Dimension dim, int cell) const; - -protected: - T* data_; /**< \brief Storage for the full set of data elements */ - T default_object_; /**< \brief The default object to return in case of out-of-bounds query */ - T*** data_ptrs_; /**< \brief 3D array of pointers to the data elements */ - double size_[3]; /**< \brief The size of each dimension in meters (in Dimension order) */ - double resolution_; /**< \brief The resolution of each dimension in meters (in Dimension order) */ - double oo_resolution_; /**< \brief 1.0/resolution_ */ - double origin_[3]; /**< \brief The origin (minimum point) of each dimension in meters (in Dimension order) */ - double origin_minus_[3]; /**< \brief origin - 0.5/resolution */ - int num_cells_[3]; /**< \brief The number of cells in each dimension (in Dimension order) */ - int num_cells_total_; /**< \brief The total number of voxels in the grid */ - int stride1_; /**< \brief The step to take when stepping between consecutive X members in the 1D array */ - int stride2_; /**< \brief The step to take when stepping between consecutive Y members given an X in the 1D array */ - - /** - * \brief Gets the 1D index into the array, with no validity check. - * - * @param [in] x The integer X location - * @param [in] y The integer Y location - * @param [in] z The integer Z location - * - * @return The computed 1D index - */ - int ref(int x, int y, int z) const; - - /** - * \brief Gets the cell number from the location - */ - /** - * \brief Gets the cell number in a given dimension given a world - * value. No validity check. - * - * @param [in] dim The dimension of the query - * @param [in] loc The world location along that dimension - * - * @return The computed cell index along the given dimension - */ - int getCellFromLocation(Dimension dim, double loc) const; - - /** - * \brief Gets the center of the cell in world coordinates along the - * given dimension. No validity check. - * - * @param [in] dim The dimension of the query - * @param [in] cell The cell along the given dimension - * - * @return The world coordinate of the center of the cell - */ - double getLocationFromCell(Dimension dim, int cell) const; -}; - -//////////////////////////// template function definitions follow ////////////////// - -template -VoxelGrid::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, - double origin_y, double origin_z, T default_object) - : data_(nullptr) -{ - resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object); -} - -template -VoxelGrid::VoxelGrid() : data_(nullptr) -{ - for (int i = DIM_X; i <= DIM_Z; ++i) - { - size_[i] = 0; - origin_[i] = 0; - origin_minus_[i] = 0; - num_cells_[i] = 0; - } - resolution_ = 1.0; - oo_resolution_ = 1.0 / resolution_; - num_cells_total_ = 0; - stride1_ = 0; - stride2_ = 0; -} - -template -void VoxelGrid::resize(double size_x, double size_y, double size_z, double resolution, double origin_x, - double origin_y, double origin_z, T default_object) -{ - delete[] data_; - data_ = nullptr; - - size_[DIM_X] = size_x; - size_[DIM_Y] = size_y; - size_[DIM_Z] = size_z; - origin_[DIM_X] = origin_x; - origin_[DIM_Y] = origin_y; - origin_[DIM_Z] = origin_z; - origin_minus_[DIM_X] = origin_x - 0.5 * resolution; - origin_minus_[DIM_Y] = origin_y - 0.5 * resolution; - origin_minus_[DIM_Z] = origin_z - 0.5 * resolution; - num_cells_total_ = 1; - resolution_ = resolution; - oo_resolution_ = 1.0 / resolution_; - for (int i = DIM_X; i <= DIM_Z; ++i) - { - num_cells_[i] = size_[i] * oo_resolution_; - num_cells_total_ *= num_cells_[i]; - } - - default_object_ = default_object; - - stride1_ = num_cells_[DIM_Y] * num_cells_[DIM_Z]; - stride2_ = num_cells_[DIM_Z]; - - // initialize the data: - if (num_cells_total_ > 0) - data_ = new T[num_cells_total_]; -} - -template -VoxelGrid::~VoxelGrid() -{ - delete[] data_; -} - -template -inline bool VoxelGrid::isCellValid(int x, int y, int z) const -{ - return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]); -} - -template -inline bool VoxelGrid::isCellValid(const Eigen::Vector3i& pos) const -{ - return isCellValid(pos.x(), pos.y(), pos.z()); -} - -template -inline bool VoxelGrid::isCellValid(Dimension dim, int cell) const -{ - return cell >= 0 && cell < num_cells_[dim]; -} - -template -inline int VoxelGrid::ref(int x, int y, int z) const -{ - return x * stride1_ + y * stride2_ + z; -} - -template -inline double VoxelGrid::getSize(Dimension dim) const -{ - return size_[dim]; -} - -template -inline double VoxelGrid::getResolution() const -{ - return resolution_; -} - -template -inline double VoxelGrid::getOrigin(Dimension dim) const -{ - return origin_[dim]; -} - -template -inline int VoxelGrid::getNumCells(Dimension dim) const -{ - return num_cells_[dim]; -} - -template -inline const T& VoxelGrid::operator()(double x, double y, double z) const -{ - int cell_x = getCellFromLocation(DIM_X, x); - int cell_y = getCellFromLocation(DIM_Y, y); - int cell_z = getCellFromLocation(DIM_Z, z); - if (!isCellValid(cell_x, cell_y, cell_z)) - return default_object_; - return getCell(cell_x, cell_y, cell_z); -} - -template -inline const T& VoxelGrid::operator()(const Eigen::Vector3d& pos) const -{ - return operator()(pos.x(), pos.y(), pos.z()); -} - -template -inline T& VoxelGrid::getCell(int x, int y, int z) -{ - return data_[ref(x, y, z)]; -} - -template -inline const T& VoxelGrid::getCell(int x, int y, int z) const -{ - return data_[ref(x, y, z)]; -} - -template -inline T& VoxelGrid::getCell(const Eigen::Vector3i& pos) -{ - return data_[ref(pos.x(), pos.y(), pos.z())]; -} - -template -inline const T& VoxelGrid::getCell(const Eigen::Vector3i& pos) const -{ - return data_[ref(pos.x(), pos.y(), pos.z())]; -} - -template -inline void VoxelGrid::setCell(int x, int y, int z, const T& obj) -{ - data_[ref(x, y, z)] = obj; -} - -template -inline void VoxelGrid::setCell(const Eigen::Vector3i& pos, const T& obj) -{ - data_[ref(pos.x(), pos.y(), pos.z())] = obj; -} - -template -inline int VoxelGrid::getCellFromLocation(Dimension dim, double loc) const -{ - // This implements - // - // ( loc - origin ) - // floor( ------------ + 0.5 ) - // ( resolution ) - // - // In other words, the rounded quantized location. - // - // For speed implemented like this: - // - // floor( ( loc - origin_minus ) * oo_resolution ) - // - // where origin_minus = origin - 0.5*resolution - // - return int(floor((loc - origin_minus_[dim]) * oo_resolution_)); -} - -template -inline double VoxelGrid::getLocationFromCell(Dimension dim, int cell) const -{ - return origin_[dim] + resolution_ * (double(cell)); -} - -template -inline void VoxelGrid::reset(const T& initial) -{ - std::fill(data_, data_ + num_cells_total_, initial); -} - -template -inline void VoxelGrid::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const -{ - world_x = getLocationFromCell(DIM_X, x); - world_y = getLocationFromCell(DIM_Y, y); - world_z = getLocationFromCell(DIM_Z, z); -} - -template -inline void VoxelGrid::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const -{ - world.x() = getLocationFromCell(DIM_X, grid.x()); - world.y() = getLocationFromCell(DIM_Y, grid.y()); - world.z() = getLocationFromCell(DIM_Z, grid.z()); -} - -template -inline bool VoxelGrid::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const -{ - x = getCellFromLocation(DIM_X, world_x); - y = getCellFromLocation(DIM_Y, world_y); - z = getCellFromLocation(DIM_Z, world_z); - return isCellValid(x, y, z); -} - -template -inline bool VoxelGrid::worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const -{ - grid.x() = getCellFromLocation(DIM_X, world.x()); - grid.y() = getCellFromLocation(DIM_Y, world.y()); - grid.z() = getCellFromLocation(DIM_Z, world.z()); - return isCellValid(grid.x(), grid.y(), grid.z()); -} - -} // namespace distance_field +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.hpp b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.hpp new file mode 100644 index 0000000000..b58f2804c1 --- /dev/null +++ b/moveit_core/distance_field/include/moveit/distance_field/voxel_grid.hpp @@ -0,0 +1,571 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan, Acorn Pooley */ + +#pragma once + +#include +#include +#include +#include + +namespace distance_field +{ +/// \brief Specifies dimension of different axes +enum Dimension +{ + DIM_X = 0, + DIM_Y = 1, + DIM_Z = 2 +}; + +/** + * \brief VoxelGrid holds a dense 3D, axis-aligned set of data at a + * given resolution, where the data is supplied as a template + * parameter. + * + */ +template +class VoxelGrid +{ +public: + MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid); + + /** + * \brief Constructor for the VoxelGrid. + * + * Constructs a dense representation of a 3D, axis-aligned volume at + * a given resolution. The volume can be represented in any + * consistent set of units, but for the sake of documentation we + * assume that the units are meters. The size of the the volume is + * given along each of the X, Y, and Z axes. The volume begins at + * the minimum point in each dimension, as specified by the origin + * parameters. The data structure will remain uninitialized until + * the \ref VoxelGrid::reset function is called. + * + * @param [in] size_x Size of the X axis in meters + * @param [in] size_y Size of the Y axis in meters + * @param [in] size_z Size of the Z axis in meters + * + * @param [in] resolution Resolution of a single cell in meters + * + * @param [in] origin_x Minimum point along the X axis of the volume + * @param [in] origin_y Minimum point along the Y axis of the volume + * @param [in] origin_z Minimum point along the Z axis of the volume + * + * @param [in] default_object An object that will be returned for any + * future queries that are not valid + */ + VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, + double origin_z, T default_object); + virtual ~VoxelGrid(); + + /** + * \brief Default constructor for the VoxelGrid. + * + * This is only useful if resize() is called after construction to set the + * size and resolution of the VoxelGrid. + */ + VoxelGrid(); + + /** + * \brief Resize the VoxelGrid. + * + * This discards all the data in the voxel grid and reinitializes + * it with a new size and resolution. This is mainly useful if the size or + * resolution is not known until after the voxelgrid is constructed. + * + * @param [in] size_x Size of the X axis in meters + * @param [in] size_y Size of the Y axis in meters + * @param [in] size_z Size of the Z axis in meters + * + * @param [in] resolution Resolution of a single cell in meters + * + * @param [in] origin_x Minimum point along the X axis of the volume + * @param [in] origin_y Minimum point along the Y axis of the volume + * @param [in] origin_z Minimum point along the Z axis of the volume + */ + void resize(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, + double origin_z, T default_object); + + /** + * \brief Operator that gets the value of the given location (x, y, + * z) given the discretization of the volume. The location + * represents a location in the original coordinate frame used to + * construct the voxel grid. + * + * @param [in] x X component of the desired location + * @param [in] y Y component of the desired location + * @param [in] z Z component of the desired location + * + * @return The data stored at that location, or a default value + * supplied in the constructor if the location is not valid. + */ + const T& operator()(double x, double y, double z) const; + const T& operator()(const Eigen::Vector3d& pos) const; + + /** + * \brief Gives the value of the given location (x,y,z) in the + * discretized voxel grid space. + * + * The address here is in the discretized space of the voxel grid, + * where the cell indicated by the constructor arguments (origin_x, + * origin_y, origin_z) is cell (0,0,0), and the cell indicated by + * (origin_x+x_size, origin_y+y_size, origin_z+z_size) will be + * (size_x/resolution, size_y/resolution, size_z/resolution). + * + * @param [in] x The X index of the desired cell + * @param [in] y The Y index of the desired cell + * @param [in] z The Z index of the desired cell + * + * @return The data in the indicated cell. If x,y,z is invalid then + * corruption and/or SEGFAULTS will occur. + */ + T& getCell(int x, int y, int z); + T& getCell(const Eigen::Vector3i& pos); + const T& getCell(int x, int y, int z) const; + const T& getCell(const Eigen::Vector3i& pos) const; + + /** + * \brief Sets the value of the given location (x,y,z) in the + * discretized voxel grid space to supplied value. + * + * The address here is in the discretized space of the voxel grid, + * where the cell indicated by the constructor arguments (origin_x, + * origin_y, origin_z) is cell (0,0,0), and the cell indicated by + * (origin_x+x_size, origin_y+y_size, origin_z+z_size) will be + * (size_x/resolution, size_y/resolution, size_z/resolution). + * + * If the arguments do not indicate a valid cell, corruption and/or SEGFAULTS + * will occur. + * + * @param [in] x The X index of the desired cell + * @param [in] y The Y index of the desired cell + * @param [in] z The Z index of the desired cell + * @param [out] obj The data to place into the given cell + */ + void setCell(int x, int y, int z, const T& obj); + void setCell(const Eigen::Vector3i& pos, const T& obj); + + /** + * \brief Sets every cell in the voxel grid to the supplied data + * + * @param [in] initial The template variable to which to set the data + */ + void reset(const T& initial); + + /** + * \brief Gets the size in arbitrary units of the indicated dimension + * + * @param [in] dim The dimension for the query + * + * @return The size in meters + */ + double getSize(Dimension dim) const; + + /** + * \brief Gets the resolution in arbitrary consistent units + * + * @return The resolution in meters + */ + double getResolution() const; + + /** + * \brief Gets the origin (minimum point) of the indicated dimension + * + * @param [in] dim The dimension for the query + * + * @return The indicated axis origin + */ + double getOrigin(Dimension dim) const; + + /** + * \brief Gets the number of cells in the indicated dimension + * + * @param [in] dim The dimension for the query + * + * @return The number of cells for the indicated dimension + */ + int getNumCells(Dimension dim) const; + + /** + * \brief Converts grid coordinates to world coordinates. + */ + /** + * \brief Converts from an set of integer indices to a world + * location given the origin and resolution parameters. There is no + * check whether or not the cell or world locations lie within the + * represented region. + * + * @param [in] x The integer X location + * @param [in] y The integer Y location + * @param [in] z The integer Z location + * @param [out] world_x The computed world X location + * @param [out] world_y The computed world X location + * @param [out] world_z The computed world X location + * + * @return True, as there is no check that the integer locations are valid + */ + void gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const; + void gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const; + + /** + * \brief Converts from a world location to a set of integer + * indices. Does check whether or not the cell being returned is + * valid. The returned indices will be computed even if they are + * invalid. + * + * @param [in] world_x The world X location + * @param [in] world_y The world Y location + * @param [in] world_z The world Z location + * @param [out] x The computed integer X location + * @param [out] y The computed integer X location + * @param [out] z The computed integer X location + * + * @return True if all the world values result in integer indices + * that pass a validity check; otherwise False. + */ + bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const; + bool worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const; + + /** + * \brief Checks if the given cell in integer coordinates is within the voxel grid + * + * @param [in] x The integer X location + * @param [in] y The integer Y location + * @param [in] z The integer Z location + * + * @return True if the cell lies within the voxel grid; otherwise False. + */ + bool isCellValid(int x, int y, int z) const; + bool isCellValid(const Eigen::Vector3i& pos) const; + + /** + * \brief Checks if the indicated index is valid along a particular dimension. + * + * @param [in] dim The dimension for the query + * @param [in] cell The index along that dimension + * + * @return True if the cell is valid along that dimension; otherwise False. + */ + bool isCellValid(Dimension dim, int cell) const; + +protected: + T* data_; /**< \brief Storage for the full set of data elements */ + T default_object_; /**< \brief The default object to return in case of out-of-bounds query */ + T*** data_ptrs_; /**< \brief 3D array of pointers to the data elements */ + double size_[3]; /**< \brief The size of each dimension in meters (in Dimension order) */ + double resolution_; /**< \brief The resolution of each dimension in meters (in Dimension order) */ + double oo_resolution_; /**< \brief 1.0/resolution_ */ + double origin_[3]; /**< \brief The origin (minimum point) of each dimension in meters (in Dimension order) */ + double origin_minus_[3]; /**< \brief origin - 0.5/resolution */ + int num_cells_[3]; /**< \brief The number of cells in each dimension (in Dimension order) */ + int num_cells_total_; /**< \brief The total number of voxels in the grid */ + int stride1_; /**< \brief The step to take when stepping between consecutive X members in the 1D array */ + int stride2_; /**< \brief The step to take when stepping between consecutive Y members given an X in the 1D array */ + + /** + * \brief Gets the 1D index into the array, with no validity check. + * + * @param [in] x The integer X location + * @param [in] y The integer Y location + * @param [in] z The integer Z location + * + * @return The computed 1D index + */ + int ref(int x, int y, int z) const; + + /** + * \brief Gets the cell number from the location + */ + /** + * \brief Gets the cell number in a given dimension given a world + * value. No validity check. + * + * @param [in] dim The dimension of the query + * @param [in] loc The world location along that dimension + * + * @return The computed cell index along the given dimension + */ + int getCellFromLocation(Dimension dim, double loc) const; + + /** + * \brief Gets the center of the cell in world coordinates along the + * given dimension. No validity check. + * + * @param [in] dim The dimension of the query + * @param [in] cell The cell along the given dimension + * + * @return The world coordinate of the center of the cell + */ + double getLocationFromCell(Dimension dim, int cell) const; +}; + +//////////////////////////// template function definitions follow ////////////////// + +template +VoxelGrid::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, + double origin_y, double origin_z, T default_object) + : data_(nullptr) +{ + resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object); +} + +template +VoxelGrid::VoxelGrid() : data_(nullptr) +{ + for (int i = DIM_X; i <= DIM_Z; ++i) + { + size_[i] = 0; + origin_[i] = 0; + origin_minus_[i] = 0; + num_cells_[i] = 0; + } + resolution_ = 1.0; + oo_resolution_ = 1.0 / resolution_; + num_cells_total_ = 0; + stride1_ = 0; + stride2_ = 0; +} + +template +void VoxelGrid::resize(double size_x, double size_y, double size_z, double resolution, double origin_x, + double origin_y, double origin_z, T default_object) +{ + delete[] data_; + data_ = nullptr; + + size_[DIM_X] = size_x; + size_[DIM_Y] = size_y; + size_[DIM_Z] = size_z; + origin_[DIM_X] = origin_x; + origin_[DIM_Y] = origin_y; + origin_[DIM_Z] = origin_z; + origin_minus_[DIM_X] = origin_x - 0.5 * resolution; + origin_minus_[DIM_Y] = origin_y - 0.5 * resolution; + origin_minus_[DIM_Z] = origin_z - 0.5 * resolution; + num_cells_total_ = 1; + resolution_ = resolution; + oo_resolution_ = 1.0 / resolution_; + for (int i = DIM_X; i <= DIM_Z; ++i) + { + num_cells_[i] = size_[i] * oo_resolution_; + num_cells_total_ *= num_cells_[i]; + } + + default_object_ = default_object; + + stride1_ = num_cells_[DIM_Y] * num_cells_[DIM_Z]; + stride2_ = num_cells_[DIM_Z]; + + // initialize the data: + if (num_cells_total_ > 0) + data_ = new T[num_cells_total_]; +} + +template +VoxelGrid::~VoxelGrid() +{ + delete[] data_; +} + +template +inline bool VoxelGrid::isCellValid(int x, int y, int z) const +{ + return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]); +} + +template +inline bool VoxelGrid::isCellValid(const Eigen::Vector3i& pos) const +{ + return isCellValid(pos.x(), pos.y(), pos.z()); +} + +template +inline bool VoxelGrid::isCellValid(Dimension dim, int cell) const +{ + return cell >= 0 && cell < num_cells_[dim]; +} + +template +inline int VoxelGrid::ref(int x, int y, int z) const +{ + return x * stride1_ + y * stride2_ + z; +} + +template +inline double VoxelGrid::getSize(Dimension dim) const +{ + return size_[dim]; +} + +template +inline double VoxelGrid::getResolution() const +{ + return resolution_; +} + +template +inline double VoxelGrid::getOrigin(Dimension dim) const +{ + return origin_[dim]; +} + +template +inline int VoxelGrid::getNumCells(Dimension dim) const +{ + return num_cells_[dim]; +} + +template +inline const T& VoxelGrid::operator()(double x, double y, double z) const +{ + int cell_x = getCellFromLocation(DIM_X, x); + int cell_y = getCellFromLocation(DIM_Y, y); + int cell_z = getCellFromLocation(DIM_Z, z); + if (!isCellValid(cell_x, cell_y, cell_z)) + return default_object_; + return getCell(cell_x, cell_y, cell_z); +} + +template +inline const T& VoxelGrid::operator()(const Eigen::Vector3d& pos) const +{ + return operator()(pos.x(), pos.y(), pos.z()); +} + +template +inline T& VoxelGrid::getCell(int x, int y, int z) +{ + return data_[ref(x, y, z)]; +} + +template +inline const T& VoxelGrid::getCell(int x, int y, int z) const +{ + return data_[ref(x, y, z)]; +} + +template +inline T& VoxelGrid::getCell(const Eigen::Vector3i& pos) +{ + return data_[ref(pos.x(), pos.y(), pos.z())]; +} + +template +inline const T& VoxelGrid::getCell(const Eigen::Vector3i& pos) const +{ + return data_[ref(pos.x(), pos.y(), pos.z())]; +} + +template +inline void VoxelGrid::setCell(int x, int y, int z, const T& obj) +{ + data_[ref(x, y, z)] = obj; +} + +template +inline void VoxelGrid::setCell(const Eigen::Vector3i& pos, const T& obj) +{ + data_[ref(pos.x(), pos.y(), pos.z())] = obj; +} + +template +inline int VoxelGrid::getCellFromLocation(Dimension dim, double loc) const +{ + // This implements + // + // ( loc - origin ) + // floor( ------------ + 0.5 ) + // ( resolution ) + // + // In other words, the rounded quantized location. + // + // For speed implemented like this: + // + // floor( ( loc - origin_minus ) * oo_resolution ) + // + // where origin_minus = origin - 0.5*resolution + // + return int(floor((loc - origin_minus_[dim]) * oo_resolution_)); +} + +template +inline double VoxelGrid::getLocationFromCell(Dimension dim, int cell) const +{ + return origin_[dim] + resolution_ * (double(cell)); +} + +template +inline void VoxelGrid::reset(const T& initial) +{ + std::fill(data_, data_ + num_cells_total_, initial); +} + +template +inline void VoxelGrid::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const +{ + world_x = getLocationFromCell(DIM_X, x); + world_y = getLocationFromCell(DIM_Y, y); + world_z = getLocationFromCell(DIM_Z, z); +} + +template +inline void VoxelGrid::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const +{ + world.x() = getLocationFromCell(DIM_X, grid.x()); + world.y() = getLocationFromCell(DIM_Y, grid.y()); + world.z() = getLocationFromCell(DIM_Z, grid.z()); +} + +template +inline bool VoxelGrid::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const +{ + x = getCellFromLocation(DIM_X, world_x); + y = getCellFromLocation(DIM_Y, world_y); + z = getCellFromLocation(DIM_Z, world_z); + return isCellValid(x, y, z); +} + +template +inline bool VoxelGrid::worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const +{ + grid.x() = getCellFromLocation(DIM_X, world.x()); + grid.y() = getCellFromLocation(DIM_Y, world.y()); + grid.z() = getCellFromLocation(DIM_Z, world.z()); + return isCellValid(grid.x(), grid.y(), grid.z()); +} + +} // namespace distance_field diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index c619f95d51..6e4d44bebd 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -34,8 +34,8 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/distance_field/src/find_internal_points.cpp b/moveit_core/distance_field/src/find_internal_points.cpp index 1a4da45820..b3ba81241a 100644 --- a/moveit_core/distance_field/src/find_internal_points.cpp +++ b/moveit_core/distance_field/src/find_internal_points.cpp @@ -34,7 +34,7 @@ /* Author: Acorn Pooley */ -#include +#include void distance_field::findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points) diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index 6b70134120..b088922a31 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -34,7 +34,7 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson */ -#include +#include #include #include #include diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index 62cbfe6d63..37e392bbf1 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -36,9 +36,9 @@ #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_core/distance_field/test/test_voxel_grid.cpp b/moveit_core/distance_field/test/test_voxel_grid.cpp index cdd24250e9..9930c94b6c 100644 --- a/moveit_core/distance_field/test/test_voxel_grid.cpp +++ b/moveit_core/distance_field/test/test_voxel_grid.cpp @@ -36,7 +36,7 @@ #include -#include +#include #include using namespace distance_field; diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 048c937428..3cfb314a36 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,117 +47,5 @@ /* Author: Sachin Chitta */ #pragma once - -// KDL -#include -#include - -#include -#include -#include -#include - -/** \brief This namespace includes the dynamics_solver library */ -namespace dynamics_solver -{ -MOVEIT_CLASS_FORWARD(DynamicsSolver); // Defines DynamicsSolverPtr, ConstPtr, WeakPtr... etc - -/** - * This solver currently computes the required torques given a - * joint configuration, velocities, accelerations and external wrenches - * acting on the links of a robot - */ -class DynamicsSolver -{ -public: - /** - * @brief Initialize the dynamics solver - * @param urdf_model The urdf model for the robot - * @param srdf_model The srdf model for the robot - * @param group_name The name of the group to compute stuff for - * @return False if initialization failed - */ - DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, - const geometry_msgs::msg::Vector3& gravity_vector); - - /** - * @brief Get the torques (the order of all input and output is the same - * as the order of joints for this group in the RobotModel) - * @param joint_angles The joint angles (desired joint configuration) - * this must have size = number of joints in the group - * @param joint_velocities The desired joint velocities - * this must have size = number of joints in the group - * @param joint_accelerations The desired joint accelerations - * this must have size = number of joints in the group - * @param wrenches External wrenches acting on the links of the robot - * this must have size = number of links in the group - * @param torques Computed set of torques are filled in here - * this must have size = number of joints in the group - * @return False if any of the input vectors are of the wrong size - */ - bool getTorques(const std::vector& joint_angles, const std::vector& joint_velocities, - const std::vector& joint_accelerations, - const std::vector& wrenches, std::vector& torques) const; - - /** - * @brief Get the maximum payload for this group (in kg). Payload is - * the weight that this group can hold when the weight is attached to the origin - * of the last link of this group. (The order of joint_angles vector is the same - * as the order of joints for this group in the RobotModel) - * @param joint_angles The joint angles (desired joint configuration) - * this must have size = number of joints in the group - * @param payload The computed maximum payload - * @param joint_saturated The first saturated joint and the maximum payload - * @return False if the input set of joint angles is of the wrong size - */ - bool getMaxPayload(const std::vector& joint_angles, double& payload, unsigned int& joint_saturated) const; - - /** - * @brief Get torques corresponding to a particular payload value. Payload is - * the weight that this group can hold when the weight is attached to the origin - * of the last link of this group. - * @param joint_angles The joint angles (desired joint configuration) - * this must have size = number of joints in the group - * @param payload The payload for which to compute torques (in kg) - * @param joint_torques The resulting joint torques - * @return False if the input vectors are of the wrong size - */ - bool getPayloadTorques(const std::vector& joint_angles, double payload, - std::vector& joint_torques) const; - - /** - * @brief Get maximum torques for this group - * @return Vector of max torques - */ - const std::vector& getMaxTorques() const; - - /** - * @brief Get the kinematic model - * @return kinematic model - */ - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - const moveit::core::JointModelGroup* getGroup() const - { - return joint_model_group_; - } - -private: - std::shared_ptr chain_id_solver_; // KDL chain inverse dynamics - KDL::Chain kdl_chain_; // KDL chain - - moveit::core::RobotModelConstPtr robot_model_; - const moveit::core::JointModelGroup* joint_model_group_; - - moveit::core::RobotStatePtr state_; // robot state - - std::string base_name_, tip_name_; // base name, tip name - unsigned int num_joints_, num_segments_; // number of joints in group, number of segments in group - std::vector max_torques_; // vector of max torques - - double gravity_; // Norm of the gravity vector passed in initialize() -}; -} // namespace dynamics_solver +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.hpp b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.hpp new file mode 100644 index 0000000000..c3df5dae43 --- /dev/null +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.hpp @@ -0,0 +1,151 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Sachin Chitta */ + +#pragma once + +// KDL +#include +#include + +#include +#include +#include +#include + +/** \brief This namespace includes the dynamics_solver library */ +namespace dynamics_solver +{ +MOVEIT_CLASS_FORWARD(DynamicsSolver); // Defines DynamicsSolverPtr, ConstPtr, WeakPtr... etc + +/** + * This solver currently computes the required torques given a + * joint configuration, velocities, accelerations and external wrenches + * acting on the links of a robot + */ +class DynamicsSolver +{ +public: + /** + * @brief Initialize the dynamics solver + * @param urdf_model The urdf model for the robot + * @param srdf_model The srdf model for the robot + * @param group_name The name of the group to compute stuff for + * @return False if initialization failed + */ + DynamicsSolver(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const geometry_msgs::msg::Vector3& gravity_vector); + + /** + * @brief Get the torques (the order of all input and output is the same + * as the order of joints for this group in the RobotModel) + * @param joint_angles The joint angles (desired joint configuration) + * this must have size = number of joints in the group + * @param joint_velocities The desired joint velocities + * this must have size = number of joints in the group + * @param joint_accelerations The desired joint accelerations + * this must have size = number of joints in the group + * @param wrenches External wrenches acting on the links of the robot + * this must have size = number of links in the group + * @param torques Computed set of torques are filled in here + * this must have size = number of joints in the group + * @return False if any of the input vectors are of the wrong size + */ + bool getTorques(const std::vector& joint_angles, const std::vector& joint_velocities, + const std::vector& joint_accelerations, + const std::vector& wrenches, std::vector& torques) const; + + /** + * @brief Get the maximum payload for this group (in kg). Payload is + * the weight that this group can hold when the weight is attached to the origin + * of the last link of this group. (The order of joint_angles vector is the same + * as the order of joints for this group in the RobotModel) + * @param joint_angles The joint angles (desired joint configuration) + * this must have size = number of joints in the group + * @param payload The computed maximum payload + * @param joint_saturated The first saturated joint and the maximum payload + * @return False if the input set of joint angles is of the wrong size + */ + bool getMaxPayload(const std::vector& joint_angles, double& payload, unsigned int& joint_saturated) const; + + /** + * @brief Get torques corresponding to a particular payload value. Payload is + * the weight that this group can hold when the weight is attached to the origin + * of the last link of this group. + * @param joint_angles The joint angles (desired joint configuration) + * this must have size = number of joints in the group + * @param payload The payload for which to compute torques (in kg) + * @param joint_torques The resulting joint torques + * @return False if the input vectors are of the wrong size + */ + bool getPayloadTorques(const std::vector& joint_angles, double payload, + std::vector& joint_torques) const; + + /** + * @brief Get maximum torques for this group + * @return Vector of max torques + */ + const std::vector& getMaxTorques() const; + + /** + * @brief Get the kinematic model + * @return kinematic model + */ + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + const moveit::core::JointModelGroup* getGroup() const + { + return joint_model_group_; + } + +private: + std::shared_ptr chain_id_solver_; // KDL chain inverse dynamics + KDL::Chain kdl_chain_; // KDL chain + + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + + moveit::core::RobotStatePtr state_; // robot state + + std::string base_name_, tip_name_; // base name, tip name + unsigned int num_joints_, num_segments_; // number of joints in group, number of segments in group + std::vector max_torques_; // vector of max torques + + double gravity_; // Norm of the gravity vector passed in initialize() +}; +} // namespace dynamics_solver diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 9c39863869..edf4e27fcb 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -34,7 +34,7 @@ /* Author: Sachin Chitta */ -#include +#include // KDL #include #include diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index 1e4d28ddb9..5011a823c0 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,23 +47,5 @@ /* Author: Acorn Pooley, Ioan Sucan */ #pragma once - -#include - -/** \brief Main namespace for MoveIt */ -namespace moveit -{ -/** \brief This may be thrown during construction of objects if errors occur */ -class ConstructException : public std::runtime_error -{ -public: - explicit ConstructException(const std::string& what_arg); -}; - -/** \brief This may be thrown if unrecoverable errors occur */ -class Exception : public std::runtime_error -{ -public: - explicit Exception(const std::string& what_arg); -}; -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.hpp b/moveit_core/exceptions/include/moveit/exceptions/exceptions.hpp new file mode 100644 index 0000000000..1e4d28ddb9 --- /dev/null +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Acorn Pooley, Ioan Sucan */ + +#pragma once + +#include + +/** \brief Main namespace for MoveIt */ +namespace moveit +{ +/** \brief This may be thrown during construction of objects if errors occur */ +class ConstructException : public std::runtime_error +{ +public: + explicit ConstructException(const std::string& what_arg); +}; + +/** \brief This may be thrown if unrecoverable errors occur */ +class Exception : public std::runtime_error +{ +public: + explicit Exception(const std::string& what_arg); +}; +} // namespace moveit diff --git a/moveit_core/exceptions/src/exceptions.cpp b/moveit_core/exceptions/src/exceptions.cpp index 05cc9bc7ae..2a58c6ab18 100644 --- a/moveit_core/exceptions/src/exceptions.cpp +++ b/moveit_core/exceptions/src/exceptions.cpp @@ -34,7 +34,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 5684d55fcf..fb78e1c6ce 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,1073 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -/** \brief Representation and evaluation of kinematic constraints */ -namespace kinematic_constraints -{ -/// \brief Struct for containing the results of constraint evaluation -struct ConstraintEvaluationResult -{ - /** - * \brief Constructor - * - * @param [in] result_satisfied True if the constraint evaluated to true, otherwise false - * @param [in] dist The distance evaluated by the constraint - * - * @return - */ - ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0) - : satisfied(result_satisfied), distance(dist) - { - } - - bool satisfied; /**< \brief Whether or not the constraint or constraints were satisfied */ - double distance; /**< \brief The distance evaluation from the constraint or constraints */ -}; - -MOVEIT_CLASS_FORWARD(KinematicConstraint); // Defines KinematicConstraintPtr, ConstPtr, WeakPtr... etc - -/// \brief Base class for representing a kinematic constraint -class KinematicConstraint -{ -public: - /// \brief Enum for representing a constraint - enum ConstraintType - { - UNKNOWN_CONSTRAINT, - JOINT_CONSTRAINT, - POSITION_CONSTRAINT, - ORIENTATION_CONSTRAINT, - VISIBILITY_CONSTRAINT - }; - - /** - * \brief Constructor - * - * @param [in] model The kinematic model used for constraint evaluation - */ - KinematicConstraint(const moveit::core::RobotModelConstPtr& model); - virtual ~KinematicConstraint(); - - /** \brief Clear the stored constraint */ - virtual void clear() = 0; - - /** - * \brief Decide whether the constraint is satisfied in the indicated state - * - * @param [in] state The kinematic state used for evaluation - * @param [in] verbose Whether or not to print output - * - * @return - */ - virtual ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const = 0; - - /** \brief This function returns true if this constraint is - configured and able to decide whether states do meet the - constraint or not. If this function returns false it means - that decide() will always return true -- there is no - constraint to be checked. */ - virtual bool enabled() const = 0; - - /** - * \brief Check if two constraints are the same. This means that - * the types are the same, the subject of the constraint is the - * same, and all values associated with the constraint are within a - * margin. The other constraint must also be enabled. - * - * @param [in] other The other constraint to test - * @param [in] margin The margin to apply to all values associated with constraint - * - * @return True if equal, otherwise false - */ - virtual bool equal(const KinematicConstraint& other, double margin) const = 0; - - /** - * \brief Get the type of constraint - * - * - * @return The constraint type - */ - ConstraintType getType() const - { - return type_; - } - - /** - * \brief Print the constraint data - * - * @param [in] out The file descriptor for printing - */ - virtual void print(std::ostream& /*unused*/ = std::cout) const - { - } - - /** - * - * \brief The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() - *function - * - * @return The constraint weight - */ - double getConstraintWeight() const - { - return constraint_weight_; - } - - /** - * - * - * - * @return The kinematic model associated with this constraint - */ - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - -protected: - ConstraintType type_; /**< \brief The type of the constraint */ - moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model associated with this constraint */ - double constraint_weight_; /**< \brief The weight of a constraint is a multiplicative factor associated to the - distance computed by the decide() function */ -}; - -MOVEIT_CLASS_FORWARD(JointConstraint); // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc - -/** - * \brief Class for handling single DOF joint constraints. - * - * This class handles single DOF constraints expressed as a tolerance - * above and below a target position. Multi-DOF joints can be - * accommodated by using local name formulations - i.e. for a planar - * joint specifying a constraint in terms of "planar_joint_name"/x. - * - * Continuous revolute single DOF joints will be evaluated based on - * wrapping around 3.14 and -3.14. Tolerances above and below will be - * evaluating over the wrap. For instance, if the constraint value is - * 3.14 and the tolerance above is .04, a value of -3.14 is in bounds, - * as is a value of -3.12. -3.1 is out of bounds. Similarly, if the - * value of the constraint is -3.14, the tolerance above is .04, and - * the tolerance below is .02 then -3.1 is a valid value, as is 3.14; - * 3.1 is out of bounds. - * - * Type will be JOINT_CONSTRAINT. - */ -class JointConstraint : public KinematicConstraint -{ -public: - /** - * \brief Constructor - * - * @param [in] model The kinematic model used for constraint evaluation - */ - JointConstraint(const moveit::core::RobotModelConstPtr& model) - : KinematicConstraint(model), joint_model_(nullptr), joint_variable_index_(-1) - { - type_ = JOINT_CONSTRAINT; - } - - /** - * \brief Configure the constraint based on a - * moveit_msgs::msg::JointConstraint - * - * For the configure command to be successful, the joint must exist - * in the kinematic model, the joint must not be a multi-DOF joint - * (for these joints, local variables should be used), and the - * tolerance values must be positive. - * - * @param [in] jc JointConstraint for configuration - * - * @return True if constraint can be configured from jc - */ - bool configure(const moveit_msgs::msg::JointConstraint& jc); - - /** - * \brief Check if two joint constraints are the same. - * - * This means that the types are the same, the subject of the - * constraint is the same, and all values associated with the - * constraint are within a margin. The other constraint must also - * be enabled. For this to be true of joint constraints, they must - * act on the same joint, and the position and tolerance values must - * be within the margins. - * - * @param [in] other The other constraint to test - * @param [in] margin The margin to apply to all values associated with constraint - * - * @return True if equal, otherwise false - */ - bool equal(const KinematicConstraint& other, double margin) const override; - - ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; - bool enabled() const override; - void clear() override; - void print(std::ostream& out = std::cout) const override; - - /** - * \brief Get the joint model for which this constraint operates - * - * @return The relevant joint model if enabled, and otherwise nullptr - */ - const moveit::core::JointModel* getJointModel() const - { - return joint_model_; - } - /** - * \brief Gets the local variable name if this constraint was - * configured for a part of a multi-DOF joint - * - * - * @return The component of the joint name after the slash, or the - * empty string if there is no local variable name - */ - const std::string& getLocalVariableName() const - { - return local_variable_name_; - } - - /** - * \brief Gets the joint variable name, as known to the moveit::core::RobotModel - * - * This will include the local variable name if a variable of a multi-DOF joint is constrained. - * - * @return The joint variable name - */ - const std::string& getJointVariableName() const - { - return joint_variable_name_; - } - - int getJointVariableIndex() const - { - return joint_variable_index_; - } - - /** - * \brief Gets the desired position component of the constraint - * - * - * @return The desired joint position - */ - double getDesiredJointPosition() const - { - return joint_position_; - } - - /** - * \brief Gets the upper tolerance component of the joint constraint - * - * - * @return The above joint tolerance - */ - double getJointToleranceAbove() const - { - return joint_tolerance_above_; - } - - /** - * \brief Gets the lower tolerance component of the joint constraint - * - * - * @return The below joint tolerance - */ - double getJointToleranceBelow() const - { - return joint_tolerance_below_; - } - -protected: - const moveit::core::JointModel* joint_model_; /**< \brief The joint from the kinematic model for this constraint */ - bool joint_is_continuous_; /**< \brief Whether or not the joint is continuous */ - std::string local_variable_name_; /**< \brief The local variable name for a multi DOF joint, if any */ - std::string joint_variable_name_; /**< \brief The joint variable name */ - int joint_variable_index_; /**< \brief The index of the joint variable name in the full robot state */ - double joint_position_, joint_tolerance_above_, joint_tolerance_below_; /**< \brief Position and tolerance values*/ -}; - -MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc - -/** - * \brief Class for constraints on the orientation of a link - * - * This class expresses an orientation constraint on a particular link. - * The constraint specifies a target orientation via a quaternion as well as - * tolerances on X,Y, and Z rotation axes. - * The rotation difference between the target and actual link orientation is expressed - * either as XYZ Euler angles or as a rotation vector (depending on the `parameterization` type). - * The latter is highly recommended, because it supports resolution of subframes and attached bodies. - * Also, rotation vector representation allows to interpret the tolerances always w.r.t. the given reference frame. - * Euler angles are much more restricted and exhibit singularities. - * - * For efficiency, if the target orientation is expressed w.r.t. to a fixed frame (relative to the planning frame), - * some stuff is precomputed. However, mobile reference frames are supported as well. - * - * The type value will return ORIENTATION_CONSTRAINT. - * - */ -class OrientationConstraint : public KinematicConstraint -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -public: - /** - * \brief Constructor - * - * @param [in] model The kinematic model used for constraint evaluation - */ - OrientationConstraint(const moveit::core::RobotModelConstPtr& model) - : KinematicConstraint(model), link_model_(nullptr) - { - type_ = ORIENTATION_CONSTRAINT; - } - - /** - * \brief Configure the constraint based on a - * moveit_msgs::msg::OrientationConstraint - * - * For the configure command to be successful, the link must exist - * in the kinematic model. Note that if the absolute tolerance - * values are left as 0.0 only values less than a very small epsilon - * will evaluate to satisfied. - * - * @param [in] oc OrientationConstraint for configuration - * - * @return True if constraint can be configured from oc - */ - bool configure(const moveit_msgs::msg::OrientationConstraint& oc, const moveit::core::Transforms& tf); - - /** - * \brief Check if two orientation constraints are the same. - - * This means that the types are the same, the subject of the - * constraint is the same, and all values associated with the - * constraint are within a margin. The other constraint must also - * be enabled. For this to be true of orientation constraints: - * \li The link must be the same - * \li The rotations specified by the quaternions must be within the margin - * \li The tolerances must all be within the margin - * - * @param [in] other The other constraint to test - * @param [in] margin The margin to apply to all values associated with constraint - * - * @return True if equal, otherwise false - */ - bool equal(const KinematicConstraint& other, double margin) const override; - - void clear() override; - ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; - bool enabled() const override; - void print(std::ostream& out = std::cout) const override; - - /** - * \brief Gets the subject link model - * - * - * @return Returns the current link model - */ - const moveit::core::LinkModel* getLinkModel() const - { - return link_model_; - } - - /** - * \brief The target frame of the planning_models::Transforms class, - * for interpreting the rotation frame. - * - * @return The reference frame. - */ - const std::string& getReferenceFrame() const - { - return desired_rotation_frame_id_; - } - - /** - * \brief Whether or not a mobile reference frame is being employed. - * - * @return True if a mobile reference frame is being employed, and - * otherwise false. - */ - bool mobileReferenceFrame() const - { - return mobile_frame_; - } - - /** - * \brief The rotation target in the reference frame. - * - * @return The target rotation. - * - * The returned matrix is always a valid rotation matrix. - */ - const Eigen::Matrix3d& getDesiredRotationMatrixInRefFrame() const - { - // validity of the rotation matrix is enforced in configure() - return desired_R_in_frame_id_; - } - - /** - * \brief The rotation target in the reference or tf frame. - * - * @return The target rotation. - * - * The returned matrix is always a valid rotation matrix. - */ - const Eigen::Matrix3d& getDesiredRotationMatrix() const - { - // validity of the rotation matrix is enforced in configure() - return desired_rotation_matrix_; - } - - /** - * \brief Gets the X axis tolerance - * - * - * @return The X axis tolerance - */ - double getXAxisTolerance() const - { - return absolute_x_axis_tolerance_; - } - - /** - * \brief Gets the Y axis tolerance - * - * - * @return The Y axis tolerance - */ - double getYAxisTolerance() const - { - return absolute_y_axis_tolerance_; - } - - /** - * \brief Gets the Z axis tolerance - * - * - * @return The Z axis tolerance - */ - double getZAxisTolerance() const - { - return absolute_z_axis_tolerance_; - } - - int getParameterizationType() const - { - return parameterization_type_; - } - -protected: - const moveit::core::LinkModel* link_model_; /**< The target link model */ - Eigen::Matrix3d desired_R_in_frame_id_; /**< Desired rotation matrix in frame_id */ - Eigen::Matrix3d desired_rotation_matrix_; /**< The desired rotation matrix in the tf frame */ - Eigen::Matrix3d desired_rotation_matrix_inv_; /**< The inverse of desired_rotation_matrix_ (for efficiency) */ - std::string desired_rotation_frame_id_; /**< The target frame of the transform tree */ - bool mobile_frame_; /**< Whether or not the header frame is mobile or fixed */ - int parameterization_type_; /**< Parameterization type for orientation tolerance */ - double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, - absolute_z_axis_tolerance_; /**< Storage for the tolerances */ -}; - -MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc - -/** - * \brief Class for constraints on the XYZ position of a link - * - * This class expresses X,Y,Z position constraints of a link. The - * position area is specified as a bounding volume consisting of one - * or more shapes - either solid primitives or meshes. The pose - * information in the volumes will be interpreted by using the header - * information. The header may either specify a fixed frame or a - * mobile frame. Additionally, a target offset specified in the frame - * of the link being constrained can be specified. The type value - * will return POSITION_CONSTRAINT. - * - */ -class PositionConstraint : public KinematicConstraint -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -public: - /** - * \brief Constructor - * - * @param [in] model The kinematic model used for constraint evaluation - */ - PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr) - { - type_ = POSITION_CONSTRAINT; - } - - /** - * \brief Configure the constraint based on a - * moveit_msgs::msg::PositionConstraint - * - * For the configure command to be successful, the link must be - * specified in the model, and one or more constrained regions must - * be correctly specified, which requires containing a valid shape - * and a pose for that shape. If the header frame on the constraint - * is empty, the constraint will fail to configure. If an invalid - * quaternion is passed for a shape, the identity quaternion will be - * substituted. - * - * @param [in] pc moveit_msgs::msg::PositionConstraint for configuration - * - * @return True if constraint can be configured from pc - */ - bool configure(const moveit_msgs::msg::PositionConstraint& pc, const moveit::core::Transforms& tf); - - /** - * \brief Check if two constraints are the same. For position - * constraints this means that: - * \li The types are the same - * \li The link model is the same - * \li The frame of the constraints are the same - * \li The target offsets are no more than the margin apart - * \li Each entry in the constraint region of this constraint matches a region in the other constraint - * \li Each entry in the other constraint region matches a region in the other constraint - * - * Two constraint regions matching each other means that: - * \li The poses match within the margin - * \li The types are the same - * \li The shape volumes are within the margin - * - * Note that the two shapes can have different numbers of regions as - * long as all regions are matched up to another. - * - * @param [in] other The other constraint to test - * @param [in] margin The margin to apply to all values associated with constraint - * - * @return True if equal, otherwise false - */ - bool equal(const KinematicConstraint& other, double margin) const override; - - void clear() override; - ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; - bool enabled() const override; - void print(std::ostream& out = std::cout) const override; - - /** - * \brief Returns the associated link model, or nullptr if not enabled - * - * - * @return The link model - */ - const moveit::core::LinkModel* getLinkModel() const - { - return link_model_; - } - - /** - * \brief Returns the target offset - * - * - * @return The target offset - */ - const Eigen::Vector3d& getLinkOffset() const - { - return offset_; - } - - /** - * \brief If the constraint is enabled and the link offset is - * substantially different than zero - * - * - * @return Whether or not there is a link offset - */ - bool hasLinkOffset() const - { - if (!enabled()) - return false; - return has_offset_; - } - - /** - * \brief Returns all the constraint regions - * - * - * @return The constraint regions - */ - const std::vector& getConstraintRegions() const - { - return constraint_region_; - } - - /** - * \brief Returns the reference frame - * - * - * @return The reference frame - */ - const std::string& getReferenceFrame() const - { - return constraint_frame_id_; - } - - /** - * \brief If enabled and the specified frame is a mobile frame, - * return true. Otherwise, returns false. - * - * - * @return Whether a mobile reference frame is being employed - */ - bool mobileReferenceFrame() const - { - if (!enabled()) - return false; - return mobile_frame_; - } - -protected: - Eigen::Vector3d offset_; /**< \brief The target offset */ - bool has_offset_; /**< \brief Whether the offset is substantially different than 0.0 */ - std::vector constraint_region_; /**< \brief The constraint region vector */ - /** \brief The constraint region pose vector. All isometries are guaranteed to be valid. */ - EigenSTL::vector_Isometry3d constraint_region_pose_; - bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ - std::string constraint_frame_id_; /**< \brief The constraint frame id */ - const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ -}; - -MOVEIT_CLASS_FORWARD(VisibilityConstraint); // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc - -/** - * \brief Class for constraints on the visibility relationship between - * a sensor and a target. - * - * Visibility constraints are the most complicated kinematic - * constraint type. They are designed to test whether a visibility - * relationship is maintained in a given state. For the visibility - * relationship to be maintained, a sensor must have an unimpeded view - * of a target - this is useful, for instance, if one wants to test - * whether a robot can see its hand with a given sensor in a given - * state. The mechanism that is used to test the constraint is a - * combination of geometric checks, and then a collision check that - * tests whether a cone that connects the sensor and the target is - * entirely unobstructed by the robot's links. - * - * The constraint consists of a sensor pose, a target pose, a few - * parameters that govern the shape of the target, and a few - * parameters that allow finer control over the geometry of the check. - * There are two general ways that this constraint can be used. The - * first way to leave max_view_angle and max_range_angle in the - * VisibilityConstraint message as 0.0. In this case, the only check - * that is performed is the collision check against the cone. This - * check assumes that the sensor can see in all directions - the - * orientation of the sensor has no effect on the creation of the - * cone. It is important to note that the sensor and the target poses - * must not result in collision - if they are associated with sensor - * frames or robot links they must be specified to be outside the - * robot's body or all states will violate the Visibility Constraint. - * For an example of a visualization of a visibility constraint - * (produced using the getMarkers member function), see this image: - * - * \image html fingertip_cone.png "Visibility constraint satisfied as cone volume is clear" - * - * The cone is shown in red, and the arrows show normals. This - * visibility constraint uses a sensor pose (the narrow end of the - * cone ) of the PR2's narrow_stereo_optical_frame, except shifted - * along the Z axis such that the pose is outside of the robot's head - * and doesn't collide. The cone is allowed to collide with the - * actual sensor associated with the header frame, just not with any - * other links. The target pose is a 5 cm circle offset from the - * l_gripper_r_finger_tip_link, again so as not to collide - again, - * the cone can collide with the target link, but now with any other - * links. In the indicated state the cone is collision free and thus - * satisfies the collision component of the visibility constraint. In - * this image, however, the right arm intersects the cone, violating - * the visibility constraint: - * - * \image html fingertip_collision.png "Visibility constraint violated as right arm is within the cone" - * - * Note that both the target and the sensor frame can change with the - * robot's state. - * - * The collision check essentially asks whether or not the volume - * between the sensor pose and the target pose are clear, but that's - * only one aspect of visibility we may care about. The visibility - * constraint also allows for some geometric reasoning about the - * relationship between the sensor or the target - this allows - * information such as approximate field of view of the sensor to be - * factored into the constraint. To use this reasoning, the - * sensor_view_direction of the field should first be set - this - * specifies which axis of the sensor pose frame is actually pointing. - * The system assumes that the the sensor pose has been set up such - * that a single of the axes is pointing directly out of the sensor. - * Once this value is set correctly, one or both of the max_view_angle - * and the max_range_angle values can be set. Setting a positive - * max_view_angle will constrain the sensor along the specified axis - * and the target along its Z axis to be pointing at each other. - * Practically speaking, this ensures that the sensor has sufficient - * visibility to the front of the target - if the target is pointing - * the opposite direction, or is too steeply perpendicular to the - * target, then the max_view_angle part of the constraint will be - * violated. The getMarkers function can again help explain this - - * the view angle is the angular difference between the blue arrow - * associated with the sensor and the red arrow associated with the - * target. - - * \image html exact_opposites.png "Max view angle is evaluated at 0.0" - * \image html fourty_five.png "Max view angle evaluates around pi/4" - * \image html perpendicular.png "Max view angle evaluates at pi/2, the maximum" - * \image html other_side.png "Sensor pointed at wrong side of target, will violate constraint as long as max_view_angle - > 0.0" - * - * If constraining the target to be within the field of view of the - * sensor is the goal, then the max_range_angle can be set. The range - * angle is calculated as the angle between the origins of the sensor - * and the target frame - the orientations are unimportant. In the - * above images, the range angle is always 0, as the target's center - * is directly lined up with the blue arrow. In this image, however, - * the view angle is evaluated at 0.0, while the range angle is - * evaluated at .65. - * - * \image html range_angle.png "Range angle is high, so only sensors with wide fields of view would see the target" - * - * By limiting the max_range_angle, you can constrain the target to be - * within the field of view of the sensor. Max_view_angle and - * max_range_angle can be used at once. - */ -class VisibilityConstraint : public KinematicConstraint -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -public: - /** - * \brief Constructor - * - * @param [in] model The kinematic model used for constraint evaluation - */ - VisibilityConstraint(const moveit::core::RobotModelConstPtr& model); - - /** - * \brief Configure the constraint based on a - * moveit_msgs::msg::VisibilityConstraint - * - * For the configure command to be successful, one of the three possible - * constraint criteria must be set to a non-zero value: - * - The target radius (negative values will have the absolute value taken). - * - The range angle. - * - The view angle. - * - * If cone sides are less than 3, a value of 3 will be used. - * - * @param [in] vc moveit_msgs::msg::VisibilityConstraint for configuration - * - * @return True if constraint can be configured from vc - */ - bool configure(const moveit_msgs::msg::VisibilityConstraint& vc, const moveit::core::Transforms& tf); - - /** - * \brief Check if two constraints are the same. - * - * For visibility constraints this means that: - * \li The types are the same, - * \li The target frame ids are the same - * \li The sensor frame ids are the same - * \li The cone sides number is the same - * \li The sensor view directions are the same - * \li The max view angles and target radii are within the margin - * \li The sensor and target poses are within the margin, as computed by taking the norm of the difference. - * - * @param [in] other The other constraint to test - * @param [in] margin The margin to apply to all values associated with constraint - * - * @return True if equal, otherwise false - */ - bool equal(const KinematicConstraint& other, double margin) const override; - void clear() override; - - /** - * \brief Gets a trimesh shape representing the visibility cone - * - * @param [in] tform_world_to_sensor Transform from the world to the sensor frame - * @param [in] tform_world_to_target Transform from the world to the target frame - * - * @return The shape associated with the cone - */ - shapes::Mesh* getVisibilityCone(const Eigen::Isometry3d& tform_world_to_sensor, - const Eigen::Isometry3d& tform_world_to_target) const; - - /** - * \brief Adds markers associated with the visibility cone, sensor - * and target to the visualization array - * - * The visibility cone and two arrows - a blue array that issues - * from the sensor_view_direction of the sensor, and a red arrow the - * issues along the Z axis of the the target frame. - * - * @param [in] state The state from which to produce the markers - * @param [out] markers The marker array to which the markers will be added - */ - void getMarkers(const moveit::core::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const; - - bool enabled() const override; - ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; - void print(std::ostream& out = std::cout) const override; - -protected: - /** - * \brief Function that gets passed into collision checking to allow some collisions. - * - * The cone object is allowed to touch either the sensor or the header frame, but not anything else - * - * @param [in] contact The contact in question - * - * @return True if the collision is allowed, otherwise false - */ - bool decideContact(const collision_detection::Contact& contact) const; - - moveit::core::RobotModelConstPtr robot_model_; /**< \brief A copy of the robot model used to create collision - environments to check the cone against robot links */ - - std::string target_frame_id_; /**< \brief The target frame id */ - std::string sensor_frame_id_; /**< \brief The sensor frame id */ - Eigen::Isometry3d sensor_pose_; /**< \brief The sensor pose transformed into the transform frame */ - int sensor_view_direction_; /**< \brief Storage for the sensor view direction */ - Eigen::Isometry3d target_pose_; /**< \brief The target pose transformed into the transform frame */ - unsigned int cone_sides_; /**< \brief Storage for the cone sides */ - EigenSTL::vector_Vector3d points_; /**< \brief A set of points along the base of the circle */ - double target_radius_; /**< \brief Storage for the target radius */ - double max_view_angle_; /**< \brief Storage for the max view angle */ - double max_range_angle_; /**< \brief Storage for the max range angle */ -}; - -MOVEIT_CLASS_FORWARD(KinematicConstraintSet); // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc - -/** - * \brief A class that contains many different constraints, and can - * check RobotState *versus the full set. A set is satisfied if - * and only if all constraints are satisfied. - * - * The set may contain any number of different kinds of constraints. - * All constraints, including invalid ones, are stored internally. - */ -class KinematicConstraintSet -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -public: - /** - * \brief Constructor - * - * @param [in] model The kinematic model used for constraint evaluation - */ - KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model) - { - } - - ~KinematicConstraintSet() - { - clear(); - } - - /** \brief Clear the stored constraints */ - void clear(); - - /** - * \brief Add all known constraints - * - * @param [in] c A message potentially contain vectors of constraints of add types - * - * @return Whether or not all constraints could be successfully - * configured given the contents of the message. The - * KinematicConstraintSet can still be used even if the addition - * returns false. - */ - bool add(const moveit_msgs::msg::Constraints& c, const moveit::core::Transforms& tf); - - /** - * \brief Add a vector of joint constraints - * - * @param [in] jc A vector of joint constraints - * - * @return Will return true only if all constraints are valid, and false otherwise - */ - bool add(const std::vector& jc); - - /** - * \brief Add a vector of position constraints - * - * @param [in] pc A vector of position constraints - * - * @return Will return true only if all constraints are valid, and false otherwise - */ - bool add(const std::vector& pc, const moveit::core::Transforms& tf); - - /** - * \brief Add a vector of orientation constraints - * - * @param [in] oc A vector of orientation constraints - * - * @return Will return true only if all constraints are valid, and false otherwise - */ - bool add(const std::vector& oc, const moveit::core::Transforms& tf); - - /** - * \brief Add a vector of visibility constraints - * - * @param [in] vc A vector of visibility constraints - * - * @return Will return true only if all constraints are valid, and false otherwise - */ - bool add(const std::vector& vc, const moveit::core::Transforms& tf); - - /** - * \brief Determines whether all constraints are satisfied by state, - * returning a single evaluation result - * - * @param [in] state The state to test - * @param [in] verbose Whether or not to make each constraint give debug output - * - * @return A single constraint evaluation result, where it will - * report satisfied only if all constraints are satisfied, and with - * a distance that is the sum of all individual distances. - */ - ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const; - - /** - * - * \brief Determines whether all constraints are satisfied by state, - * returning a vector of results through a parameter in addition to - * a summed result. - * - * @param [in] state The state to test - * - * @param [out] results The individual results from constraint - * evaluation on each constraint contained in the set. - * - * @param [in] verbose Whether to print the results of each constraint - * check. - * - * @return A single constraint evaluation result, where it will - * report satisfied only if all constraints are satisfied, and with - * a distance that is the sum of all individual distances. - */ - ConstraintEvaluationResult decide(const moveit::core::RobotState& state, - std::vector& results, bool verbose = false) const; - - /** - * \brief Whether or not another KinematicConstraintSet is equal to - * this one. - * - * Equality means that for each constraint in this set there is a - * constraint in the other set for which equal() is true with the - * given margin. Multiple constraints in this set can be matched to - * single constraints in the other set. Some constraints in the - * other set may not be matched to constraints in this set. - * - * @param [in] other The other set against which to test - * @param [in] margin The margin to apply to all individual constraint equality tests - * - * @return True if all constraints are matched, false otherwise - */ - bool equal(const KinematicConstraintSet& other, double margin) const; - - /** - * \brief Print the constraint data - * - * @param [in] out The file stream for printing - */ - void print(std::ostream& out = std::cout) const; - - /** - * \brief Get all position constraints in the set - * - * - * @return All position constraints - */ - const std::vector& getPositionConstraints() const - { - return position_constraints_; - } - - /** - * \brief Get all orientation constraints in the set - * - * - * @return All orientation constraints - */ - const std::vector& getOrientationConstraints() const - { - return orientation_constraints_; - } - - /** - * \brief Get all joint constraints in the set - * - * - * @return All joint constraints - */ - const std::vector& getJointConstraints() const - { - return joint_constraints_; - } - - /** - * \brief Get all visibility constraints in the set - * - * - * @return All visibility constraints - */ - const std::vector& getVisibilityConstraints() const - { - return visibility_constraints_; - } - - /** - * \brief Get all constraints in the set - * - * - * @return All constraints in a single message - */ - const moveit_msgs::msg::Constraints& getAllConstraints() const - { - return all_constraints_; - } - - /** - * \brief Returns whether or not there are any constraints in the set - * - * - * @return True if there are no constraints, otherwise false. - */ - bool empty() const - { - return kinematic_constraints_.empty(); - } - -protected: - moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model used for by the Set */ - std::vector - kinematic_constraints_; /**< \brief Shared pointers to all the member constraints */ - - std::vector joint_constraints_; /**< \brief Messages corresponding to all internal - joint constraints */ - std::vector position_constraints_; /**< \brief Messages corresponding to all - internal position constraints */ - std::vector orientation_constraints_; /**< \brief Messages corresponding to - all - internal orientation constraints */ - std::vector visibility_constraints_; /**< \brief Messages corresponding to - all - internal visibility constraints */ - moveit_msgs::msg::Constraints all_constraints_; /**< \brief Messages corresponding to all internal constraints */ -}; -} // namespace kinematic_constraints +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.hpp b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.hpp new file mode 100644 index 0000000000..c462e6e95e --- /dev/null +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.hpp @@ -0,0 +1,1107 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +/** \brief Representation and evaluation of kinematic constraints */ +namespace kinematic_constraints +{ +/// \brief Struct for containing the results of constraint evaluation +struct ConstraintEvaluationResult +{ + /** + * \brief Constructor + * + * @param [in] result_satisfied True if the constraint evaluated to true, otherwise false + * @param [in] dist The distance evaluated by the constraint + * + * @return + */ + ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0) + : satisfied(result_satisfied), distance(dist) + { + } + + bool satisfied; /**< \brief Whether or not the constraint or constraints were satisfied */ + double distance; /**< \brief The distance evaluation from the constraint or constraints */ +}; + +MOVEIT_CLASS_FORWARD(KinematicConstraint); // Defines KinematicConstraintPtr, ConstPtr, WeakPtr... etc + +/// \brief Base class for representing a kinematic constraint +class KinematicConstraint +{ +public: + /// \brief Enum for representing a constraint + enum ConstraintType + { + UNKNOWN_CONSTRAINT, + JOINT_CONSTRAINT, + POSITION_CONSTRAINT, + ORIENTATION_CONSTRAINT, + VISIBILITY_CONSTRAINT + }; + + /** + * \brief Constructor + * + * @param [in] model The kinematic model used for constraint evaluation + */ + KinematicConstraint(const moveit::core::RobotModelConstPtr& model); + virtual ~KinematicConstraint(); + + /** \brief Clear the stored constraint */ + virtual void clear() = 0; + + /** + * \brief Decide whether the constraint is satisfied in the indicated state + * + * @param [in] state The kinematic state used for evaluation + * @param [in] verbose Whether or not to print output + * + * @return + */ + virtual ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const = 0; + + /** \brief This function returns true if this constraint is + configured and able to decide whether states do meet the + constraint or not. If this function returns false it means + that decide() will always return true -- there is no + constraint to be checked. */ + virtual bool enabled() const = 0; + + /** + * \brief Check if two constraints are the same. This means that + * the types are the same, the subject of the constraint is the + * same, and all values associated with the constraint are within a + * margin. The other constraint must also be enabled. + * + * @param [in] other The other constraint to test + * @param [in] margin The margin to apply to all values associated with constraint + * + * @return True if equal, otherwise false + */ + virtual bool equal(const KinematicConstraint& other, double margin) const = 0; + + /** + * \brief Get the type of constraint + * + * + * @return The constraint type + */ + ConstraintType getType() const + { + return type_; + } + + /** + * \brief Print the constraint data + * + * @param [in] out The file descriptor for printing + */ + virtual void print(std::ostream& /*unused*/ = std::cout) const + { + } + + /** + * + * \brief The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() + *function + * + * @return The constraint weight + */ + double getConstraintWeight() const + { + return constraint_weight_; + } + + /** + * + * + * + * @return The kinematic model associated with this constraint + */ + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + +protected: + ConstraintType type_; /**< \brief The type of the constraint */ + moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model associated with this constraint */ + double constraint_weight_; /**< \brief The weight of a constraint is a multiplicative factor associated to the + distance computed by the decide() function */ +}; + +MOVEIT_CLASS_FORWARD(JointConstraint); // Defines JointConstraintPtr, ConstPtr, WeakPtr... etc + +/** + * \brief Class for handling single DOF joint constraints. + * + * This class handles single DOF constraints expressed as a tolerance + * above and below a target position. Multi-DOF joints can be + * accommodated by using local name formulations - i.e. for a planar + * joint specifying a constraint in terms of "planar_joint_name"/x. + * + * Continuous revolute single DOF joints will be evaluated based on + * wrapping around 3.14 and -3.14. Tolerances above and below will be + * evaluating over the wrap. For instance, if the constraint value is + * 3.14 and the tolerance above is .04, a value of -3.14 is in bounds, + * as is a value of -3.12. -3.1 is out of bounds. Similarly, if the + * value of the constraint is -3.14, the tolerance above is .04, and + * the tolerance below is .02 then -3.1 is a valid value, as is 3.14; + * 3.1 is out of bounds. + * + * Type will be JOINT_CONSTRAINT. + */ +class JointConstraint : public KinematicConstraint +{ +public: + /** + * \brief Constructor + * + * @param [in] model The kinematic model used for constraint evaluation + */ + JointConstraint(const moveit::core::RobotModelConstPtr& model) + : KinematicConstraint(model), joint_model_(nullptr), joint_variable_index_(-1) + { + type_ = JOINT_CONSTRAINT; + } + + /** + * \brief Configure the constraint based on a + * moveit_msgs::msg::JointConstraint + * + * For the configure command to be successful, the joint must exist + * in the kinematic model, the joint must not be a multi-DOF joint + * (for these joints, local variables should be used), and the + * tolerance values must be positive. + * + * @param [in] jc JointConstraint for configuration + * + * @return True if constraint can be configured from jc + */ + bool configure(const moveit_msgs::msg::JointConstraint& jc); + + /** + * \brief Check if two joint constraints are the same. + * + * This means that the types are the same, the subject of the + * constraint is the same, and all values associated with the + * constraint are within a margin. The other constraint must also + * be enabled. For this to be true of joint constraints, they must + * act on the same joint, and the position and tolerance values must + * be within the margins. + * + * @param [in] other The other constraint to test + * @param [in] margin The margin to apply to all values associated with constraint + * + * @return True if equal, otherwise false + */ + bool equal(const KinematicConstraint& other, double margin) const override; + + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; + bool enabled() const override; + void clear() override; + void print(std::ostream& out = std::cout) const override; + + /** + * \brief Get the joint model for which this constraint operates + * + * @return The relevant joint model if enabled, and otherwise nullptr + */ + const moveit::core::JointModel* getJointModel() const + { + return joint_model_; + } + /** + * \brief Gets the local variable name if this constraint was + * configured for a part of a multi-DOF joint + * + * + * @return The component of the joint name after the slash, or the + * empty string if there is no local variable name + */ + const std::string& getLocalVariableName() const + { + return local_variable_name_; + } + + /** + * \brief Gets the joint variable name, as known to the moveit::core::RobotModel + * + * This will include the local variable name if a variable of a multi-DOF joint is constrained. + * + * @return The joint variable name + */ + const std::string& getJointVariableName() const + { + return joint_variable_name_; + } + + int getJointVariableIndex() const + { + return joint_variable_index_; + } + + /** + * \brief Gets the desired position component of the constraint + * + * + * @return The desired joint position + */ + double getDesiredJointPosition() const + { + return joint_position_; + } + + /** + * \brief Gets the upper tolerance component of the joint constraint + * + * + * @return The above joint tolerance + */ + double getJointToleranceAbove() const + { + return joint_tolerance_above_; + } + + /** + * \brief Gets the lower tolerance component of the joint constraint + * + * + * @return The below joint tolerance + */ + double getJointToleranceBelow() const + { + return joint_tolerance_below_; + } + +protected: + const moveit::core::JointModel* joint_model_; /**< \brief The joint from the kinematic model for this constraint */ + bool joint_is_continuous_; /**< \brief Whether or not the joint is continuous */ + std::string local_variable_name_; /**< \brief The local variable name for a multi DOF joint, if any */ + std::string joint_variable_name_; /**< \brief The joint variable name */ + int joint_variable_index_; /**< \brief The index of the joint variable name in the full robot state */ + double joint_position_, joint_tolerance_above_, joint_tolerance_below_; /**< \brief Position and tolerance values*/ +}; + +MOVEIT_CLASS_FORWARD(OrientationConstraint); // Defines OrientationConstraintPtr, ConstPtr, WeakPtr... etc + +/** + * \brief Class for constraints on the orientation of a link + * + * This class expresses an orientation constraint on a particular link. + * The constraint specifies a target orientation via a quaternion as well as + * tolerances on X,Y, and Z rotation axes. + * The rotation difference between the target and actual link orientation is expressed + * either as XYZ Euler angles or as a rotation vector (depending on the `parameterization` type). + * The latter is highly recommended, because it supports resolution of subframes and attached bodies. + * Also, rotation vector representation allows to interpret the tolerances always w.r.t. the given reference frame. + * Euler angles are much more restricted and exhibit singularities. + * + * For efficiency, if the target orientation is expressed w.r.t. to a fixed frame (relative to the planning frame), + * some stuff is precomputed. However, mobile reference frames are supported as well. + * + * The type value will return ORIENTATION_CONSTRAINT. + * + */ +class OrientationConstraint : public KinematicConstraint +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +public: + /** + * \brief Constructor + * + * @param [in] model The kinematic model used for constraint evaluation + */ + OrientationConstraint(const moveit::core::RobotModelConstPtr& model) + : KinematicConstraint(model), link_model_(nullptr) + { + type_ = ORIENTATION_CONSTRAINT; + } + + /** + * \brief Configure the constraint based on a + * moveit_msgs::msg::OrientationConstraint + * + * For the configure command to be successful, the link must exist + * in the kinematic model. Note that if the absolute tolerance + * values are left as 0.0 only values less than a very small epsilon + * will evaluate to satisfied. + * + * @param [in] oc OrientationConstraint for configuration + * + * @return True if constraint can be configured from oc + */ + bool configure(const moveit_msgs::msg::OrientationConstraint& oc, const moveit::core::Transforms& tf); + + /** + * \brief Check if two orientation constraints are the same. + + * This means that the types are the same, the subject of the + * constraint is the same, and all values associated with the + * constraint are within a margin. The other constraint must also + * be enabled. For this to be true of orientation constraints: + * \li The link must be the same + * \li The rotations specified by the quaternions must be within the margin + * \li The tolerances must all be within the margin + * + * @param [in] other The other constraint to test + * @param [in] margin The margin to apply to all values associated with constraint + * + * @return True if equal, otherwise false + */ + bool equal(const KinematicConstraint& other, double margin) const override; + + void clear() override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; + bool enabled() const override; + void print(std::ostream& out = std::cout) const override; + + /** + * \brief Gets the subject link model + * + * + * @return Returns the current link model + */ + const moveit::core::LinkModel* getLinkModel() const + { + return link_model_; + } + + /** + * \brief The target frame of the planning_models::Transforms class, + * for interpreting the rotation frame. + * + * @return The reference frame. + */ + const std::string& getReferenceFrame() const + { + return desired_rotation_frame_id_; + } + + /** + * \brief Whether or not a mobile reference frame is being employed. + * + * @return True if a mobile reference frame is being employed, and + * otherwise false. + */ + bool mobileReferenceFrame() const + { + return mobile_frame_; + } + + /** + * \brief The rotation target in the reference frame. + * + * @return The target rotation. + * + * The returned matrix is always a valid rotation matrix. + */ + const Eigen::Matrix3d& getDesiredRotationMatrixInRefFrame() const + { + // validity of the rotation matrix is enforced in configure() + return desired_R_in_frame_id_; + } + + /** + * \brief The rotation target in the reference or tf frame. + * + * @return The target rotation. + * + * The returned matrix is always a valid rotation matrix. + */ + const Eigen::Matrix3d& getDesiredRotationMatrix() const + { + // validity of the rotation matrix is enforced in configure() + return desired_rotation_matrix_; + } + + /** + * \brief Gets the X axis tolerance + * + * + * @return The X axis tolerance + */ + double getXAxisTolerance() const + { + return absolute_x_axis_tolerance_; + } + + /** + * \brief Gets the Y axis tolerance + * + * + * @return The Y axis tolerance + */ + double getYAxisTolerance() const + { + return absolute_y_axis_tolerance_; + } + + /** + * \brief Gets the Z axis tolerance + * + * + * @return The Z axis tolerance + */ + double getZAxisTolerance() const + { + return absolute_z_axis_tolerance_; + } + + int getParameterizationType() const + { + return parameterization_type_; + } + +protected: + const moveit::core::LinkModel* link_model_; /**< The target link model */ + Eigen::Matrix3d desired_R_in_frame_id_; /**< Desired rotation matrix in frame_id */ + Eigen::Matrix3d desired_rotation_matrix_; /**< The desired rotation matrix in the tf frame */ + Eigen::Matrix3d desired_rotation_matrix_inv_; /**< The inverse of desired_rotation_matrix_ (for efficiency) */ + std::string desired_rotation_frame_id_; /**< The target frame of the transform tree */ + bool mobile_frame_; /**< Whether or not the header frame is mobile or fixed */ + int parameterization_type_; /**< Parameterization type for orientation tolerance */ + double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, + absolute_z_axis_tolerance_; /**< Storage for the tolerances */ +}; + +MOVEIT_CLASS_FORWARD(PositionConstraint); // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc + +/** + * \brief Class for constraints on the XYZ position of a link + * + * This class expresses X,Y,Z position constraints of a link. The + * position area is specified as a bounding volume consisting of one + * or more shapes - either solid primitives or meshes. The pose + * information in the volumes will be interpreted by using the header + * information. The header may either specify a fixed frame or a + * mobile frame. Additionally, a target offset specified in the frame + * of the link being constrained can be specified. The type value + * will return POSITION_CONSTRAINT. + * + */ +class PositionConstraint : public KinematicConstraint +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +public: + /** + * \brief Constructor + * + * @param [in] model The kinematic model used for constraint evaluation + */ + PositionConstraint(const moveit::core::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(nullptr) + { + type_ = POSITION_CONSTRAINT; + } + + /** + * \brief Configure the constraint based on a + * moveit_msgs::msg::PositionConstraint + * + * For the configure command to be successful, the link must be + * specified in the model, and one or more constrained regions must + * be correctly specified, which requires containing a valid shape + * and a pose for that shape. If the header frame on the constraint + * is empty, the constraint will fail to configure. If an invalid + * quaternion is passed for a shape, the identity quaternion will be + * substituted. + * + * @param [in] pc moveit_msgs::msg::PositionConstraint for configuration + * + * @return True if constraint can be configured from pc + */ + bool configure(const moveit_msgs::msg::PositionConstraint& pc, const moveit::core::Transforms& tf); + + /** + * \brief Check if two constraints are the same. For position + * constraints this means that: + * \li The types are the same + * \li The link model is the same + * \li The frame of the constraints are the same + * \li The target offsets are no more than the margin apart + * \li Each entry in the constraint region of this constraint matches a region in the other constraint + * \li Each entry in the other constraint region matches a region in the other constraint + * + * Two constraint regions matching each other means that: + * \li The poses match within the margin + * \li The types are the same + * \li The shape volumes are within the margin + * + * Note that the two shapes can have different numbers of regions as + * long as all regions are matched up to another. + * + * @param [in] other The other constraint to test + * @param [in] margin The margin to apply to all values associated with constraint + * + * @return True if equal, otherwise false + */ + bool equal(const KinematicConstraint& other, double margin) const override; + + void clear() override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; + bool enabled() const override; + void print(std::ostream& out = std::cout) const override; + + /** + * \brief Returns the associated link model, or nullptr if not enabled + * + * + * @return The link model + */ + const moveit::core::LinkModel* getLinkModel() const + { + return link_model_; + } + + /** + * \brief Returns the target offset + * + * + * @return The target offset + */ + const Eigen::Vector3d& getLinkOffset() const + { + return offset_; + } + + /** + * \brief If the constraint is enabled and the link offset is + * substantially different than zero + * + * + * @return Whether or not there is a link offset + */ + bool hasLinkOffset() const + { + if (!enabled()) + return false; + return has_offset_; + } + + /** + * \brief Returns all the constraint regions + * + * + * @return The constraint regions + */ + const std::vector& getConstraintRegions() const + { + return constraint_region_; + } + + /** + * \brief Returns the reference frame + * + * + * @return The reference frame + */ + const std::string& getReferenceFrame() const + { + return constraint_frame_id_; + } + + /** + * \brief If enabled and the specified frame is a mobile frame, + * return true. Otherwise, returns false. + * + * + * @return Whether a mobile reference frame is being employed + */ + bool mobileReferenceFrame() const + { + if (!enabled()) + return false; + return mobile_frame_; + } + +protected: + Eigen::Vector3d offset_; /**< \brief The target offset */ + bool has_offset_; /**< \brief Whether the offset is substantially different than 0.0 */ + std::vector constraint_region_; /**< \brief The constraint region vector */ + /** \brief The constraint region pose vector. All isometries are guaranteed to be valid. */ + EigenSTL::vector_Isometry3d constraint_region_pose_; + bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/ + std::string constraint_frame_id_; /**< \brief The constraint frame id */ + const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */ +}; + +MOVEIT_CLASS_FORWARD(VisibilityConstraint); // Defines VisibilityConstraintPtr, ConstPtr, WeakPtr... etc + +/** + * \brief Class for constraints on the visibility relationship between + * a sensor and a target. + * + * Visibility constraints are the most complicated kinematic + * constraint type. They are designed to test whether a visibility + * relationship is maintained in a given state. For the visibility + * relationship to be maintained, a sensor must have an unimpeded view + * of a target - this is useful, for instance, if one wants to test + * whether a robot can see its hand with a given sensor in a given + * state. The mechanism that is used to test the constraint is a + * combination of geometric checks, and then a collision check that + * tests whether a cone that connects the sensor and the target is + * entirely unobstructed by the robot's links. + * + * The constraint consists of a sensor pose, a target pose, a few + * parameters that govern the shape of the target, and a few + * parameters that allow finer control over the geometry of the check. + * There are two general ways that this constraint can be used. The + * first way to leave max_view_angle and max_range_angle in the + * VisibilityConstraint message as 0.0. In this case, the only check + * that is performed is the collision check against the cone. This + * check assumes that the sensor can see in all directions - the + * orientation of the sensor has no effect on the creation of the + * cone. It is important to note that the sensor and the target poses + * must not result in collision - if they are associated with sensor + * frames or robot links they must be specified to be outside the + * robot's body or all states will violate the Visibility Constraint. + * For an example of a visualization of a visibility constraint + * (produced using the getMarkers member function), see this image: + * + * \image html fingertip_cone.png "Visibility constraint satisfied as cone volume is clear" + * + * The cone is shown in red, and the arrows show normals. This + * visibility constraint uses a sensor pose (the narrow end of the + * cone ) of the PR2's narrow_stereo_optical_frame, except shifted + * along the Z axis such that the pose is outside of the robot's head + * and doesn't collide. The cone is allowed to collide with the + * actual sensor associated with the header frame, just not with any + * other links. The target pose is a 5 cm circle offset from the + * l_gripper_r_finger_tip_link, again so as not to collide - again, + * the cone can collide with the target link, but now with any other + * links. In the indicated state the cone is collision free and thus + * satisfies the collision component of the visibility constraint. In + * this image, however, the right arm intersects the cone, violating + * the visibility constraint: + * + * \image html fingertip_collision.png "Visibility constraint violated as right arm is within the cone" + * + * Note that both the target and the sensor frame can change with the + * robot's state. + * + * The collision check essentially asks whether or not the volume + * between the sensor pose and the target pose are clear, but that's + * only one aspect of visibility we may care about. The visibility + * constraint also allows for some geometric reasoning about the + * relationship between the sensor or the target - this allows + * information such as approximate field of view of the sensor to be + * factored into the constraint. To use this reasoning, the + * sensor_view_direction of the field should first be set - this + * specifies which axis of the sensor pose frame is actually pointing. + * The system assumes that the the sensor pose has been set up such + * that a single of the axes is pointing directly out of the sensor. + * Once this value is set correctly, one or both of the max_view_angle + * and the max_range_angle values can be set. Setting a positive + * max_view_angle will constrain the sensor along the specified axis + * and the target along its Z axis to be pointing at each other. + * Practically speaking, this ensures that the sensor has sufficient + * visibility to the front of the target - if the target is pointing + * the opposite direction, or is too steeply perpendicular to the + * target, then the max_view_angle part of the constraint will be + * violated. The getMarkers function can again help explain this - + * the view angle is the angular difference between the blue arrow + * associated with the sensor and the red arrow associated with the + * target. + + * \image html exact_opposites.png "Max view angle is evaluated at 0.0" + * \image html fourty_five.png "Max view angle evaluates around pi/4" + * \image html perpendicular.png "Max view angle evaluates at pi/2, the maximum" + * \image html other_side.png "Sensor pointed at wrong side of target, will violate constraint as long as max_view_angle + > 0.0" + * + * If constraining the target to be within the field of view of the + * sensor is the goal, then the max_range_angle can be set. The range + * angle is calculated as the angle between the origins of the sensor + * and the target frame - the orientations are unimportant. In the + * above images, the range angle is always 0, as the target's center + * is directly lined up with the blue arrow. In this image, however, + * the view angle is evaluated at 0.0, while the range angle is + * evaluated at .65. + * + * \image html range_angle.png "Range angle is high, so only sensors with wide fields of view would see the target" + * + * By limiting the max_range_angle, you can constrain the target to be + * within the field of view of the sensor. Max_view_angle and + * max_range_angle can be used at once. + */ +class VisibilityConstraint : public KinematicConstraint +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +public: + /** + * \brief Constructor + * + * @param [in] model The kinematic model used for constraint evaluation + */ + VisibilityConstraint(const moveit::core::RobotModelConstPtr& model); + + /** + * \brief Configure the constraint based on a + * moveit_msgs::msg::VisibilityConstraint + * + * For the configure command to be successful, one of the three possible + * constraint criteria must be set to a non-zero value: + * - The target radius (negative values will have the absolute value taken). + * - The range angle. + * - The view angle. + * + * If cone sides are less than 3, a value of 3 will be used. + * + * @param [in] vc moveit_msgs::msg::VisibilityConstraint for configuration + * + * @return True if constraint can be configured from vc + */ + bool configure(const moveit_msgs::msg::VisibilityConstraint& vc, const moveit::core::Transforms& tf); + + /** + * \brief Check if two constraints are the same. + * + * For visibility constraints this means that: + * \li The types are the same, + * \li The target frame ids are the same + * \li The sensor frame ids are the same + * \li The cone sides number is the same + * \li The sensor view directions are the same + * \li The max view angles and target radii are within the margin + * \li The sensor and target poses are within the margin, as computed by taking the norm of the difference. + * + * @param [in] other The other constraint to test + * @param [in] margin The margin to apply to all values associated with constraint + * + * @return True if equal, otherwise false + */ + bool equal(const KinematicConstraint& other, double margin) const override; + void clear() override; + + /** + * \brief Gets a trimesh shape representing the visibility cone + * + * @param [in] tform_world_to_sensor Transform from the world to the sensor frame + * @param [in] tform_world_to_target Transform from the world to the target frame + * + * @return The shape associated with the cone + */ + shapes::Mesh* getVisibilityCone(const Eigen::Isometry3d& tform_world_to_sensor, + const Eigen::Isometry3d& tform_world_to_target) const; + + /** + * \brief Adds markers associated with the visibility cone, sensor + * and target to the visualization array + * + * The visibility cone and two arrows - a blue array that issues + * from the sensor_view_direction of the sensor, and a red arrow the + * issues along the Z axis of the the target frame. + * + * @param [in] state The state from which to produce the markers + * @param [out] markers The marker array to which the markers will be added + */ + void getMarkers(const moveit::core::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const; + + bool enabled() const override; + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const override; + void print(std::ostream& out = std::cout) const override; + +protected: + /** + * \brief Function that gets passed into collision checking to allow some collisions. + * + * The cone object is allowed to touch either the sensor or the header frame, but not anything else + * + * @param [in] contact The contact in question + * + * @return True if the collision is allowed, otherwise false + */ + bool decideContact(const collision_detection::Contact& contact) const; + + moveit::core::RobotModelConstPtr robot_model_; /**< \brief A copy of the robot model used to create collision + environments to check the cone against robot links */ + + std::string target_frame_id_; /**< \brief The target frame id */ + std::string sensor_frame_id_; /**< \brief The sensor frame id */ + Eigen::Isometry3d sensor_pose_; /**< \brief The sensor pose transformed into the transform frame */ + int sensor_view_direction_; /**< \brief Storage for the sensor view direction */ + Eigen::Isometry3d target_pose_; /**< \brief The target pose transformed into the transform frame */ + unsigned int cone_sides_; /**< \brief Storage for the cone sides */ + EigenSTL::vector_Vector3d points_; /**< \brief A set of points along the base of the circle */ + double target_radius_; /**< \brief Storage for the target radius */ + double max_view_angle_; /**< \brief Storage for the max view angle */ + double max_range_angle_; /**< \brief Storage for the max range angle */ +}; + +MOVEIT_CLASS_FORWARD(KinematicConstraintSet); // Defines KinematicConstraintSetPtr, ConstPtr, WeakPtr... etc + +/** + * \brief A class that contains many different constraints, and can + * check RobotState *versus the full set. A set is satisfied if + * and only if all constraints are satisfied. + * + * The set may contain any number of different kinds of constraints. + * All constraints, including invalid ones, are stored internally. + */ +class KinematicConstraintSet +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +public: + /** + * \brief Constructor + * + * @param [in] model The kinematic model used for constraint evaluation + */ + KinematicConstraintSet(const moveit::core::RobotModelConstPtr& model) : robot_model_(model) + { + } + + ~KinematicConstraintSet() + { + clear(); + } + + /** \brief Clear the stored constraints */ + void clear(); + + /** + * \brief Add all known constraints + * + * @param [in] c A message potentially contain vectors of constraints of add types + * + * @return Whether or not all constraints could be successfully + * configured given the contents of the message. The + * KinematicConstraintSet can still be used even if the addition + * returns false. + */ + bool add(const moveit_msgs::msg::Constraints& c, const moveit::core::Transforms& tf); + + /** + * \brief Add a vector of joint constraints + * + * @param [in] jc A vector of joint constraints + * + * @return Will return true only if all constraints are valid, and false otherwise + */ + bool add(const std::vector& jc); + + /** + * \brief Add a vector of position constraints + * + * @param [in] pc A vector of position constraints + * + * @return Will return true only if all constraints are valid, and false otherwise + */ + bool add(const std::vector& pc, const moveit::core::Transforms& tf); + + /** + * \brief Add a vector of orientation constraints + * + * @param [in] oc A vector of orientation constraints + * + * @return Will return true only if all constraints are valid, and false otherwise + */ + bool add(const std::vector& oc, const moveit::core::Transforms& tf); + + /** + * \brief Add a vector of visibility constraints + * + * @param [in] vc A vector of visibility constraints + * + * @return Will return true only if all constraints are valid, and false otherwise + */ + bool add(const std::vector& vc, const moveit::core::Transforms& tf); + + /** + * \brief Determines whether all constraints are satisfied by state, + * returning a single evaluation result + * + * @param [in] state The state to test + * @param [in] verbose Whether or not to make each constraint give debug output + * + * @return A single constraint evaluation result, where it will + * report satisfied only if all constraints are satisfied, and with + * a distance that is the sum of all individual distances. + */ + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, bool verbose = false) const; + + /** + * + * \brief Determines whether all constraints are satisfied by state, + * returning a vector of results through a parameter in addition to + * a summed result. + * + * @param [in] state The state to test + * + * @param [out] results The individual results from constraint + * evaluation on each constraint contained in the set. + * + * @param [in] verbose Whether to print the results of each constraint + * check. + * + * @return A single constraint evaluation result, where it will + * report satisfied only if all constraints are satisfied, and with + * a distance that is the sum of all individual distances. + */ + ConstraintEvaluationResult decide(const moveit::core::RobotState& state, + std::vector& results, bool verbose = false) const; + + /** + * \brief Whether or not another KinematicConstraintSet is equal to + * this one. + * + * Equality means that for each constraint in this set there is a + * constraint in the other set for which equal() is true with the + * given margin. Multiple constraints in this set can be matched to + * single constraints in the other set. Some constraints in the + * other set may not be matched to constraints in this set. + * + * @param [in] other The other set against which to test + * @param [in] margin The margin to apply to all individual constraint equality tests + * + * @return True if all constraints are matched, false otherwise + */ + bool equal(const KinematicConstraintSet& other, double margin) const; + + /** + * \brief Print the constraint data + * + * @param [in] out The file stream for printing + */ + void print(std::ostream& out = std::cout) const; + + /** + * \brief Get all position constraints in the set + * + * + * @return All position constraints + */ + const std::vector& getPositionConstraints() const + { + return position_constraints_; + } + + /** + * \brief Get all orientation constraints in the set + * + * + * @return All orientation constraints + */ + const std::vector& getOrientationConstraints() const + { + return orientation_constraints_; + } + + /** + * \brief Get all joint constraints in the set + * + * + * @return All joint constraints + */ + const std::vector& getJointConstraints() const + { + return joint_constraints_; + } + + /** + * \brief Get all visibility constraints in the set + * + * + * @return All visibility constraints + */ + const std::vector& getVisibilityConstraints() const + { + return visibility_constraints_; + } + + /** + * \brief Get all constraints in the set + * + * + * @return All constraints in a single message + */ + const moveit_msgs::msg::Constraints& getAllConstraints() const + { + return all_constraints_; + } + + /** + * \brief Returns whether or not there are any constraints in the set + * + * + * @return True if there are no constraints, otherwise false. + */ + bool empty() const + { + return kinematic_constraints_.empty(); + } + +protected: + moveit::core::RobotModelConstPtr robot_model_; /**< \brief The kinematic model used for by the Set */ + std::vector + kinematic_constraints_; /**< \brief Shared pointers to all the member constraints */ + + std::vector joint_constraints_; /**< \brief Messages corresponding to all internal + joint constraints */ + std::vector position_constraints_; /**< \brief Messages corresponding to all + internal position constraints */ + std::vector orientation_constraints_; /**< \brief Messages corresponding to + all + internal orientation constraints */ + std::vector visibility_constraints_; /**< \brief Messages corresponding to + all + internal visibility constraints */ + moveit_msgs::msg::Constraints all_constraints_; /**< \brief Messages corresponding to all internal constraints */ +}; +} // namespace kinematic_constraints diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index 956b45054d..a7462a8718 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,255 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace XmlRpc -{ -class XmlRpcValue; -} - -namespace kinematic_constraints -{ -/** - * \brief Merge two sets of constraints into one. - * - * This just does appending of all constraints except joint - * constraints. For members of type \ref JointConstraint, the bounds - * specified in the parameter \e first take precedence over parameter - * \e second - * - * @param [in] first The first constraint to merge - * @param [in] second The second constraint to merge - * - * @return The merged set of constraints - */ -moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints& first, - const moveit_msgs::msg::Constraints& second); - -std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& constr); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a joint group. The full constraint will contain a - * vector of type \ref JointConstraint, one for each DOF in the group. - * - * @param [in] state The state from which to generate goal joint constraints - * @param [in] jmg The group for which to generate goal joint constraints - * @param [in] tolerance_below The below tolerance to apply to all constraints [rad or meters for prismatic joints] - * @param [in] tolerance_above The above tolerance to apply to all constraints [rad or meters for prismatic joints] - * - * @return A full constraint message containing all the joint constraints - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state, - const moveit::core::JointModelGroup* jmg, double tolerance_below, - double tolerance_above); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a joint group. The full constraint will contain a - * vector of type \ref JointConstraint, one for each DOF in the group. - * - * @param [in] state The state from which to generate goal joint constraints - * @param [in] jmg The group for which to generate joint constraints - * @param [in] tolerance An angular tolerance to apply both above and below for all constraints [rad or meters for - * prismatic joints] - * - * @return A full constraint message containing all the joint constraints - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state, - const moveit::core::JointModelGroup* jmg, - double tolerance = std::numeric_limits::epsilon()); - -/** - * \brief Update joint constraints with a new JointModelGroup state - * - * @param [in, out] constraints Previously-constructed constraints to update - * @param [in] state The new target state - * @param [in] jmg Specify which JointModelGroup to update - * - * @return true if all joint constraints were updated - */ -bool updateJointConstraints(moveit_msgs::msg::Constraints& constraints, const moveit::core::RobotState& state, - const moveit::core::JointModelGroup* jmg); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a given link. The full constraint will contain a - * \ref PositionConstraint and a \ref OrientationConstraint, - * constructed from the pose. A sphere will be used to represent the - * constraint region for the \ref PositionConstraint. - * - * @param [in] link_name The link name for both constraints - * @param [in] pose The pose stamped to be used for the target region. - * @param [in] tolerance_pos The radius of a sphere defining a \ref PositionConstraint - * @param [in] tolerance_angle The value to assign to the absolute tolerances of the \ref OrientationConstraint - * - * @return A full constraint message containing both constraints - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::msg::PoseStamped& pose, - double tolerance_pos = 1e-3, double tolerance_angle = 1e-2); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a given link. The full constraint will contain a - * \ref PositionConstraint and a \ref OrientationConstraint, - * constructed from the pose. A box will be used to represent the - * constraint region for the \ref PositionConstraint. - * - * @param [in] link_name The link name for both constraints - * @param [in] pose The pose stamped to be used for the target region. - * @param [in] tolerance_pos The dimensions of the box (xyz) associated with the target region of the \ref - *PositionConstraint - * @param [in] tolerance_angle The values to assign to the absolute tolerances (xyz) of the \ref OrientationConstraint - * - * @return A full constraint message containing both constraints - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::msg::PoseStamped& pose, - const std::vector& tolerance_pos, - const std::vector& tolerance_angle); - -/** - * \brief Update a pose constraint for one link with a new pose - * - * @param [in, out] constraints Previously-constructed constraints to update - * @param [in] link The link to update for - * @param [in] pose The new target pose - * - * @return true if the constraint was updated - */ -bool updatePoseConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, - const geometry_msgs::msg::PoseStamped& pose); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a given link. The full constraint message will - * contain only an \ref OrientationConstraint. - * - * @param [in] link_name The link name for the \ref OrientationConstraint - * @param [in] quat The quaternion for the \ref OrientationConstraint - * @param [in] tolerance The absolute axes tolerances to apply to the \ref OrientationConstraint - * - * @return A full constraint message containing the orientation constraint - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::msg::QuaternionStamped& quat, - double tolerance = 1e-2); - -/** - * \brief Update an orientation constraint for one link with a new quaternion - * - * @param [in, out] constraints Previously-constructed constraints to update - * @param [in] link The link to update for - * @param [in] quat The new target quaternion - * - * @return true if the constraint was updated - */ -bool updateOrientationConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, - const geometry_msgs::msg::QuaternionStamped& quat); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a given link. The full constraint message will - * contain only a \ref PositionConstraint. A sphere will be used to - * represent the constraint region. - * - * @param [in] link_name The link name for the \ref PositionConstraint - * @param [in] reference_point A point corresponding to the target_point_offset of the \ref PositionConstraint - * @param [in] goal_point The position associated with the constraint region - * @param [in] tolerance The radius of a sphere defining a \ref PositionConstraint - * - * @return A full constraint message containing the position constraint - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::msg::Point& reference_point, - const geometry_msgs::msg::PointStamped& goal_point, - double tolerance = 1e-3); - -/** - * \brief Generates a constraint message intended to be used as a goal - * constraint for a given link. The full constraint message will - * contain only a \ref PositionConstraint. A sphere will be used to - * represent the constraint region. - * - * @param [in] link_name The link name for the \ref PositionConstraint - * @param [in] goal_point The position associated with the constraint region - * @param [in] tolerance The radius of a sphere defining a \ref PositionConstraint - * - * @return A full constraint message containing the position constraint - */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::msg::PointStamped& goal_point, - double tolerance = 1e-3); - -/** - * \brief Update a position constraint for one link with a new position - * - * @param [in, out] constraints Previously-constructed constraints to update - * @param [in] link The link to update for - * @param [in] goal_point The new target point - * - * @return true if the constraint was updated - */ -bool updatePositionConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, - const geometry_msgs::msg::PointStamped& goal_point); - -/** - * \brief extract constraint message from node parameters. - * - * This can be used to construct a Constraints message from - * specifications provided from a yaml file. - * An example for a constraint yaml structure (loaded at constraint_param): - * """ - * name: constraint_name - * constraint_ids: [constraint_1, constraint_2] - * constraints: - * constraint_1: - * type: orientation - * frame_id: world - * link_name: tool0 - * orientation: [0, 0, 0] # [r, p, y] - * tolerances: [0.01, 0.01, 3.15] - * weight: 1.0 - * constraint_2: - * type: position - * frame_id: base_link - * link_name: tool0 - * target_offset: [0.1, 0.1, 0.1] # [x, y, z] - * region: - * x: [0.1, 0.4] # [min, max] - * y: [0.2, 0.3] - * z: [0.1, 0.6] - * weight: 1.0 - * """ - * - * @param [in] params Node node to load the parameters from - * @param [in] params string namespace from where to load the constraints parameters - * @param [out] constraints The constructed constraints message - * - * @return was the construction successful? - */ -bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string& constraints_param, - moveit_msgs::msg::Constraints& constraints); - -/** - * \brief Resolves frames used in constraints to links in the robot model. - * - * The link_name field of a constraint is changed from the name of an object's frame or subframe - * to the name of the robot link that the object is attached to. - * - * This is used in a planning request adapter which ensures that the planning problem is defined - * properly (the attached objects' frames are not known to the planner). - * - * @param [in] state The RobotState used to resolve frames. - * @param [in] constraints The constraint to resolve. - */ -bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::msg::Constraints& constraints); -} // namespace kinematic_constraints +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.hpp b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.hpp new file mode 100644 index 0000000000..72a9632006 --- /dev/null +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.hpp @@ -0,0 +1,289 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace XmlRpc +{ +class XmlRpcValue; +} + +namespace kinematic_constraints +{ +/** + * \brief Merge two sets of constraints into one. + * + * This just does appending of all constraints except joint + * constraints. For members of type \ref JointConstraint, the bounds + * specified in the parameter \e first take precedence over parameter + * \e second + * + * @param [in] first The first constraint to merge + * @param [in] second The second constraint to merge + * + * @return The merged set of constraints + */ +moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints& first, + const moveit_msgs::msg::Constraints& second); + +std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& constr); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a joint group. The full constraint will contain a + * vector of type \ref JointConstraint, one for each DOF in the group. + * + * @param [in] state The state from which to generate goal joint constraints + * @param [in] jmg The group for which to generate goal joint constraints + * @param [in] tolerance_below The below tolerance to apply to all constraints [rad or meters for prismatic joints] + * @param [in] tolerance_above The above tolerance to apply to all constraints [rad or meters for prismatic joints] + * + * @return A full constraint message containing all the joint constraints + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, double tolerance_below, + double tolerance_above); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a joint group. The full constraint will contain a + * vector of type \ref JointConstraint, one for each DOF in the group. + * + * @param [in] state The state from which to generate goal joint constraints + * @param [in] jmg The group for which to generate joint constraints + * @param [in] tolerance An angular tolerance to apply both above and below for all constraints [rad or meters for + * prismatic joints] + * + * @return A full constraint message containing all the joint constraints + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg, + double tolerance = std::numeric_limits::epsilon()); + +/** + * \brief Update joint constraints with a new JointModelGroup state + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] state The new target state + * @param [in] jmg Specify which JointModelGroup to update + * + * @return true if all joint constraints were updated + */ +bool updateJointConstraints(moveit_msgs::msg::Constraints& constraints, const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a given link. The full constraint will contain a + * \ref PositionConstraint and a \ref OrientationConstraint, + * constructed from the pose. A sphere will be used to represent the + * constraint region for the \ref PositionConstraint. + * + * @param [in] link_name The link name for both constraints + * @param [in] pose The pose stamped to be used for the target region. + * @param [in] tolerance_pos The radius of a sphere defining a \ref PositionConstraint + * @param [in] tolerance_angle The value to assign to the absolute tolerances of the \ref OrientationConstraint + * + * @return A full constraint message containing both constraints + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, + const geometry_msgs::msg::PoseStamped& pose, + double tolerance_pos = 1e-3, double tolerance_angle = 1e-2); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a given link. The full constraint will contain a + * \ref PositionConstraint and a \ref OrientationConstraint, + * constructed from the pose. A box will be used to represent the + * constraint region for the \ref PositionConstraint. + * + * @param [in] link_name The link name for both constraints + * @param [in] pose The pose stamped to be used for the target region. + * @param [in] tolerance_pos The dimensions of the box (xyz) associated with the target region of the \ref + *PositionConstraint + * @param [in] tolerance_angle The values to assign to the absolute tolerances (xyz) of the \ref OrientationConstraint + * + * @return A full constraint message containing both constraints + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, + const geometry_msgs::msg::PoseStamped& pose, + const std::vector& tolerance_pos, + const std::vector& tolerance_angle); + +/** + * \brief Update a pose constraint for one link with a new pose + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] link The link to update for + * @param [in] pose The new target pose + * + * @return true if the constraint was updated + */ +bool updatePoseConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::PoseStamped& pose); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a given link. The full constraint message will + * contain only an \ref OrientationConstraint. + * + * @param [in] link_name The link name for the \ref OrientationConstraint + * @param [in] quat The quaternion for the \ref OrientationConstraint + * @param [in] tolerance The absolute axes tolerances to apply to the \ref OrientationConstraint + * + * @return A full constraint message containing the orientation constraint + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, + const geometry_msgs::msg::QuaternionStamped& quat, + double tolerance = 1e-2); + +/** + * \brief Update an orientation constraint for one link with a new quaternion + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] link The link to update for + * @param [in] quat The new target quaternion + * + * @return true if the constraint was updated + */ +bool updateOrientationConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::QuaternionStamped& quat); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a given link. The full constraint message will + * contain only a \ref PositionConstraint. A sphere will be used to + * represent the constraint region. + * + * @param [in] link_name The link name for the \ref PositionConstraint + * @param [in] reference_point A point corresponding to the target_point_offset of the \ref PositionConstraint + * @param [in] goal_point The position associated with the constraint region + * @param [in] tolerance The radius of a sphere defining a \ref PositionConstraint + * + * @return A full constraint message containing the position constraint + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, + const geometry_msgs::msg::Point& reference_point, + const geometry_msgs::msg::PointStamped& goal_point, + double tolerance = 1e-3); + +/** + * \brief Generates a constraint message intended to be used as a goal + * constraint for a given link. The full constraint message will + * contain only a \ref PositionConstraint. A sphere will be used to + * represent the constraint region. + * + * @param [in] link_name The link name for the \ref PositionConstraint + * @param [in] goal_point The position associated with the constraint region + * @param [in] tolerance The radius of a sphere defining a \ref PositionConstraint + * + * @return A full constraint message containing the position constraint + */ +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, + const geometry_msgs::msg::PointStamped& goal_point, + double tolerance = 1e-3); + +/** + * \brief Update a position constraint for one link with a new position + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] link The link to update for + * @param [in] goal_point The new target point + * + * @return true if the constraint was updated + */ +bool updatePositionConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::PointStamped& goal_point); + +/** + * \brief extract constraint message from node parameters. + * + * This can be used to construct a Constraints message from + * specifications provided from a yaml file. + * An example for a constraint yaml structure (loaded at constraint_param): + * """ + * name: constraint_name + * constraint_ids: [constraint_1, constraint_2] + * constraints: + * constraint_1: + * type: orientation + * frame_id: world + * link_name: tool0 + * orientation: [0, 0, 0] # [r, p, y] + * tolerances: [0.01, 0.01, 3.15] + * weight: 1.0 + * constraint_2: + * type: position + * frame_id: base_link + * link_name: tool0 + * target_offset: [0.1, 0.1, 0.1] # [x, y, z] + * region: + * x: [0.1, 0.4] # [min, max] + * y: [0.2, 0.3] + * z: [0.1, 0.6] + * weight: 1.0 + * """ + * + * @param [in] params Node node to load the parameters from + * @param [in] params string namespace from where to load the constraints parameters + * @param [out] constraints The constructed constraints message + * + * @return was the construction successful? + */ +bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string& constraints_param, + moveit_msgs::msg::Constraints& constraints); + +/** + * \brief Resolves frames used in constraints to links in the robot model. + * + * The link_name field of a constraint is changed from the name of an object's frame or subframe + * to the name of the robot link that the object is attached to. + * + * This is used in a planning request adapter which ensures that the planning problem is defined + * properly (the attached objects' frames are not known to the planner). + * + * @param [in] state The RobotState used to resolve frames. + * @param [in] constraints The constraint to resolve. + */ +bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::msg::Constraints& constraints); +} // namespace kinematic_constraints diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index a56678402e..022d7a7196 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -34,11 +34,11 @@ /* Author: Ioan Sucan */ -#include +#include #include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 96fa5a70bc..17bf578f86 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -37,8 +37,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index 3faa4db05c..9dedc66303 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -34,13 +34,13 @@ /* Author: Ioan Sucan, E. Gil Jones */ -#include +#include #include #include #include #include #include -#include +#include class LoadPlanningModelsPr2 : public testing::Test { diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index 1a26e34abe..235f4f3075 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp @@ -34,7 +34,7 @@ /* Author: Jeroen De Maeyer */ -#include +#include #include #include @@ -42,7 +42,7 @@ #include #include -#include +#include class SphericalRobot : public testing::Test { diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 1963d7c389..9b40ba51a8 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,579 +47,5 @@ /* Author: Sachin Chitta, Dave Coleman */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace moveit -{ -namespace core -{ -MOVEIT_CLASS_FORWARD(JointModelGroup); -MOVEIT_CLASS_FORWARD(RobotState); -MOVEIT_CLASS_FORWARD(RobotModel); -} // namespace core -} // namespace moveit - -/** @brief API for forward and inverse kinematics */ -namespace kinematics -{ -/* - * @enum DiscretizationMethods - * - * @brief Flags for choosing the type discretization method applied on the redundant joints during an ik query - */ -namespace DiscretizationMethods -{ -enum DiscretizationMethod -{ - NO_DISCRETIZATION = 1, /**< The redundant joints will be fixed at their current value. */ - ALL_DISCRETIZED, /**< All redundant joints will be discretized uniformly */ - SOME_DISCRETIZED, /**< Some redundant joints will be discretized uniformly. The unused redundant joints will be fixed - at their - current value */ - ALL_RANDOM_SAMPLED, /**< the discretization for each redundant joint will be randomly generated.*/ - SOME_RANDOM_SAMPLED /**< the discretization for some redundant joint will be randomly generated. - The unused redundant joints will be fixed at their current value. */ -}; -} // namespace DiscretizationMethods -using DiscretizationMethod = DiscretizationMethods::DiscretizationMethod; - -/* - * @enum KinematicErrors - * @brief Kinematic error codes that occur in a ik query - */ -namespace KinematicErrors -{ -enum KinematicError -{ - OK = 1, /**< No errors*/ - UNSUPORTED_DISCRETIZATION_REQUESTED, /**< Discretization method isn't supported by this implementation */ - DISCRETIZATION_NOT_INITIALIZED, /**< Discretization values for the redundancy has not been set. See - setSearchDiscretization(...) method*/ - MULTIPLE_TIPS_NOT_SUPPORTED, /**< Only single tip link support is allowed */ - EMPTY_TIP_POSES, /**< Empty ik_poses array passed */ - IK_SEED_OUTSIDE_LIMITS, /**< Ik seed is out of bounds*/ - SOLVER_NOT_ACTIVE, /**< Solver isn't active */ - NO_SOLUTION /**< A valid joint solution that can reach this pose(s) could not be found */ - -}; -} // namespace KinematicErrors -using KinematicError = KinematicErrors::KinematicError; - -/** - * @struct KinematicsQueryOptions - * @brief A set of options for the kinematics solver - */ -struct KinematicsQueryOptions -{ - KinematicsQueryOptions() - : lock_redundant_joints(false) - , return_approximate_solution(false) - , discretization_method(DiscretizationMethods::NO_DISCRETIZATION) - { - } - - bool lock_redundant_joints; /**< KinematicsQueryOptions#lock_redundant_joints. */ - bool return_approximate_solution; /**< KinematicsQueryOptions#return_approximate_solution. */ - DiscretizationMethod discretization_method; /**< Enumeration value that indicates the method for discretizing the - redundant. joints KinematicsQueryOptions#discretization_method. */ -}; - -/* - * @struct KinematicsResult - * @brief Reports result details of an ik query - * - * This struct is used as an output argument of the getPositionIK(...) method that returns multiple joint solutions. - * It contains the type of error that led to a failure or KinematicErrors::OK when a set of joint solutions is found. - * The solution percentage shall provide a ratio of solutions found over solutions searched. - * - */ -struct KinematicsResult -{ - KinematicError kinematic_error; /**< Error code that indicates the type of failure */ - double solution_percentage; /**< The percentage of solutions achieved over the total number - of solutions explored. */ -}; - -MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc - -/** - * @class KinematicsBase - * @brief Provides an interface for kinematics solvers. - */ -class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase -{ -public: - static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */ - static const double DEFAULT_TIMEOUT; /* = 1.0 */ - - /** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */ - using IKCallbackFn = std::function&, - moveit_msgs::msg::MoveItErrorCodes&)>; - - /** @brief Signature for a cost function used to evaluate IK solutions. */ - using IKCostFn = std::function&)>; - - /** - * @brief Given a desired pose of the end-effector, compute the joint angles to reach it - * - * In contrast to the searchPositionIK methods, this one is expected to return the solution - * closest to the seed state. Randomly re-seeding is explicitly not allowed. - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param solution the solution vector - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; - - /** - * @brief Given the desired poses of all end-effectors, compute joint angles that are able to reach it. - * - * The default implementation returns only one solution and so its result is equivalent to calling - * 'getPositionIK(...)' with a zero initialized seed. - * - * Some planners (e.g. IKFast) support getting multiple joint solutions for a single pose. - * This can be enabled using the |DiscretizationMethods| enum and choosing an option that is not |NO_DISCRETIZATION|. - * - * @param ik_poses The desired pose of each tip link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param solutions A vector of valid joint vectors. This return has two variant behaviors: - * 1) Return a joint solution for every input |ik_poses|, e.g. multi-arm support - * 2) Return multiple joint solutions for a single |ik_poses| input, e.g. underconstrained IK - * TODO(dave): This dual behavior is confusing and should be changed in a future refactor of this API - * @param result A struct that reports the results of the query - * @param options An option struct which contains the type of redundancy discretization used. This default - * implementation only supports the KinematicSearches::NO_DISCRETIZATION method; requesting any - * other will result in failure. - * @return True if a valid set of solutions was found, false otherwise. - */ - virtual bool getPositionIK(const std::vector& ik_poses, - const std::vector& ik_seed_state, std::vector >& solutions, - KinematicsResult& result, const kinematics::KinematicsQueryOptions& options) const; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solution by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param solution the solution vector - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solution by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the - * current seed state - * @param solution the solution vector - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solution by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solution by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the - * current seed state - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; - - /** - * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles - * required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. - * Not necessary for most robots that have kinematic chains. - * This particular method is intended for "searching" for a solution by stepping through the redundancy - * (or other numerical routines). - * @param ik_poses the desired pose of each tip link, in the same order as the getTipFrames() vector - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the - * current seed state - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @param context_state (optional) the context in which this request is being made. - * The position values corresponding to joints in the current group may not match those in ik_seed_state. - * The values in ik_seed_state are the ones to use. The state is passed to provide the \em other joint values, - * in case they are needed for context, like with an IK solver that computes a balanced result for a biped. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const moveit::core::RobotState* context_state = nullptr) const - { - (void)context_state; - // For IK solvers that do not support multiple poses, fall back to single pose call - if (ik_poses.size() == 1) - { - // Check if a solution_callback function was provided and call the corresponding function - if (solution_callback) - { - return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback, - error_code, options); - } - else - { - return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code, options); - } - } - - // Otherwise throw error because this function should have been implemented - RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), - "This kinematic solver does not support searchPositionIK with multiple poses"); - return false; - } - - /** - * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles - * required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. - * Not necessary for most robots that have kinematic chains. - * This particular method is intended for "searching" for a solution by stepping through the redundancy - * (or other numerical routines). - * @param ik_poses the desired pose of each tip link, in the same order as the getTipFrames() vector - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param timeout The amount of time (in seconds) available to the solver - * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the - * current seed state - * @param solution the solution vector - * @param solution_callback A callback to validate an IK solution - * @param cost_function A function to evaluate the cost of a particular IK solution - * @param error_code an error code that encodes the reason for failure or success - * @param options container for other IK options. See definition of KinematicsQueryOptions for details. - * @param context_state (optional) the context in which this request is being made. - * The position values corresponding to joints in the current group may not match those in ik_seed_state. - * The values in ik_seed_state are the ones to use. The state is passed to provide the \em other joint values, - * in case they are needed for context, like with an IK solver that computes a balanced result for a biped. - * @return True if a valid solution was found, false otherwise - */ - virtual bool - searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, const IKCostFn& cost_function, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const moveit::core::RobotState* context_state = nullptr) const - { - // if cost function is empty, omit - if (!cost_function) - { - return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, - error_code, options, context_state); - } - RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), - "This kinematic solver does not support IK solution cost functions"); - return false; - } - - /** - * @brief Given a set of joint angles and a set of links, compute their pose - * @param link_names A set of links for which FK needs to be computed - * @param joint_angles The state for which FK is being computed - * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) - * @return True if a valid solution was found, false otherwise - */ - virtual bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const = 0; - - /** - * @brief Set the parameters for the solver, for use with non-chain IK solvers - * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for; - * For example, the name of the ROS parameter that contains the robot description; - * @param group_name The group for which this solver is being configured - * @param base_frame The base frame in which all input poses are expected. - * This may (or may not) be the root frame of the chain that the solver operates on - * @param tip_frames A vector of tips of the kinematic tree - * @param search_discretization The discretization of the search when the solver steps through the redundancy - */ - virtual void setValues(const std::string& robot_description, const std::string& group_name, - const std::string& base_frame, const std::vector& tip_frames, - double search_discretization); - - /** - * @brief Initialization function for the kinematics, for use with kinematic chain IK solvers - * @param robot_model - allow the URDF to be loaded much quicker by passing in a pre-parsed model of the robot - * @param group_name The group for which this solver is being configured - * @param base_frame The base frame in which all input poses are expected. - * This may (or may not) be the root frame of the chain that the solver operates on - * @param tip_frames The tip of the chain - * @param search_discretization The discretization of the search when the solver steps through the redundancy - * @return true if initialization was successful, false otherwise - * - * Default implementation returns false and issues a warning to implement this new API. - * TODO: Make this method purely virtual after some soaking time, replacing the fallback. - */ - virtual bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization); - - /** - * @brief Return the name of the group that the solver is operating on - * @return The string name of the group that the solver is operating on - */ - virtual const std::string& getGroupName() const - { - return group_name_; - } - - /** - * @brief Return the name of the frame in which the solver is operating. This is usually a link name. - * No namespacing (e.g., no "/" prefix) should be used. - * @return The string name of the frame in which the solver is operating - */ - virtual const std::string& getBaseFrame() const - { - return base_frame_; - } - - /** - * @brief Return the name of the tip frame of the chain on which the solver is operating. - * This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. - * @return The string name of the tip frame of the chain on which the solver is operating - */ - virtual const std::string& getTipFrame() const - { - if (tip_frames_.size() > 1) - { - RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), - "This kinematic solver has more than one tip frame, " - "do not call getTipFrame()"); - } - - return tip_frames_[0]; - } - - /** - * @brief Return the names of the tip frames of the kinematic tree on which the solver is operating. - * This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. - * @return The vector of names of the tip frames of the kinematic tree on which the solver is operating - */ - virtual const std::vector& getTipFrames() const - { - return tip_frames_; - } - - /** - * @brief Set a set of redundant joints for the kinematics solver to use. - * This can fail, depending on the IK solver and choice of redundant joints!. Also, it sets - * the discretization values for each redundant joint to a default value. - * @param redundant_joint_indices The set of redundant joint indices - * (corresponding to the list of joints you get from getJointNames()). - * @return False if any of the input joint indices are invalid (exceed number of joints) - */ - virtual bool setRedundantJoints(const std::vector& redundant_joint_indices); - - /** - * @brief Set a set of redundant joints for the kinematics solver to use. - * This function is just a convenience function that calls the previous definition of setRedundantJoints() - * @param redundant_joint_names The set of redundant joint names. - * @return False if any of the input joint indices are invalid (exceed number of joints) - */ - bool setRedundantJoints(const std::vector& redundant_joint_names); - - /** - * @brief Get the set of redundant joints - */ - virtual void getRedundantJoints(std::vector& redundant_joint_indices) const - { - redundant_joint_indices = redundant_joint_indices_; - } - - /** - * @brief Return all the joint names in the order they are used internally - */ - virtual const std::vector& getJointNames() const = 0; - - /** - * @brief Return all the link names in the order they are represented internally - */ - virtual const std::vector& getLinkNames() const = 0; - - /** - * \brief Check if this solver supports a given JointModelGroup. - * - * Override this function to check if your kinematics solver - * implementation supports the given group. - * - * The default implementation just returns jmg->isChain(), since - * solvers written before this function was added all supported only - * chain groups. - * - * \param jmg the planning group being proposed to be solved by this IK solver - * \param error_text_out If this pointer is non-null and the group is - * not supported, this is filled with a description of why it's not - * supported. - * \return True if the group is supported, false if not. - */ - virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = nullptr) const; - - /** - * @brief Set the search discretization value for all the redundant joints - */ - void setSearchDiscretization(double sd) - { - redundant_joint_discretization_.clear(); - for (unsigned int index : redundant_joint_indices_) - redundant_joint_discretization_[index] = sd; - } - - /** - * @brief Sets individual discretization values for each redundant joint. - * - * Calling this method replaces previous discretization settings. - * - * @param discretization a map of joint indices and discretization value pairs. - */ - void setSearchDiscretization(const std::map& discretization) - { - redundant_joint_discretization_.clear(); - redundant_joint_indices_.clear(); - for (const auto& pair : discretization) - { - redundant_joint_discretization_.insert(pair); - redundant_joint_indices_.push_back(pair.first); - } - } - - /** - * @brief Get the value of the search discretization - */ - double getSearchDiscretization(int joint_index = 0) const - { - if (redundant_joint_discretization_.count(joint_index) > 0) - { - return redundant_joint_discretization_.at(joint_index); - } - else - { - return 0.0; // returned when there aren't any redundant joints - } - } - - /** - * @brief Returns the set of supported kinematics discretization search types. This implementation only supports - * the DiscretizationMethods::ONE search. - */ - std::vector getSupportedDiscretizationMethods() const - { - return supported_methods_; - } - - /** @brief For functions that require a timeout specified but one is not specified using arguments, - a default timeout is used, as set by this function (and initialized to KinematicsBase::DEFAULT_TIMEOUT) */ - void setDefaultTimeout(double timeout) - { - default_timeout_ = timeout; - } - - /** @brief For functions that require a timeout specified but one is not specified using arguments, - this default timeout is used */ - double getDefaultTimeout() const - { - return default_timeout_; - } - - /** - * @brief Virtual destructor for the interface - */ - virtual ~KinematicsBase(); - - KinematicsBase(); - -protected: - rclcpp::Node::SharedPtr node_; - moveit::core::RobotModelConstPtr robot_model_; - std::string robot_description_; - std::string group_name_; - std::string base_frame_; - std::vector tip_frames_; - - double default_timeout_; - std::vector redundant_joint_indices_; - std::map redundant_joint_discretization_; - std::vector supported_methods_; - - /** Store some core variables passed via initialize(). - * - * @param robot_model RobotModel, this kinematics solver should act on. - * @param group_name The group for which this solver is being configured. - * @param base_frame The base frame in which all input poses are expected. - * @param tip_frames The tips of the kinematics tree. - * @param search_discretization The discretization of the search when the solver steps through the redundancy - */ - void storeValues(const moveit::core::RobotModel& robot_model, const std::string& group_name, - const std::string& base_frame, const std::vector& tip_frames, - double search_discretization); - -private: - std::string removeSlash(const std::string& str) const; -}; -} // namespace kinematics +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.hpp b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.hpp new file mode 100644 index 0000000000..4f674dd9fc --- /dev/null +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.hpp @@ -0,0 +1,613 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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: Sachin Chitta, Dave Coleman */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace moveit +{ +namespace core +{ +MOVEIT_CLASS_FORWARD(JointModelGroup); +MOVEIT_CLASS_FORWARD(RobotState); +MOVEIT_CLASS_FORWARD(RobotModel); +} // namespace core +} // namespace moveit + +/** @brief API for forward and inverse kinematics */ +namespace kinematics +{ +/* + * @enum DiscretizationMethods + * + * @brief Flags for choosing the type discretization method applied on the redundant joints during an ik query + */ +namespace DiscretizationMethods +{ +enum DiscretizationMethod +{ + NO_DISCRETIZATION = 1, /**< The redundant joints will be fixed at their current value. */ + ALL_DISCRETIZED, /**< All redundant joints will be discretized uniformly */ + SOME_DISCRETIZED, /**< Some redundant joints will be discretized uniformly. The unused redundant joints will be fixed + at their + current value */ + ALL_RANDOM_SAMPLED, /**< the discretization for each redundant joint will be randomly generated.*/ + SOME_RANDOM_SAMPLED /**< the discretization for some redundant joint will be randomly generated. + The unused redundant joints will be fixed at their current value. */ +}; +} // namespace DiscretizationMethods +using DiscretizationMethod = DiscretizationMethods::DiscretizationMethod; + +/* + * @enum KinematicErrors + * @brief Kinematic error codes that occur in a ik query + */ +namespace KinematicErrors +{ +enum KinematicError +{ + OK = 1, /**< No errors*/ + UNSUPORTED_DISCRETIZATION_REQUESTED, /**< Discretization method isn't supported by this implementation */ + DISCRETIZATION_NOT_INITIALIZED, /**< Discretization values for the redundancy has not been set. See + setSearchDiscretization(...) method*/ + MULTIPLE_TIPS_NOT_SUPPORTED, /**< Only single tip link support is allowed */ + EMPTY_TIP_POSES, /**< Empty ik_poses array passed */ + IK_SEED_OUTSIDE_LIMITS, /**< Ik seed is out of bounds*/ + SOLVER_NOT_ACTIVE, /**< Solver isn't active */ + NO_SOLUTION /**< A valid joint solution that can reach this pose(s) could not be found */ + +}; +} // namespace KinematicErrors +using KinematicError = KinematicErrors::KinematicError; + +/** + * @struct KinematicsQueryOptions + * @brief A set of options for the kinematics solver + */ +struct KinematicsQueryOptions +{ + KinematicsQueryOptions() + : lock_redundant_joints(false) + , return_approximate_solution(false) + , discretization_method(DiscretizationMethods::NO_DISCRETIZATION) + { + } + + bool lock_redundant_joints; /**< KinematicsQueryOptions#lock_redundant_joints. */ + bool return_approximate_solution; /**< KinematicsQueryOptions#return_approximate_solution. */ + DiscretizationMethod discretization_method; /**< Enumeration value that indicates the method for discretizing the + redundant. joints KinematicsQueryOptions#discretization_method. */ +}; + +/* + * @struct KinematicsResult + * @brief Reports result details of an ik query + * + * This struct is used as an output argument of the getPositionIK(...) method that returns multiple joint solutions. + * It contains the type of error that led to a failure or KinematicErrors::OK when a set of joint solutions is found. + * The solution percentage shall provide a ratio of solutions found over solutions searched. + * + */ +struct KinematicsResult +{ + KinematicError kinematic_error; /**< Error code that indicates the type of failure */ + double solution_percentage; /**< The percentage of solutions achieved over the total number + of solutions explored. */ +}; + +MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, WeakPtr... etc + +/** + * @class KinematicsBase + * @brief Provides an interface for kinematics solvers. + */ +class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase +{ +public: + static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */ + static const double DEFAULT_TIMEOUT; /* = 1.0 */ + + /** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */ + using IKCallbackFn = std::function&, + moveit_msgs::msg::MoveItErrorCodes&)>; + + /** @brief Signature for a cost function used to evaluate IK solutions. */ + using IKCostFn = std::function&)>; + + /** + * @brief Given a desired pose of the end-effector, compute the joint angles to reach it + * + * In contrast to the searchPositionIK methods, this one is expected to return the solution + * closest to the seed state. Randomly re-seeding is explicitly not allowed. + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solution the solution vector + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; + + /** + * @brief Given the desired poses of all end-effectors, compute joint angles that are able to reach it. + * + * The default implementation returns only one solution and so its result is equivalent to calling + * 'getPositionIK(...)' with a zero initialized seed. + * + * Some planners (e.g. IKFast) support getting multiple joint solutions for a single pose. + * This can be enabled using the |DiscretizationMethods| enum and choosing an option that is not |NO_DISCRETIZATION|. + * + * @param ik_poses The desired pose of each tip link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solutions A vector of valid joint vectors. This return has two variant behaviors: + * 1) Return a joint solution for every input |ik_poses|, e.g. multi-arm support + * 2) Return multiple joint solutions for a single |ik_poses| input, e.g. underconstrained IK + * TODO(dave): This dual behavior is confusing and should be changed in a future refactor of this API + * @param result A struct that reports the results of the query + * @param options An option struct which contains the type of redundancy discretization used. This default + * implementation only supports the KinematicSearches::NO_DISCRETIZATION method; requesting any + * other will result in failure. + * @return True if a valid set of solutions was found, false otherwise. + */ + virtual bool getPositionIK(const std::vector& ik_poses, + const std::vector& ik_seed_state, std::vector >& solutions, + KinematicsResult& result, const kinematics::KinematicsQueryOptions& options) const; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solution by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param solution the solution vector + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solution by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the + * current seed state + * @param solution the solution vector + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solution by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param solution the solution vector + * @param solution_callback A callback to validate an IK solution + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solution by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the + * current seed state + * @param solution the solution vector + * @param solution_callback A callback to validate an IK solution + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0; + + /** + * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles + * required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. + * Not necessary for most robots that have kinematic chains. + * This particular method is intended for "searching" for a solution by stepping through the redundancy + * (or other numerical routines). + * @param ik_poses the desired pose of each tip link, in the same order as the getTipFrames() vector + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the + * current seed state + * @param solution the solution vector + * @param solution_callback A callback to validate an IK solution + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @param context_state (optional) the context in which this request is being made. + * The position values corresponding to joints in the current group may not match those in ik_seed_state. + * The values in ik_seed_state are the ones to use. The state is passed to provide the \em other joint values, + * in case they are needed for context, like with an IK solver that computes a balanced result for a biped. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const moveit::core::RobotState* context_state = nullptr) const + { + (void)context_state; + // For IK solvers that do not support multiple poses, fall back to single pose call + if (ik_poses.size() == 1) + { + // Check if a solution_callback function was provided and call the corresponding function + if (solution_callback) + { + return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback, + error_code, options); + } + else + { + return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code, options); + } + } + + // Otherwise throw error because this function should have been implemented + RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), + "This kinematic solver does not support searchPositionIK with multiple poses"); + return false; + } + + /** + * @brief Given a set of desired poses for a planning group with multiple end-effectors, search for the joint angles + * required to reach them. This is useful for e.g. biped robots that need to perform whole-body IK. + * Not necessary for most robots that have kinematic chains. + * This particular method is intended for "searching" for a solution by stepping through the redundancy + * (or other numerical routines). + * @param ik_poses the desired pose of each tip link, in the same order as the getTipFrames() vector + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param timeout The amount of time (in seconds) available to the solver + * @param consistency_limits the distance that any joint in the solution can be from the corresponding joints in the + * current seed state + * @param solution the solution vector + * @param solution_callback A callback to validate an IK solution + * @param cost_function A function to evaluate the cost of a particular IK solution + * @param error_code an error code that encodes the reason for failure or success + * @param options container for other IK options. See definition of KinematicsQueryOptions for details. + * @param context_state (optional) the context in which this request is being made. + * The position values corresponding to joints in the current group may not match those in ik_seed_state. + * The values in ik_seed_state are the ones to use. The state is passed to provide the \em other joint values, + * in case they are needed for context, like with an IK solver that computes a balanced result for a biped. + * @return True if a valid solution was found, false otherwise + */ + virtual bool + searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, const IKCostFn& cost_function, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const moveit::core::RobotState* context_state = nullptr) const + { + // if cost function is empty, omit + if (!cost_function) + { + return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, + error_code, options, context_state); + } + RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), + "This kinematic solver does not support IK solution cost functions"); + return false; + } + + /** + * @brief Given a set of joint angles and a set of links, compute their pose + * @param link_names A set of links for which FK needs to be computed + * @param joint_angles The state for which FK is being computed + * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) + * @return True if a valid solution was found, false otherwise + */ + virtual bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const = 0; + + /** + * @brief Set the parameters for the solver, for use with non-chain IK solvers + * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for; + * For example, the name of the ROS parameter that contains the robot description; + * @param group_name The group for which this solver is being configured + * @param base_frame The base frame in which all input poses are expected. + * This may (or may not) be the root frame of the chain that the solver operates on + * @param tip_frames A vector of tips of the kinematic tree + * @param search_discretization The discretization of the search when the solver steps through the redundancy + */ + virtual void setValues(const std::string& robot_description, const std::string& group_name, + const std::string& base_frame, const std::vector& tip_frames, + double search_discretization); + + /** + * @brief Initialization function for the kinematics, for use with kinematic chain IK solvers + * @param robot_model - allow the URDF to be loaded much quicker by passing in a pre-parsed model of the robot + * @param group_name The group for which this solver is being configured + * @param base_frame The base frame in which all input poses are expected. + * This may (or may not) be the root frame of the chain that the solver operates on + * @param tip_frames The tip of the chain + * @param search_discretization The discretization of the search when the solver steps through the redundancy + * @return true if initialization was successful, false otherwise + * + * Default implementation returns false and issues a warning to implement this new API. + * TODO: Make this method purely virtual after some soaking time, replacing the fallback. + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization); + + /** + * @brief Return the name of the group that the solver is operating on + * @return The string name of the group that the solver is operating on + */ + virtual const std::string& getGroupName() const + { + return group_name_; + } + + /** + * @brief Return the name of the frame in which the solver is operating. This is usually a link name. + * No namespacing (e.g., no "/" prefix) should be used. + * @return The string name of the frame in which the solver is operating + */ + virtual const std::string& getBaseFrame() const + { + return base_frame_; + } + + /** + * @brief Return the name of the tip frame of the chain on which the solver is operating. + * This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * @return The string name of the tip frame of the chain on which the solver is operating + */ + virtual const std::string& getTipFrame() const + { + if (tip_frames_.size() > 1) + { + RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), + "This kinematic solver has more than one tip frame, " + "do not call getTipFrame()"); + } + + return tip_frames_[0]; + } + + /** + * @brief Return the names of the tip frames of the kinematic tree on which the solver is operating. + * This is usually a link name. No namespacing (e.g., no "/" prefix) should be used. + * @return The vector of names of the tip frames of the kinematic tree on which the solver is operating + */ + virtual const std::vector& getTipFrames() const + { + return tip_frames_; + } + + /** + * @brief Set a set of redundant joints for the kinematics solver to use. + * This can fail, depending on the IK solver and choice of redundant joints!. Also, it sets + * the discretization values for each redundant joint to a default value. + * @param redundant_joint_indices The set of redundant joint indices + * (corresponding to the list of joints you get from getJointNames()). + * @return False if any of the input joint indices are invalid (exceed number of joints) + */ + virtual bool setRedundantJoints(const std::vector& redundant_joint_indices); + + /** + * @brief Set a set of redundant joints for the kinematics solver to use. + * This function is just a convenience function that calls the previous definition of setRedundantJoints() + * @param redundant_joint_names The set of redundant joint names. + * @return False if any of the input joint indices are invalid (exceed number of joints) + */ + bool setRedundantJoints(const std::vector& redundant_joint_names); + + /** + * @brief Get the set of redundant joints + */ + virtual void getRedundantJoints(std::vector& redundant_joint_indices) const + { + redundant_joint_indices = redundant_joint_indices_; + } + + /** + * @brief Return all the joint names in the order they are used internally + */ + virtual const std::vector& getJointNames() const = 0; + + /** + * @brief Return all the link names in the order they are represented internally + */ + virtual const std::vector& getLinkNames() const = 0; + + /** + * \brief Check if this solver supports a given JointModelGroup. + * + * Override this function to check if your kinematics solver + * implementation supports the given group. + * + * The default implementation just returns jmg->isChain(), since + * solvers written before this function was added all supported only + * chain groups. + * + * \param jmg the planning group being proposed to be solved by this IK solver + * \param error_text_out If this pointer is non-null and the group is + * not supported, this is filled with a description of why it's not + * supported. + * \return True if the group is supported, false if not. + */ + virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = nullptr) const; + + /** + * @brief Set the search discretization value for all the redundant joints + */ + void setSearchDiscretization(double sd) + { + redundant_joint_discretization_.clear(); + for (unsigned int index : redundant_joint_indices_) + redundant_joint_discretization_[index] = sd; + } + + /** + * @brief Sets individual discretization values for each redundant joint. + * + * Calling this method replaces previous discretization settings. + * + * @param discretization a map of joint indices and discretization value pairs. + */ + void setSearchDiscretization(const std::map& discretization) + { + redundant_joint_discretization_.clear(); + redundant_joint_indices_.clear(); + for (const auto& pair : discretization) + { + redundant_joint_discretization_.insert(pair); + redundant_joint_indices_.push_back(pair.first); + } + } + + /** + * @brief Get the value of the search discretization + */ + double getSearchDiscretization(int joint_index = 0) const + { + if (redundant_joint_discretization_.count(joint_index) > 0) + { + return redundant_joint_discretization_.at(joint_index); + } + else + { + return 0.0; // returned when there aren't any redundant joints + } + } + + /** + * @brief Returns the set of supported kinematics discretization search types. This implementation only supports + * the DiscretizationMethods::ONE search. + */ + std::vector getSupportedDiscretizationMethods() const + { + return supported_methods_; + } + + /** @brief For functions that require a timeout specified but one is not specified using arguments, + a default timeout is used, as set by this function (and initialized to KinematicsBase::DEFAULT_TIMEOUT) */ + void setDefaultTimeout(double timeout) + { + default_timeout_ = timeout; + } + + /** @brief For functions that require a timeout specified but one is not specified using arguments, + this default timeout is used */ + double getDefaultTimeout() const + { + return default_timeout_; + } + + /** + * @brief Virtual destructor for the interface + */ + virtual ~KinematicsBase(); + + KinematicsBase(); + +protected: + rclcpp::Node::SharedPtr node_; + moveit::core::RobotModelConstPtr robot_model_; + std::string robot_description_; + std::string group_name_; + std::string base_frame_; + std::vector tip_frames_; + + double default_timeout_; + std::vector redundant_joint_indices_; + std::map redundant_joint_discretization_; + std::vector supported_methods_; + + /** Store some core variables passed via initialize(). + * + * @param robot_model RobotModel, this kinematics solver should act on. + * @param group_name The group for which this solver is being configured. + * @param base_frame The base frame in which all input poses are expected. + * @param tip_frames The tips of the kinematics tree. + * @param search_discretization The discretization of the search when the solver steps through the redundancy + */ + void storeValues(const moveit::core::RobotModel& robot_model, const std::string& group_name, + const std::string& base_frame, const std::vector& tip_frames, + double search_discretization); + +private: + std::string removeSlash(const std::string& str) const; +}; +} // namespace kinematics diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index 3b8d61854d..a97c7f96d2 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -34,8 +34,8 @@ /* Author: Sachin Chitta, Dave Coleman */ -#include -#include +#include +#include #include #include diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index daabe12ccb..901b92a559 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,124 +47,5 @@ /* Author: Sachin Chitta */ #pragma once - -#include - -/** @brief Namespace for kinematics metrics */ -namespace kinematics_metrics -{ -MOVEIT_CLASS_FORWARD(KinematicsMetrics); // Defines KinematicsMetricsPtr, ConstPtr, WeakPtr... etc - -/** - * \brief Compute different kinds of metrics for kinematics evaluation. Currently includes - * manipulability. - */ -class KinematicsMetrics -{ -public: - /** \brief Construct a KinematicsMetricss from a RobotModel */ - KinematicsMetrics(const moveit::core::RobotModelConstPtr& robot_model) - : robot_model_(robot_model), penalty_multiplier_(0.0) - { - } - - /** - * @brief Get the manipulability for a given group at a given joint configuration - * @param state Complete kinematic state for the robot - * @param group_name The group name (e.g. "arm") - * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) - * @return False if the group was not found - */ - bool getManipulabilityIndex(const moveit::core::RobotState& state, const std::string& group_name, - double& manipulability_index, bool translation = false) const; - - /** - * @brief Get the manipulability for a given group at a given joint configuration - * @param state Complete kinematic state for the robot - * @param joint_model_group A pointer to the desired joint model group - * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) - * @return False if the group was not found - */ - bool getManipulabilityIndex(const moveit::core::RobotState& state, - const moveit::core::JointModelGroup* joint_model_group, double& manipulability_index, - bool translation = false) const; - - /** - * @brief Get the (translation) manipulability ellipsoid for a given group at a given joint configuration - * @param state Complete kinematic state for the robot - * @param group_name The group name (e.g. "arm") - * @param eigen_values The eigen values for the translation part of JJ^T - * @param eigen_vectors The eigen vectors for the translation part of JJ^T - * @return False if the group was not found - */ - bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, const std::string& group_name, - Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; - - /** - * @brief Get the (translation) manipulability ellipsoid for a given group at a given joint configuration - * @param state Complete kinematic state for the robot - * @param joint_model_group A pointer to the desired joint model group - * @param eigen_values The eigen values for the translation part of JJ^T - * @param eigen_vectors The eigen vectors for the translation part of JJ^T - * @return False if the group was not found - */ - bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, - const moveit::core::JointModelGroup* joint_model_group, - Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; - - /** - * @brief Get the manipulability = sigma_min/sigma_max - * where sigma_min and sigma_max are the smallest and largest singular values - * of the Jacobian matrix J - * @param state Complete kinematic state for the robot - * @param group_name The group name (e.g. "arm") - * @param condition_number Condition number for JJ^T - * @return False if the group was not found - */ - bool getManipulability(const moveit::core::RobotState& state, const std::string& group_name, double& condition_number, - bool translation = false) const; - - /** - * @brief Get the manipulability = sigma_min/sigma_max - * where sigma_min and sigma_max are the smallest and largest singular values - * of the Jacobian matrix J - * @param state Complete kinematic state for the robot - * @param joint_model_group A pointer to the desired joint model group - * @param condition_number Condition number for JJ^T - * @return False if the group was not found - */ - bool getManipulability(const moveit::core::RobotState& state, const moveit::core::JointModelGroup* joint_model_group, - double& condition_number, bool translation = false) const; - - void setPenaltyMultiplier(double multiplier) - { - penalty_multiplier_ = fabs(multiplier); - } - - double getPenaltyMultiplier() const - { - return penalty_multiplier_; - } - -protected: - moveit::core::RobotModelConstPtr robot_model_; - -private: - /** - * @brief Defines a multiplier for the manipulabilty - * = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * - * distance_to_higher_limit/(joint_range*joint_range))) - * where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with - * finite bounds - * are considered. - * Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. - * See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, - * Ohio State University, 1986, for more details. - * @return multiplier that is multiplied with every manipulability measure computed here - */ - double getJointLimitsPenalty(const moveit::core::RobotState& state, - const moveit::core::JointModelGroup* joint_model_group) const; - - double penalty_multiplier_; -}; -} // namespace kinematics_metrics +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.hpp b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.hpp new file mode 100644 index 0000000000..4e27a4dfd4 --- /dev/null +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.hpp @@ -0,0 +1,158 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Sachin Chitta */ + +#pragma once + +#include + +/** @brief Namespace for kinematics metrics */ +namespace kinematics_metrics +{ +MOVEIT_CLASS_FORWARD(KinematicsMetrics); // Defines KinematicsMetricsPtr, ConstPtr, WeakPtr... etc + +/** + * \brief Compute different kinds of metrics for kinematics evaluation. Currently includes + * manipulability. + */ +class KinematicsMetrics +{ +public: + /** \brief Construct a KinematicsMetricss from a RobotModel */ + KinematicsMetrics(const moveit::core::RobotModelConstPtr& robot_model) + : robot_model_(robot_model), penalty_multiplier_(0.0) + { + } + + /** + * @brief Get the manipulability for a given group at a given joint configuration + * @param state Complete kinematic state for the robot + * @param group_name The group name (e.g. "arm") + * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) + * @return False if the group was not found + */ + bool getManipulabilityIndex(const moveit::core::RobotState& state, const std::string& group_name, + double& manipulability_index, bool translation = false) const; + + /** + * @brief Get the manipulability for a given group at a given joint configuration + * @param state Complete kinematic state for the robot + * @param joint_model_group A pointer to the desired joint model group + * @param manipulability_index The computed manipulability = sqrt(det(JJ^T)) + * @return False if the group was not found + */ + bool getManipulabilityIndex(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, double& manipulability_index, + bool translation = false) const; + + /** + * @brief Get the (translation) manipulability ellipsoid for a given group at a given joint configuration + * @param state Complete kinematic state for the robot + * @param group_name The group name (e.g. "arm") + * @param eigen_values The eigen values for the translation part of JJ^T + * @param eigen_vectors The eigen vectors for the translation part of JJ^T + * @return False if the group was not found + */ + bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, const std::string& group_name, + Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; + + /** + * @brief Get the (translation) manipulability ellipsoid for a given group at a given joint configuration + * @param state Complete kinematic state for the robot + * @param joint_model_group A pointer to the desired joint model group + * @param eigen_values The eigen values for the translation part of JJ^T + * @param eigen_vectors The eigen vectors for the translation part of JJ^T + * @return False if the group was not found + */ + bool getManipulabilityEllipsoid(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group, + Eigen::MatrixXcd& eigen_values, Eigen::MatrixXcd& eigen_vectors) const; + + /** + * @brief Get the manipulability = sigma_min/sigma_max + * where sigma_min and sigma_max are the smallest and largest singular values + * of the Jacobian matrix J + * @param state Complete kinematic state for the robot + * @param group_name The group name (e.g. "arm") + * @param condition_number Condition number for JJ^T + * @return False if the group was not found + */ + bool getManipulability(const moveit::core::RobotState& state, const std::string& group_name, double& condition_number, + bool translation = false) const; + + /** + * @brief Get the manipulability = sigma_min/sigma_max + * where sigma_min and sigma_max are the smallest and largest singular values + * of the Jacobian matrix J + * @param state Complete kinematic state for the robot + * @param joint_model_group A pointer to the desired joint model group + * @param condition_number Condition number for JJ^T + * @return False if the group was not found + */ + bool getManipulability(const moveit::core::RobotState& state, const moveit::core::JointModelGroup* joint_model_group, + double& condition_number, bool translation = false) const; + + void setPenaltyMultiplier(double multiplier) + { + penalty_multiplier_ = fabs(multiplier); + } + + double getPenaltyMultiplier() const + { + return penalty_multiplier_; + } + +protected: + moveit::core::RobotModelConstPtr robot_model_; + +private: + /** + * @brief Defines a multiplier for the manipulabilty + * = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * + * distance_to_higher_limit/(joint_range*joint_range))) + * where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with + * finite bounds + * are considered. + * Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. + * See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, + * Ohio State University, 1986, for more details. + * @return multiplier that is multiplied with every manipulability measure computed here + */ + double getJointLimitsPenalty(const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* joint_model_group) const; + + double penalty_multiplier_; +}; +} // namespace kinematics_metrics diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index c6ab4eaf44..10023dedb2 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/moveit_core/macros/include/moveit/macros/class_forward.h b/moveit_core/macros/include/moveit/macros/class_forward.h index c4f1cbf6d6..b618646d2d 100644 --- a/moveit_core/macros/include/moveit/macros/class_forward.h +++ b/moveit_core/macros/include/moveit/macros/class_forward.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,23 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -/** - * \def MOVEIT_CLASS_FORWARD - * Macro that forward declares a class and defines the respective smartpointers through MOVEIT_DECLARE_PTR. - */ - -#define MOVEIT_CLASS_FORWARD(C) \ - class C; \ - MOVEIT_DECLARE_PTR(C, C) - -/** - * \def MOVEIT_STRUCT_FORWARD - * Like MOVEIT_CLASS_FORWARD, but forward declares the type as a struct - * instead of a class. - */ -#define MOVEIT_STRUCT_FORWARD(C) \ - struct C; \ - MOVEIT_DECLARE_PTR(C, C) +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/macros/include/moveit/macros/class_forward.hpp b/moveit_core/macros/include/moveit/macros/class_forward.hpp new file mode 100644 index 0000000000..44db6cd231 --- /dev/null +++ b/moveit_core/macros/include/moveit/macros/class_forward.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +/** + * \def MOVEIT_CLASS_FORWARD + * Macro that forward declares a class and defines the respective smartpointers through MOVEIT_DECLARE_PTR. + */ + +#define MOVEIT_CLASS_FORWARD(C) \ + class C; \ + MOVEIT_DECLARE_PTR(C, C) + +/** + * \def MOVEIT_STRUCT_FORWARD + * Like MOVEIT_CLASS_FORWARD, but forward declares the type as a struct + * instead of a class. + */ +#define MOVEIT_STRUCT_FORWARD(C) \ + struct C; \ + MOVEIT_DECLARE_PTR(C, C) diff --git a/moveit_core/macros/include/moveit/macros/console_colors.h b/moveit_core/macros/include/moveit/macros/console_colors.h index 0da6ae80de..be091522f4 100644 --- a/moveit_core/macros/include/moveit/macros/console_colors.h +++ b/moveit_core/macros/include/moveit/macros/console_colors.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,13 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#define MOVEIT_CONSOLE_COLOR_RESET "\033[0m" -#define MOVEIT_CONSOLE_COLOR_GRAY "\033[90m" -#define MOVEIT_CONSOLE_COLOR_RED "\033[91m" -#define MOVEIT_CONSOLE_COLOR_GREEN "\033[92m" -#define MOVEIT_CONSOLE_COLOR_BROWN "\033[93m" // same as yellow -#define MOVEIT_CONSOLE_COLOR_YELLOW "\033[93m" -#define MOVEIT_CONSOLE_COLOR_BLUE "\033[94m" -#define MOVEIT_CONSOLE_COLOR_PURPLE "\033[95m" -#define MOVEIT_CONSOLE_COLOR_CYAN "\033[96m" +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/macros/include/moveit/macros/console_colors.hpp b/moveit_core/macros/include/moveit/macros/console_colors.hpp new file mode 100644 index 0000000000..0da6ae80de --- /dev/null +++ b/moveit_core/macros/include/moveit/macros/console_colors.hpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#define MOVEIT_CONSOLE_COLOR_RESET "\033[0m" +#define MOVEIT_CONSOLE_COLOR_GRAY "\033[90m" +#define MOVEIT_CONSOLE_COLOR_RED "\033[91m" +#define MOVEIT_CONSOLE_COLOR_GREEN "\033[92m" +#define MOVEIT_CONSOLE_COLOR_BROWN "\033[93m" // same as yellow +#define MOVEIT_CONSOLE_COLOR_YELLOW "\033[93m" +#define MOVEIT_CONSOLE_COLOR_BLUE "\033[94m" +#define MOVEIT_CONSOLE_COLOR_PURPLE "\033[95m" +#define MOVEIT_CONSOLE_COLOR_CYAN "\033[96m" diff --git a/moveit_core/macros/include/moveit/macros/declare_ptr.h b/moveit_core/macros/include/moveit/macros/declare_ptr.h index 649af49875..1fcca3e238 100644 --- a/moveit_core/macros/include/moveit/macros/declare_ptr.h +++ b/moveit_core/macros/include/moveit/macros/declare_ptr.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,44 +45,5 @@ *********************************************************************/ #pragma once - -#include - -/** - * \def MOVEIT_DELCARE_PTR - * Macro that given a Name and a Type declares the following types: - * - ${Name}Ptr = shared_ptr<${Type}> - * - ${Name}ConstPtr = shared_ptr - * - ${Name}WeakPtr = weak_ptr<${Type}> - * - ${Name}ConstWeakPtr = weak_ptr - * - ${Name}UniquePtr = unique_ptr<${Type}> - * - ${Name}ConstUniquePtr = unique_ptr - * - * For best portability the exact type of shared_ptr declared by the macro - * should be considered to be an implementation detail, liable to change in - * future releases. - */ - -#define MOVEIT_DECLARE_PTR(Name, Type) \ - typedef std::shared_ptr Name##Ptr; \ - typedef std::shared_ptr Name##ConstPtr; \ - typedef std::weak_ptr Name##WeakPtr; \ - typedef std::weak_ptr Name##ConstWeakPtr; \ - typedef std::unique_ptr Name##UniquePtr; \ - typedef std::unique_ptr Name##ConstUniquePtr - -/** - * \def MOVEIT_DELCARE_PTR_MEMBER - * The macro defines the same typedefs as MOVEIT_DECLARE_PTR, but shortens the new names to their suffix. - * - * This can be used to create `Classname::Ptr` style names, but in most situations in MoveIt's codebase, - * MOVEIT_CLASS_FORWARD and MOVEIT_DECLARE_PTR should be preferred. - */ - -#define MOVEIT_DECLARE_PTR_MEMBER(Type) \ - typedef std::shared_ptr Ptr; \ - typedef std::shared_ptr ConstPtr; \ - typedef std::weak_ptr WeakPtr; \ - typedef std::weak_ptr ConstWeakPtr; \ - typedef std::unique_ptr UniquePtr; \ - typedef std::unique_ptr ConstUniquePtr +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/macros/include/moveit/macros/declare_ptr.hpp b/moveit_core/macros/include/moveit/macros/declare_ptr.hpp new file mode 100644 index 0000000000..649af49875 --- /dev/null +++ b/moveit_core/macros/include/moveit/macros/declare_ptr.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Delft Robotics B.V. + * 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. + *********************************************************************/ + +#pragma once + +#include + +/** + * \def MOVEIT_DELCARE_PTR + * Macro that given a Name and a Type declares the following types: + * - ${Name}Ptr = shared_ptr<${Type}> + * - ${Name}ConstPtr = shared_ptr + * - ${Name}WeakPtr = weak_ptr<${Type}> + * - ${Name}ConstWeakPtr = weak_ptr + * - ${Name}UniquePtr = unique_ptr<${Type}> + * - ${Name}ConstUniquePtr = unique_ptr + * + * For best portability the exact type of shared_ptr declared by the macro + * should be considered to be an implementation detail, liable to change in + * future releases. + */ + +#define MOVEIT_DECLARE_PTR(Name, Type) \ + typedef std::shared_ptr Name##Ptr; \ + typedef std::shared_ptr Name##ConstPtr; \ + typedef std::weak_ptr Name##WeakPtr; \ + typedef std::weak_ptr Name##ConstWeakPtr; \ + typedef std::unique_ptr Name##UniquePtr; \ + typedef std::unique_ptr Name##ConstUniquePtr + +/** + * \def MOVEIT_DELCARE_PTR_MEMBER + * The macro defines the same typedefs as MOVEIT_DECLARE_PTR, but shortens the new names to their suffix. + * + * This can be used to create `Classname::Ptr` style names, but in most situations in MoveIt's codebase, + * MOVEIT_CLASS_FORWARD and MOVEIT_DECLARE_PTR should be preferred. + */ + +#define MOVEIT_DECLARE_PTR_MEMBER(Type) \ + typedef std::shared_ptr Ptr; \ + typedef std::shared_ptr ConstPtr; \ + typedef std::weak_ptr WeakPtr; \ + typedef std::weak_ptr ConstWeakPtr; \ + typedef std::unique_ptr UniquePtr; \ + typedef std::unique_ptr ConstUniquePtr diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h index 361b14852e..bdd2b4abde 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -71,92 +83,5 @@ c --------x--- v | */ #pragma once - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace online_signal_smoothing -{ -MOVEIT_STRUCT_FORWARD(OSQPDataWrapper); - -// Plugin -class AccelerationLimitedPlugin : public SmoothingBaseClass -{ -public: - /** - * Initialize the acceleration based smoothing plugin - * @param node ROS node, used for parameter retrieval - * @param robot_model typically used to retrieve vel/accel/jerk limits - * @param num_joints number of actuated joints in the JointGroup Servo controls - * @return True if initialization was successful - */ - bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, - size_t num_joints) override; - - /** - * Smooth the command signals for all DOF. This function limits the change in velocity using the acceleration - * specified in the robot model. - * @param positions array of joint position commands - * @param velocities array of joint velocity commands - * @param accelerations (unused) - * @return True if smoothing was successful - */ - bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; - - /** - * Reset to a given joint state. This method must be called before doSmoothing. - * @param positions reset the filters to the joint positions - * @param velocities reset the filters to the joint velocities - * @param accelerations (unused) - * @return True if reset was successful - */ - bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const Eigen::VectorXd& accelerations) override; - - /** - * memory allocated by osqp is freed in destructor - */ - ~AccelerationLimitedPlugin() override - { - if (osqp_workspace_ != nullptr) - { - osqp_cleanup(osqp_workspace_); - } - } - -private: - /** \brief Pointer to rclcpp node handle. */ - rclcpp::Node::SharedPtr node_; - /** \brief Parameters for plugin. */ - online_signal_smoothing::Params params_; - /** \brief The number of joints in the robot's planning group. */ - size_t num_joints_; - /** \brief Last velocities and positions received */ - Eigen::VectorXd last_velocities_; - Eigen::VectorXd last_positions_; - /** \brief Intermediate variables used in calculations */ - Eigen::VectorXd cur_acceleration_; - Eigen::VectorXd positions_offset_; - Eigen::VectorXd velocities_offset_; - /** \brief Extracted joint limits from robot model */ - Eigen::VectorXd max_acceleration_limits_; - Eigen::VectorXd min_acceleration_limits_; - /** \brief Pointer to robot model */ - moveit::core::RobotModelConstPtr robot_model_; - /** \brief Constraint matrix for optimization problem */ - Eigen::SparseMatrix constraints_sparse_; - /** \brief osqp types used for optimization problem */ - OSQPDataWrapperPtr osqp_data_; - OSQPWorkspace* osqp_workspace_ = nullptr; - OSQPSettings osqp_settings_; -}; -} // namespace online_signal_smoothing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.hpp b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.hpp new file mode 100644 index 0000000000..def9fa48ec --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.hpp @@ -0,0 +1,162 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik 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 PickNik Inc. 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: Paul Gesel +Description: applies smoothing by limiting the acceleration between consecutive commands. +The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes +to the servo command topics. + + In the diagrams below, the c-v lines show the displacement that will occur given the current velocity. The t-c lines + shows the displacement between the current position and the desired position. The dashed lines shows the maximum + possible displacements that are within the acceleration limits. The v-t lines shows the acceleration commands that + will be used by this acceleration-limiting plugin. The x point shows the position that will be used for each scenario. + +Scenario A: The desired position is within the acceleration limits. The next commanded point will be exactly the +desired point. + ________ + | | +c --|-----xt | + \__|__ v | + |________| + +Scenario B: The line between the current position and the desired position intersects the acceleration limits, but the +reference position is not within the bounds. The next commanded point will be the point on the displacement line that +is closest to the reference. + ________ + | | +c --|--------x------t + \__|__ v | + |________| + +Scenario C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within +the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while +maintaining its direction. + ________ + | | +c --------x--- v | + \ | | + \ |________| + t + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace online_signal_smoothing +{ +MOVEIT_STRUCT_FORWARD(OSQPDataWrapper); + +// Plugin +class AccelerationLimitedPlugin : public SmoothingBaseClass +{ +public: + /** + * Initialize the acceleration based smoothing plugin + * @param node ROS node, used for parameter retrieval + * @param robot_model typically used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup Servo controls + * @return True if initialization was successful + */ + bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) override; + + /** + * Smooth the command signals for all DOF. This function limits the change in velocity using the acceleration + * specified in the robot model. + * @param positions array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations (unused) + * @return True if smoothing was successful + */ + bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; + + /** + * Reset to a given joint state. This method must be called before doSmoothing. + * @param positions reset the filters to the joint positions + * @param velocities reset the filters to the joint velocities + * @param accelerations (unused) + * @return True if reset was successful + */ + bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) override; + + /** + * memory allocated by osqp is freed in destructor + */ + ~AccelerationLimitedPlugin() override + { + if (osqp_workspace_ != nullptr) + { + osqp_cleanup(osqp_workspace_); + } + } + +private: + /** \brief Pointer to rclcpp node handle. */ + rclcpp::Node::SharedPtr node_; + /** \brief Parameters for plugin. */ + online_signal_smoothing::Params params_; + /** \brief The number of joints in the robot's planning group. */ + size_t num_joints_; + /** \brief Last velocities and positions received */ + Eigen::VectorXd last_velocities_; + Eigen::VectorXd last_positions_; + /** \brief Intermediate variables used in calculations */ + Eigen::VectorXd cur_acceleration_; + Eigen::VectorXd positions_offset_; + Eigen::VectorXd velocities_offset_; + /** \brief Extracted joint limits from robot model */ + Eigen::VectorXd max_acceleration_limits_; + Eigen::VectorXd min_acceleration_limits_; + /** \brief Pointer to robot model */ + moveit::core::RobotModelConstPtr robot_model_; + /** \brief Constraint matrix for optimization problem */ + Eigen::SparseMatrix constraints_sparse_; + /** \brief osqp types used for optimization problem */ + OSQPDataWrapperPtr osqp_data_; + OSQPWorkspace* osqp_workspace_ = nullptr; + OSQPSettings osqp_settings_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index 8f5e5b362f..05bb859ab8 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,91 +50,5 @@ */ #pragma once - -#include - -#include -#include -#include - -namespace online_signal_smoothing -{ -/** - * Class ButterworthFilter - Implementation of a signal filter to soften jerks. - * This is a first-order Butterworth low-pass filter. First-order was chosen for 2 reasons: - * - It doesn't overshoot - * - Computational efficiency - * This filter has been parameterized so there is only one parameter to tune. - * See "Digital Implementation of Butterworth First–Order Filter Type IIR" by - * Horvath, Cervenanska, and Kotianova, 2019 and - * Mienkina, M., Filter-Based Algorithm for Metering Applications, - * https://www.nxp.com/docs/en/application-note/AN4265.pdf, 2016 - * It comes from finding the bilinear transform equivalent of the analog transfer function and - * further applying the inverse z-transform. - * The parameter "low_pass_filter_coeff" equals (2*pi / tan(omega_d * T)) - * where omega_d is the cutoff frequency and T is the sampling period in sec. - */ -class ButterworthFilter -{ -public: - /** - * Constructor. - * @param low_pass_filter_coeff Larger filter_coeff-> more smoothing of commands, but more lag. - * low_pass_filter_coeff = (2*pi / tan(omega_d * T)) - * where omega_d is the cutoff frequency and T is the sampling period in sec. - */ - ButterworthFilter(double low_pass_filter_coeff); - ButterworthFilter() = delete; - - double filter(double new_measurement); - - void reset(const double data); - -private: - static constexpr std::size_t FILTER_LENGTH = 2; - std::array previous_measurements_; - double previous_filtered_measurement_; - // Scale and feedback term are calculated from supplied filter coefficient - double scale_term_; - double feedback_term_; -}; - -// Plugin -class ButterworthFilterPlugin : public SmoothingBaseClass -{ -public: - /** - * Initialize the smoothing algorithm - * @param node ROS node, used for parameter retrieval - * @param robot_model typically used to retrieve vel/accel/jerk limits - * @param num_joints number of actuated joints in the JointGroup Servo controls - * @return True if initialization was successful - */ - bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, - size_t num_joints) override; - - /** - * Smooth the command signals for all DOF - * @param positions array of joint position commands - * @param velocities array of joint velocity commands - * @param accelerations array of joint acceleration commands - * @return True if initialization was successful - */ - bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; - - /** - * Reset to a given joint state - * @param positions reset the filters to these joint positions - * @param velocities (unused) - * @param accelerations (unused) - * @return True if reset was successful - */ - bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const Eigen::VectorXd& accelerations) override; - -private: - rclcpp::Node::SharedPtr node_; - std::vector position_filters_; - size_t num_joints_; -}; -} // namespace online_signal_smoothing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.hpp b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.hpp new file mode 100644 index 0000000000..aa7d4af124 --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.hpp @@ -0,0 +1,128 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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: Andy Zelenak + Description: A first-order Butterworth low-pass filter. There is only one parameter to tune. + The first-order Butterworth filter has the nice property that it will not overshoot. + */ + +#pragma once + +#include + +#include +#include +#include + +namespace online_signal_smoothing +{ +/** + * Class ButterworthFilter - Implementation of a signal filter to soften jerks. + * This is a first-order Butterworth low-pass filter. First-order was chosen for 2 reasons: + * - It doesn't overshoot + * - Computational efficiency + * This filter has been parameterized so there is only one parameter to tune. + * See "Digital Implementation of Butterworth First–Order Filter Type IIR" by + * Horvath, Cervenanska, and Kotianova, 2019 and + * Mienkina, M., Filter-Based Algorithm for Metering Applications, + * https://www.nxp.com/docs/en/application-note/AN4265.pdf, 2016 + * It comes from finding the bilinear transform equivalent of the analog transfer function and + * further applying the inverse z-transform. + * The parameter "low_pass_filter_coeff" equals (2*pi / tan(omega_d * T)) + * where omega_d is the cutoff frequency and T is the sampling period in sec. + */ +class ButterworthFilter +{ +public: + /** + * Constructor. + * @param low_pass_filter_coeff Larger filter_coeff-> more smoothing of commands, but more lag. + * low_pass_filter_coeff = (2*pi / tan(omega_d * T)) + * where omega_d is the cutoff frequency and T is the sampling period in sec. + */ + ButterworthFilter(double low_pass_filter_coeff); + ButterworthFilter() = delete; + + double filter(double new_measurement); + + void reset(const double data); + +private: + static constexpr std::size_t FILTER_LENGTH = 2; + std::array previous_measurements_; + double previous_filtered_measurement_; + // Scale and feedback term are calculated from supplied filter coefficient + double scale_term_; + double feedback_term_; +}; + +// Plugin +class ButterworthFilterPlugin : public SmoothingBaseClass +{ +public: + /** + * Initialize the smoothing algorithm + * @param node ROS node, used for parameter retrieval + * @param robot_model typically used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup Servo controls + * @return True if initialization was successful + */ + bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) override; + + /** + * Smooth the command signals for all DOF + * @param positions array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands + * @return True if initialization was successful + */ + bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; + + /** + * Reset to a given joint state + * @param positions reset the filters to these joint positions + * @param velocities (unused) + * @param accelerations (unused) + * @return True if reset was successful + */ + bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) override; + +private: + rclcpp::Node::SharedPtr node_; + std::vector position_filters_; + size_t num_joints_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h index 13cf36a742..bd50e91ec6 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,70 +49,5 @@ Description: Applies jerk/acceleration/velocity limits to online motion commands */ #pragma once - -#include - -#include -#include -#include - -#include - -namespace online_signal_smoothing -{ - -class RuckigFilterPlugin : public SmoothingBaseClass -{ -public: - /** - * Initialize the smoothing algorithm - * @param node ROS node, used for parameter retrieval - * @param robot_model used to retrieve vel/accel/jerk limits - * @param num_joints number of actuated joints in the JointGroup - * @return True if initialization was successful - */ - bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, - size_t num_joints) override; - - /** - * Smooth the command signals for all DOF - * @param positions array of joint position commands - * @param velocities array of joint velocity commands - * @param accelerations array of joint acceleration commands - * @return True if initialization was successful - */ - bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; - - /** - * Reset to a given joint state - * @param positions reset the filters to these joint positions - * @param velocities (unused) - * @param accelerations (unused) - * @return True if reset was successful - */ - bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const Eigen::VectorXd& accelerations) override; - -private: - /** - * A utility to print Ruckig's internal state - */ - void printRuckigState(); - - /** - * A utility to get velocity/acceleration/jerk bounds from the robot model - * @return true if all bounds are defined - */ - bool getVelAccelJerkBounds(std::vector& joint_velocity_bounds, std::vector& joint_acceleration_bounds, - std::vector& joint_jerk_bounds); - - /** \brief Parameters loaded from yaml file at runtime */ - online_signal_smoothing::Params params_; - /** \brief The robot model contains the vel/accel/jerk limits that Ruckig requires */ - moveit::core::RobotModelConstPtr robot_model_; - bool have_initial_ruckig_output_ = false; - std::optional> ruckig_; - std::optional> ruckig_input_; - std::optional> ruckig_output_; -}; -} // namespace online_signal_smoothing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.hpp b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.hpp new file mode 100644 index 0000000000..db523cfa2e --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.hpp @@ -0,0 +1,106 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Andrew Zelenak + * 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 PickNik Inc. 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: Andy Zelenak +Description: Applies jerk/acceleration/velocity limits to online motion commands + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace online_signal_smoothing +{ + +class RuckigFilterPlugin : public SmoothingBaseClass +{ +public: + /** + * Initialize the smoothing algorithm + * @param node ROS node, used for parameter retrieval + * @param robot_model used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup + * @return True if initialization was successful + */ + bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) override; + + /** + * Smooth the command signals for all DOF + * @param positions array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands + * @return True if initialization was successful + */ + bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; + + /** + * Reset to a given joint state + * @param positions reset the filters to these joint positions + * @param velocities (unused) + * @param accelerations (unused) + * @return True if reset was successful + */ + bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) override; + +private: + /** + * A utility to print Ruckig's internal state + */ + void printRuckigState(); + + /** + * A utility to get velocity/acceleration/jerk bounds from the robot model + * @return true if all bounds are defined + */ + bool getVelAccelJerkBounds(std::vector& joint_velocity_bounds, std::vector& joint_acceleration_bounds, + std::vector& joint_jerk_bounds); + + /** \brief Parameters loaded from yaml file at runtime */ + online_signal_smoothing::Params params_; + /** \brief The robot model contains the vel/accel/jerk limits that Ruckig requires */ + moveit::core::RobotModelConstPtr robot_model_; + bool have_initial_ruckig_output_ = false; + std::optional> ruckig_; + std::optional> ruckig_input_; + std::optional> ruckig_output_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h index e2dc886aaf..29498eefd8 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,57 +49,5 @@ */ #pragma once - -#include - -#include -#include - -#include - -namespace moveit -{ -namespace core -{ -MOVEIT_CLASS_FORWARD(RobotModel); -} // namespace core -} // namespace moveit - -namespace online_signal_smoothing -{ -class SmoothingBaseClass -{ -public: - SmoothingBaseClass(); - virtual ~SmoothingBaseClass(); - - /** - * Initialize the smoothing algorithm - * @param node ROS node, typically used for parameter retrieval - * @param robot_model typically used to retrieve vel/accel/jerk limits - * @param num_joints number of actuated joints in the JointGroup Servo controls - * @return True if initialization was successful - */ - virtual bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, - size_t num_joints) = 0; - - /** - * Smooth an array of joint position deltas - * @param positiona array of joint position commands - * @param velocities array of joint velocity commands - * @param accelerations array of joint acceleration commands - * @return True if initialization was successful - */ - virtual bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) = 0; - - /** - * Reset to a given joint state - * @param positions reset the filters to these joint positions - * @param velocities reset the filters to these joint velocities (if applicable) - * @param accelerations reset the filters to these joint accelerations (if applicable) - * @return True if reset was successful - */ - virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const Eigen::VectorXd& accelerations) = 0; -}; -} // namespace online_signal_smoothing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.hpp b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.hpp new file mode 100644 index 0000000000..dd35d00519 --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.hpp @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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: Andy Zelenak + Description: Defines a pluginlib interface for smoothing algorithms. + */ + +#pragma once + +#include + +#include +#include + +#include + +namespace moveit +{ +namespace core +{ +MOVEIT_CLASS_FORWARD(RobotModel); +} // namespace core +} // namespace moveit + +namespace online_signal_smoothing +{ +class SmoothingBaseClass +{ +public: + SmoothingBaseClass(); + virtual ~SmoothingBaseClass(); + + /** + * Initialize the smoothing algorithm + * @param node ROS node, typically used for parameter retrieval + * @param robot_model typically used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup Servo controls + * @return True if initialization was successful + */ + virtual bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) = 0; + + /** + * Smooth an array of joint position deltas + * @param positiona array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations array of joint acceleration commands + * @return True if initialization was successful + */ + virtual bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) = 0; + + /** + * Reset to a given joint state + * @param positions reset the filters to these joint positions + * @param velocities reset the filters to these joint velocities (if applicable) + * @param accelerations reset the filters to these joint accelerations (if applicable) + * @return True if reset was successful + */ + virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) = 0; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp index 5e47aaec69..3e6b3de049 100644 --- a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include // Disable -Wold-style-cast because all _THROTTLE macros trigger this diff --git a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp index d0d533799b..4792e93f4e 100644 --- a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp @@ -36,7 +36,7 @@ Description: A first-order Butterworth low-pass filter. There is only one parameter to tune. */ -#include +#include #include #include diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp index b689620761..14845e8d51 100644 --- a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include diff --git a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp index f39984f3d4..f88f3a220d 100644 --- a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp +++ b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp @@ -36,7 +36,7 @@ Description: Base class implementation for online smoothing plugins */ -#include +#include namespace online_signal_smoothing { diff --git a/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp index aeff731f2a..410c3b5c06 100644 --- a/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp +++ b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp @@ -33,8 +33,8 @@ *********************************************************************/ #include -#include -#include +#include +#include constexpr std::string_view PLANNING_GROUP_NAME = "panda_arm"; constexpr size_t PANDA_NUM_JOINTS = 7u; diff --git a/moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp b/moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp index f2f944082a..02b047cf94 100644 --- a/moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp +++ b/moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp @@ -40,7 +40,7 @@ */ #include -#include +#include TEST(SMOOTHING_PLUGINS, FilterConverge) { diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 76bb51bb3e..069b616503 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,179 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -/** \brief This namespace includes the base class for MoveIt planners */ -namespace planning_interface -{ -/** - \brief Specify the settings for a particular planning algorithm, for a particular group. The Planner plugin uses - these settings to configure the algorithm. - \note Settings with unknown keys are ignored. Settings for unknown groups are ignored. -*/ -struct PlannerConfigurationSettings -{ - /** \brief The group (as defined in the SRDF) this configuration is meant for */ - std::string group; - - /* \brief Name of the configuration. - - For a group's default configuration, this should be the same as the group name. - Otherwise, the form "group_name[config_name]" is expected for the name. */ - std::string name; - - /** \brief Key-value pairs of settings that get passed to the planning algorithm */ - std::map config; -}; - -/** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */ -typedef std::map PlannerConfigurationMap; - -MOVEIT_CLASS_FORWARD(PlanningContext); // Defines PlanningContextPtr, ConstPtr, WeakPtr... etc - -/** \brief Representation of a particular planning context -- the planning scene and the request are known, - solution is not yet computed. */ -class PlanningContext -{ -public: - /** \brief Construct a planning context named \e name for the group \e group */ - PlanningContext(const std::string& name, const std::string& group); - - virtual ~PlanningContext(); - - /** \brief Get the name of the group this planning context is for */ - const std::string& getGroupName() const - { - return group_; - } - - /** \brief Get the name of this planning context */ - const std::string& getName() const - { - return name_; - } - - /** \brief Get the planning scene associated to this planning context */ - const planning_scene::PlanningSceneConstPtr& getPlanningScene() const - { - return planning_scene_; - } - - /** \brief Get the motion plan request associated to this planning context */ - const MotionPlanRequest& getMotionPlanRequest() const - { - return request_; - } - - /** \brief Set the planning scene for this context */ - void setPlanningScene(const planning_scene::PlanningSceneConstPtr& planning_scene); - - /** \brief Set the planning request for this context */ - void setMotionPlanRequest(const MotionPlanRequest& request); - - /** \brief Solve the motion planning problem and store the result in \e res. This function should not clear data - * structures before computing. The constructor and clear() do that. */ - virtual void solve(MotionPlanResponse& res) = 0; - - /** \brief Solve the motion planning problem and store the detailed result in \e res. This function should not clear - * data structures before computing. The constructor and clear() do that. */ - virtual void solve(MotionPlanDetailedResponse& res) = 0; - - /** \brief If solve() is running, terminate the computation. Return false if termination not possible. No-op if - * solve() is not running (returns true).*/ - virtual bool terminate() = 0; - - /** \brief Clear the data structures used by the planner */ - virtual void clear() = 0; - -protected: - /// The name of this planning context - std::string name_; - - /// The group (as in the SRDF) this planning context is for - std::string group_; - - /// The planning scene for this context - planning_scene::PlanningSceneConstPtr planning_scene_; - - /// The planning request for this context - MotionPlanRequest request_; -}; - -MOVEIT_CLASS_FORWARD(PlannerManager); // Defines PlannerManagerPtr, ConstPtr, WeakPtr... etc - -/** \brief Base class for a MoveIt planner */ -class PlannerManager -{ -public: - PlannerManager() - { - } - - virtual ~PlannerManager() - { - } - - /// Initialize a planner. This function will be called after the construction of the plugin, before any other call is - /// made. - /// It is assumed that motion plans will be computed for the robot described by \e model and that any exposed ROS - /// functionality - /// or required ROS parameters are namespaced by \e parameter_namespace - virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace); - - /// \brief Get a short string that identifies the planning interface. - virtual std::string getDescription() const = 0; - - /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning - /// request) - virtual void getPlanningAlgorithms(std::vector& algs) const; - - /// \brief Construct a planning context given the current scene and a planning request. If a problem is encountered, - /// error code is set and empty ptr is returned. - /// The returned motion planner context is clean -- the motion planner will start from scratch every time a context is - /// constructed. - /// \param planning_scene A const planning scene to use for planning - /// \param req The representation of the planning request - /// \param error_code This is where the error is set if constructing the planning context fails - virtual PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const MotionPlanRequest& req, - moveit_msgs::msg::MoveItErrorCodes& error_code) const = 0; - - /// \brief Calls the function above but ignores the error_code - PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const MotionPlanRequest& req) const; - - /// \brief Determine whether this plugin instance is able to represent this planning request - virtual bool canServiceRequest(const MotionPlanRequest& req) const = 0; - - /// \brief Specify the settings to be used for specific algorithms - virtual void setPlannerConfigurations(const PlannerConfigurationMap& pcs); - - /// \brief Get the settings for a specific algorithm - const PlannerConfigurationMap& getPlannerConfigurations() const - { - return config_settings_; - } - - /// \brief Request termination, if a solve() function is currently computing plans - void terminate() const; - -protected: - /** \brief All the existing planning configurations. The name - of the configuration is the key of the map. This name can - be of the form "group_name[config_name]" if there are - particular configurations specified for a group, or of the - form "group_name" if default settings are to be used. */ - PlannerConfigurationMap config_settings_; -}; - -} // namespace planning_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.hpp b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.hpp new file mode 100644 index 0000000000..2aaa2e8a05 --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.hpp @@ -0,0 +1,213 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +/** \brief This namespace includes the base class for MoveIt planners */ +namespace planning_interface +{ +/** + \brief Specify the settings for a particular planning algorithm, for a particular group. The Planner plugin uses + these settings to configure the algorithm. + \note Settings with unknown keys are ignored. Settings for unknown groups are ignored. +*/ +struct PlannerConfigurationSettings +{ + /** \brief The group (as defined in the SRDF) this configuration is meant for */ + std::string group; + + /* \brief Name of the configuration. + + For a group's default configuration, this should be the same as the group name. + Otherwise, the form "group_name[config_name]" is expected for the name. */ + std::string name; + + /** \brief Key-value pairs of settings that get passed to the planning algorithm */ + std::map config; +}; + +/** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */ +typedef std::map PlannerConfigurationMap; + +MOVEIT_CLASS_FORWARD(PlanningContext); // Defines PlanningContextPtr, ConstPtr, WeakPtr... etc + +/** \brief Representation of a particular planning context -- the planning scene and the request are known, + solution is not yet computed. */ +class PlanningContext +{ +public: + /** \brief Construct a planning context named \e name for the group \e group */ + PlanningContext(const std::string& name, const std::string& group); + + virtual ~PlanningContext(); + + /** \brief Get the name of the group this planning context is for */ + const std::string& getGroupName() const + { + return group_; + } + + /** \brief Get the name of this planning context */ + const std::string& getName() const + { + return name_; + } + + /** \brief Get the planning scene associated to this planning context */ + const planning_scene::PlanningSceneConstPtr& getPlanningScene() const + { + return planning_scene_; + } + + /** \brief Get the motion plan request associated to this planning context */ + const MotionPlanRequest& getMotionPlanRequest() const + { + return request_; + } + + /** \brief Set the planning scene for this context */ + void setPlanningScene(const planning_scene::PlanningSceneConstPtr& planning_scene); + + /** \brief Set the planning request for this context */ + void setMotionPlanRequest(const MotionPlanRequest& request); + + /** \brief Solve the motion planning problem and store the result in \e res. This function should not clear data + * structures before computing. The constructor and clear() do that. */ + virtual void solve(MotionPlanResponse& res) = 0; + + /** \brief Solve the motion planning problem and store the detailed result in \e res. This function should not clear + * data structures before computing. The constructor and clear() do that. */ + virtual void solve(MotionPlanDetailedResponse& res) = 0; + + /** \brief If solve() is running, terminate the computation. Return false if termination not possible. No-op if + * solve() is not running (returns true).*/ + virtual bool terminate() = 0; + + /** \brief Clear the data structures used by the planner */ + virtual void clear() = 0; + +protected: + /// The name of this planning context + std::string name_; + + /// The group (as in the SRDF) this planning context is for + std::string group_; + + /// The planning scene for this context + planning_scene::PlanningSceneConstPtr planning_scene_; + + /// The planning request for this context + MotionPlanRequest request_; +}; + +MOVEIT_CLASS_FORWARD(PlannerManager); // Defines PlannerManagerPtr, ConstPtr, WeakPtr... etc + +/** \brief Base class for a MoveIt planner */ +class PlannerManager +{ +public: + PlannerManager() + { + } + + virtual ~PlannerManager() + { + } + + /// Initialize a planner. This function will be called after the construction of the plugin, before any other call is + /// made. + /// It is assumed that motion plans will be computed for the robot described by \e model and that any exposed ROS + /// functionality + /// or required ROS parameters are namespaced by \e parameter_namespace + virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace); + + /// \brief Get a short string that identifies the planning interface. + virtual std::string getDescription() const = 0; + + /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning + /// request) + virtual void getPlanningAlgorithms(std::vector& algs) const; + + /// \brief Construct a planning context given the current scene and a planning request. If a problem is encountered, + /// error code is set and empty ptr is returned. + /// The returned motion planner context is clean -- the motion planner will start from scratch every time a context is + /// constructed. + /// \param planning_scene A const planning scene to use for planning + /// \param req The representation of the planning request + /// \param error_code This is where the error is set if constructing the planning context fails + virtual PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code) const = 0; + + /// \brief Calls the function above but ignores the error_code + PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const MotionPlanRequest& req) const; + + /// \brief Determine whether this plugin instance is able to represent this planning request + virtual bool canServiceRequest(const MotionPlanRequest& req) const = 0; + + /// \brief Specify the settings to be used for specific algorithms + virtual void setPlannerConfigurations(const PlannerConfigurationMap& pcs); + + /// \brief Get the settings for a specific algorithm + const PlannerConfigurationMap& getPlannerConfigurations() const + { + return config_settings_; + } + + /// \brief Request termination, if a solve() function is currently computing plans + void terminate() const; + +protected: + /** \brief All the existing planning configurations. The name + of the configuration is the key of the map. This name can + be of the form "group_name[config_name]" if there are + particular configurations specified for a group, or of the + form "group_name" if default settings are to be used. */ + PlannerConfigurationMap config_settings_; +}; + +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index 39200155ad..9664659c38 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,13 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace planning_interface -{ - -// for now this is just a typedef -typedef moveit_msgs::msg::MotionPlanRequest MotionPlanRequest; - -} // namespace planning_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.hpp b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.hpp new file mode 100644 index 0000000000..39200155ad --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.hpp @@ -0,0 +1,47 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +namespace planning_interface +{ + +// for now this is just a typedef +typedef moveit_msgs::msg::MotionPlanRequest MotionPlanRequest; + +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h index f001f6792d..ccab57105d 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,39 +49,5 @@ */ #pragma once - -#include -#include -#include -#include -#include - -namespace planning_interface -{ -MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc - -/** @brief Concept in MoveIt which can be used to modify the planning problem(pre-processing) in a planning pipeline. - * PlanningRequestAdapters enable adjusting to or validating a planning problem for a subsequent planning algorithm. - */ -class PlanningRequestAdapter -{ -public: - /** @brief Initialize parameters using the passed Node and parameter namespace. - * @param node Node instance used by the adapter - * @param parameter_namespace Parameter namespace for adapter - * @details The default implementation is empty */ - virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; - - /** @brief Get a description of this adapter - * @return Returns a short string that identifies the planning request adapter - */ - [[nodiscard]] virtual std::string getDescription() const = 0; - - /** @brief Adapt the planning request - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @return True if response was generated correctly */ - [[nodiscard]] virtual moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - planning_interface::MotionPlanRequest& req) const = 0; -}; -} // namespace planning_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.hpp b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.hpp new file mode 100644 index 0000000000..981c24a273 --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.hpp @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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, Sebastian Jahr + Description: Generic interface to adapting motion planning requests +*/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc + +/** @brief Concept in MoveIt which can be used to modify the planning problem(pre-processing) in a planning pipeline. + * PlanningRequestAdapters enable adjusting to or validating a planning problem for a subsequent planning algorithm. + */ +class PlanningRequestAdapter +{ +public: + /** @brief Initialize parameters using the passed Node and parameter namespace. + * @param node Node instance used by the adapter + * @param parameter_namespace Parameter namespace for adapter + * @details The default implementation is empty */ + virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; + + /** @brief Get a description of this adapter + * @return Returns a short string that identifies the planning request adapter + */ + [[nodiscard]] virtual std::string getDescription() const = 0; + + /** @brief Adapt the planning request + * @param planning_scene Representation of the environment for the planning + * @param req Motion planning request with a set of constraints + * @return True if response was generated correctly */ + [[nodiscard]] virtual moveit::core::MoveItErrorCode adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const = 0; +}; +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 7f8bbadcb6..1b5e6bae2a 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,52 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -namespace planning_interface -{ -/// \brief Response to a planning query -struct MotionPlanResponse -{ - /** \brief Constructor */ - MotionPlanResponse() : trajectory(nullptr), planning_time(0.0), error_code(moveit::core::MoveItErrorCode::FAILURE) - { - } - - /** \brief Construct a ROS message from struct data */ - void getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const; - - // Trajectory generated by the queried planner - robot_trajectory::RobotTrajectoryPtr trajectory; - // Time to plan the response to the planning query - double planning_time; - // Result status of the query - moveit::core::MoveItErrorCode error_code; - /// The full starting state used for planning - moveit_msgs::msg::RobotState start_state; - std::string planner_id; - - // \brief Enable checking of query success or failure, for example if(response) ... - explicit operator bool() const - { - return bool(error_code); - } -}; - -struct MotionPlanDetailedResponse -{ - void getMessage(moveit_msgs::msg::MotionPlanDetailedResponse& msg) const; - - std::vector trajectory; - std::vector description; - std::vector processing_time; - moveit_msgs::msg::MoveItErrorCodes error_code; - std::string planner_id; -}; - -} // namespace planning_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.hpp b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.hpp new file mode 100644 index 0000000000..1fdf7c7c20 --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.hpp @@ -0,0 +1,86 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace planning_interface +{ +/// \brief Response to a planning query +struct MotionPlanResponse +{ + /** \brief Constructor */ + MotionPlanResponse() : trajectory(nullptr), planning_time(0.0), error_code(moveit::core::MoveItErrorCode::FAILURE) + { + } + + /** \brief Construct a ROS message from struct data */ + void getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const; + + // Trajectory generated by the queried planner + robot_trajectory::RobotTrajectoryPtr trajectory; + // Time to plan the response to the planning query + double planning_time; + // Result status of the query + moveit::core::MoveItErrorCode error_code; + /// The full starting state used for planning + moveit_msgs::msg::RobotState start_state; + std::string planner_id; + + // \brief Enable checking of query success or failure, for example if(response) ... + explicit operator bool() const + { + return bool(error_code); + } +}; + +struct MotionPlanDetailedResponse +{ + void getMessage(moveit_msgs::msg::MotionPlanDetailedResponse& msg) const; + + std::vector trajectory; + std::vector description; + std::vector processing_time; + moveit_msgs::msg::MoveItErrorCodes error_code; + std::string planner_id; +}; + +} // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h index ddade4c5d4..50a786770d 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,40 +47,5 @@ /* Author: Sebastian Jahr */ #pragma once - -#include -#include -#include -#include - -namespace planning_interface -{ -MOVEIT_CLASS_FORWARD(PlanningResponseAdapter); // Defines PlanningResponseAdapterPtr, ConstPtr, WeakPtr... etc - -/** @brief Concept in MoveIt which can be used to modify the planning solution (post-processing) in a planning pipeline. - * PlanningResponseAdapters enable using for example smoothing and trajectory generation algorithms in sequence to - * produce a trajectory. - */ -class PlanningResponseAdapter -{ -public: - /** @brief Initialize parameters using the passed Node and parameter namespace. - * @param node Node instance used by the adapter - * @param parameter_namespace Parameter namespace for adapter - * @details The default implementation is empty */ - virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; - - /** \brief Get a description of this adapter - * @return Returns a short string that identifies the planning response adapter - */ - [[nodiscard]] virtual std::string getDescription() const = 0; - - /** @brief Adapt the planning response - * @param planning_scene Representation of the environment for the planning - * @param req Motion planning request with a set of constraints - * @param res Motion planning response containing the solution that is adapted. */ - virtual void adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const = 0; -}; -} // namespace planning_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.hpp b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.hpp new file mode 100644 index 0000000000..0e376d5699 --- /dev/null +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response_adapter.hpp @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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: Sebastian Jahr */ + +#pragma once + +#include +#include +#include +#include + +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningResponseAdapter); // Defines PlanningResponseAdapterPtr, ConstPtr, WeakPtr... etc + +/** @brief Concept in MoveIt which can be used to modify the planning solution (post-processing) in a planning pipeline. + * PlanningResponseAdapters enable using for example smoothing and trajectory generation algorithms in sequence to + * produce a trajectory. + */ +class PlanningResponseAdapter +{ +public: + /** @brief Initialize parameters using the passed Node and parameter namespace. + * @param node Node instance used by the adapter + * @param parameter_namespace Parameter namespace for adapter + * @details The default implementation is empty */ + virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace){}; + + /** \brief Get a description of this adapter + * @return Returns a short string that identifies the planning response adapter + */ + [[nodiscard]] virtual std::string getDescription() const = 0; + + /** @brief Adapt the planning response + * @param planning_scene Representation of the environment for the planning + * @param req Motion planning request with a set of constraints + * @param res Motion planning response containing the solution that is adapted. */ + virtual void adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const = 0; +}; +} // namespace planning_interface diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 67ec7fe7d5..5aa72cbe25 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/planning_interface/src/planning_response.cpp b/moveit_core/planning_interface/src/planning_response.cpp index 8b93391e80..6b114e2ae2 100644 --- a/moveit_core/planning_interface/src/planning_response.cpp +++ b/moveit_core/planning_interface/src/planning_response.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include void planning_interface::MotionPlanResponse::getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const { diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 37d7c9787c..682da93b7f 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,969 +47,5 @@ /* Author: Ioan Sucan, Acorn Pooley */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/** \brief This namespace includes the central class for representing planning contexts */ -namespace planning_scene -{ -MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc - -/** \brief This is the function signature for additional feasibility checks to be imposed on states (in addition to - respecting constraints and collision avoidance). - The first argument is the state to check the feasibility for, the second one is whether the check should be verbose - or not. */ -typedef std::function StateFeasibilityFn; - -/** \brief This is the function signature for additional feasibility checks to be imposed on motions segments between - states (in addition to respecting constraints and collision avoidance). - The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the - first state and end at the second state. The third argument indicates - whether the check should be verbose or not. */ -using MotionFeasibilityFn = std::function; - -/** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ -using ObjectColorMap = std::map; - -/** \brief A map from object names (e.g., attached bodies, collision objects) to their types */ -using ObjectTypeMap = std::map; - -/** \brief This class maintains the representation of the - environment as seen by a planning instance. The environment - geometry, the robot geometry and state are maintained. */ -class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this -{ -public: - /** - * @brief PlanningScene cannot be copy-constructed - */ - PlanningScene(const PlanningScene&) = delete; - - /** - * @brief PlanningScene cannot be copy-assigned - */ - PlanningScene& operator=(const PlanningScene&) = delete; - - /** \brief construct using an existing RobotModel */ - PlanningScene(const moveit::core::RobotModelConstPtr& robot_model, - const collision_detection::WorldPtr& world = std::make_shared()); - - /** \brief construct using a urdf and srdf. - * A RobotModel for the PlanningScene will be created using the urdf and srdf. */ - PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model, - const collision_detection::WorldPtr& world = std::make_shared()); - - static const std::string OCTOMAP_NS; - static const std::string DEFAULT_SCENE_NAME; - - ~PlanningScene(); - - /** \brief Get the name of the planning scene. This is empty by default */ - const std::string& getName() const - { - return name_; - } - - /** \brief Set the name of the planning scene */ - void setName(const std::string& name) - { - name_ = name; - } - - /** \brief Return a new child PlanningScene that uses this one as parent. - * - * The child scene has its own copy of the world. It maintains a list (in - * world_diff_) of changes made to the child world. - * - * The robot_model_, robot_state_, scene_transforms_, and acm_ are not copied. - * They are shared with the parent. So if changes to these are made in the parent they will be visible in the child. - * But if any of these is modified (i.e. if the get*NonConst functions are called) in the child then a copy is made - * and subsequent changes to the corresponding member of the parent will no longer be visible in the child. - */ - PlanningScenePtr diff() const; - - /** \brief Return a new child PlanningScene that uses this one as parent and - * has the diffs specified by \e msg applied. */ - PlanningScenePtr diff(const moveit_msgs::msg::PlanningScene& msg) const; - - /** \brief Get the parent scene (with respect to which the diffs are maintained). This may be empty */ - const PlanningSceneConstPtr& getParent() const - { - return parent_; - } - - /** \brief Get the kinematic model for which the planning scene is maintained */ - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - // the kinematic model does not change - return robot_model_; - } - - /** \brief Get the state at which the robot is assumed to be. */ - const moveit::core::RobotState& getCurrentState() const - { - // if we have an updated state, return it; otherwise, return the parent one - return robot_state_.has_value() ? robot_state_.value() : parent_->getCurrentState(); - } - /** \brief Get the state at which the robot is assumed to be. */ - moveit::core::RobotState& getCurrentStateNonConst(); - - /** \brief Get a copy of the current state with components overwritten by the state message \e update */ - moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const; - - /** - * \name Reasoning about frames - */ - /**@{*/ - - /** \brief Get the frame in which planning is performed */ - const std::string& getPlanningFrame() const - { - // if we have an updated set of transforms, return it; otherwise, return the parent one - return scene_transforms_.has_value() ? scene_transforms_.value()->getTargetFrame() : parent_->getPlanningFrame(); - } - - /** \brief Get the set of fixed transforms from known frames to the planning frame */ - const moveit::core::Transforms& getTransforms() const - { - if (scene_transforms_.has_value() || !parent_) - { - return *scene_transforms_.value(); - } - - // if this planning scene is a child of another, and doesn't have its own custom transforms - return parent_->getTransforms(); - } - - /** \brief Get the set of fixed transforms from known frames to the planning frame. This variant is non-const and also - * updates the current state */ - const moveit::core::Transforms& getTransforms(); - - /** \brief Get the set of fixed transforms from known frames to the planning frame */ - moveit::core::Transforms& getTransformsNonConst(); - - /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached - body id or a collision object. - Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be - successful or not. */ - const Eigen::Isometry3d& getFrameTransform(const std::string& id) const; - - /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached - body id or a collision object. - Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be - successful or not. - Because this function is non-const, the current state transforms are also updated, if needed. */ - const Eigen::Isometry3d& getFrameTransform(const std::string& id); - - /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached - body id or a collision object. - Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be - successful or not. This function also - updates the link transforms of \e state. */ - const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const - { - state.updateLinkTransforms(); - return getFrameTransform(static_cast(state), id); - } - - /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached - body id or a collision object. - Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be - successful or not. */ - const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; - - /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached - * body id or a collision object */ - bool knowsFrameTransform(const std::string& id) const; - - /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached - * body id or a collision object */ - bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; - - /**@}*/ - - /** - * \name Reasoning about the geometry of the planning scene - */ - /**@{*/ - - const std::string getCollisionDetectorName() const - { - // If no collision detector is allocated, return an empty string - return collision_detector_ ? (collision_detector_->alloc_->getName()) : ""; - } - - /** \brief Get the representation of the world */ - const collision_detection::WorldConstPtr& getWorld() const - { - // we always have a world representation - return world_const_; - } - - /** \brief Get the representation of the world */ - const collision_detection::WorldPtr& getWorldNonConst() - { - // we always have a world representation - return world_; - } - - /** \brief Get the active collision environment */ - const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const - { - return collision_detector_->getCollisionEnv(); - } - - /** \brief Get the active collision detector for the robot */ - const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const - { - return collision_detector_->getCollisionEnvUnpadded(); - } - - /** \brief Get a specific collision detector for the world. If not found return active CollisionWorld. */ - const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const; - - /** \brief Get a specific collision detector for the unpadded robot. If no found return active unpadded - * CollisionRobot. */ - const collision_detection::CollisionEnvConstPtr& - getCollisionEnvUnpadded(const std::string& collision_detector_name) const; - - /** \brief Get the representation of the collision robot - * This can be used to set padding and link scale on the active collision_robot. */ - const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst(); - - /** \brief Get the allowed collision matrix */ - const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const - { - return acm_.has_value() ? acm_.value() : parent_->getAllowedCollisionMatrix(); - } - - /** \brief Get the allowed collision matrix */ - collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst(); - - /** \brief Set the allowed collision matrix */ - void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm); - - /**@}*/ - - /** - * \name Collision checking with respect to this planning scene - */ - /**@{*/ - - /** \brief Check if the current state is in collision (with the environment or self collision). - If a group name is specified, collision checking is done for that group only (plus descendent links). - Since the function is non-const, the current state transforms are updated before the collision check. */ - bool isStateColliding(const std::string& group = "", bool verbose = false); - - /** \brief Check if the current state is in collision (with the environment or self collision). If a group name is - specified, - collision checking is done for that group only (plus descendent links). It is expected the current state - transforms are up to date. */ - bool isStateColliding(const std::string& group = "", bool verbose = false) const - { - return isStateColliding(getCurrentState(), group, verbose); - } - - /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is - specified, - collision checking is done for that group only (plus descendent links). The link transforms for \e state are - updated before the collision check. */ - bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const - { - state.updateCollisionBodyTransforms(); - return isStateColliding(static_cast(state), group, verbose); - } - - /** \brief Check if a given state is in collision (with the environment or self collision) - If a group name is specified, collision checking is done for that group only (plus descendent links). It is - expected that the link transforms of \e state are up to date. */ - bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "", - bool verbose = false) const; - - /** \brief Check if a given state is in collision (with the environment or self collision) - If a group name is specified, collision checking is done for that group only (plus descendent links). */ - bool isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group = "", - bool verbose = false) const; - - /** \brief Check whether the current state is in collision, and if needed, updates the collision transforms of the - * current state before the computation. */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); - - /** \brief Check whether the current state is in collision. The current state is expected to be updated. */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes - a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e - * robot_state are - * expected to be up to date. */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given - allowed collision matrix (\e acm). This variant of the function takes - a non-const \e robot_state and updates its link transforms if needed. */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given - allowed collision matrix (\e acm). */ - void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Check whether the current state is in collision, - but use a collision_detection::CollisionRobot instance that has no padding. - Since the function is non-const, the current state transforms are also updated if needed. */ - [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void - checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); - - /** \brief Check whether the current state is in collision, - but use a collision_detection::CollisionRobot instance that has no padding. */ - [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void - checkCollisionUnpadded(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision, - but use a collision_detection::CollisionRobot instance that has no padding. */ - [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void - checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision, - but use a collision_detection::CollisionRobot instance that has no padding. - Update the link transforms of \e robot_state if needed. */ - [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void - checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given - allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. - This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */ - [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void - checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given - allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. */ - [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void - checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Check whether the current state is in self collision */ - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); - - /** \brief Check whether the current state is in self collision */ - void checkSelfCollision(const collision_detection::CollisionRequest& req, - collision_detection::CollisionResult& res) const; - - /** \brief Check whether a specified state (\e robot_state) is in self collision */ - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state) const; - - /** \brief Check whether a specified state (\e robot_state) is in self collision */ - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state) const; - - /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given - allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */ - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given - allowed collision matrix (\e acm) */ - void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, - const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Get the names of the links that are involved in collisions for the current state */ - void getCollidingLinks(std::vector& links); - - /** \brief Get the names of the links that are involved in collisions for the current state */ - void getCollidingLinks(std::vector& links) const - { - getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix()); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - Update the link transforms for \e robot_state if needed. */ - void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state) const - { - robot_state.updateCollisionBodyTransforms(); - getCollidingLinks(links, static_cast(robot_state), getAllowedCollisionMatrix()); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ - void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state) const - { - getCollidingLinks(links, robot_state, getAllowedCollisionMatrix()); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm) */ - void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - robot_state.updateCollisionBodyTransforms(); - getCollidingLinks(links, static_cast(robot_state), acm); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm) */ - void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const; - - /** \brief Get the names of the links that are involved in collisions for the current state. - Update the link transforms for the current state if needed. */ - void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts); - - /** \brief Get the names of the links that are involved in collisions for the current state */ - void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts) const - { - getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix()); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - * Can be restricted to links part of or updated by \e group_name (plus descendent links) */ - void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const moveit::core::RobotState& robot_state, const std::string& group_name = "") const - { - getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - Update the link transforms for \e robot_state if needed. - Can be restricted to links part of or updated by \e group_name (plus descendent links) */ - void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - moveit::core::RobotState& robot_state, const std::string& group_name = "") const - { - robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix(), - group_name); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. - Can be restricted to links part of or updated by \e group_name (plus descendent links) */ - void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, - const std::string& group_name = "") const - { - robot_state.updateCollisionBodyTransforms(); - getCollidingPairs(contacts, static_cast(robot_state), acm, group_name); - } - - /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name (plus descendent links) */ - void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, - const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm, - const std::string& group_name = "") const; - - /**@}*/ - - /** - * \name Distance computation - */ - /**@{*/ - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring - * self-collisions) - */ - double distanceToCollision(moveit::core::RobotState& robot_state) const - { - robot_state.updateCollisionBodyTransforms(); - return distanceToCollision(static_cast(robot_state)); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring - * self-collisions) - */ - double distanceToCollision(const moveit::core::RobotState& robot_state) const - { - return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix()); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring - * self-collisions), if the robot has no padding */ - double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state) const - { - robot_state.updateCollisionBodyTransforms(); - return distanceToCollisionUnpadded(static_cast(robot_state)); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring - * self-collisions), if the robot has no padding */ - double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state) const - { - return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix()); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring - * self-collisions - * and elements that are allowed to collide. */ - double distanceToCollision(moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - robot_state.updateCollisionBodyTransforms(); - return distanceToCollision(static_cast(robot_state), acm); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring - * self-collisions - * and elements that are allowed to collide. */ - double distanceToCollision(const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - return getCollisionEnv()->distanceRobot(robot_state, acm); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring - * self-collisions - * and elements that are allowed to collide, if the robot has no padding. */ - double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - robot_state.updateCollisionBodyTransforms(); - return distanceToCollisionUnpadded(static_cast(robot_state), acm); - } - - /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring - * self-collisions - * and elements that always allowed to collide, if the robot has no padding. */ - double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state, - const collision_detection::AllowedCollisionMatrix& acm) const - { - return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm); - } - - /**@}*/ - - /** \brief Save the geometry of the planning scene to a stream, as plain text */ - void saveGeometryToStream(std::ostream& out) const; - - /** \brief Load the geometry of the planning scene from a stream */ - bool loadGeometryFromStream(std::istream& in); - - /** \brief Load the geometry of the planning scene from a stream at a certain location using offset*/ - bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset); - - /** \brief Fill the message \e scene with the differences between this instance of PlanningScene with respect to the - parent. - If there is no parent, everything is considered to be a diff and the function behaves like getPlanningSceneMsg() - */ - void getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& scene) const; - - /** \brief Construct a message (\e scene) with all the necessary data so that the scene can be later reconstructed to - be - exactly the same using setPlanningSceneMsg() */ - void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene) const; - - /** \brief Construct a message (\e scene) with the data requested in \e comp. If all options in \e comp are filled, - this will be a complete planning scene message */ - void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene, - const moveit_msgs::msg::PlanningSceneComponents& comp) const; - - /** \brief Construct a message (\e collision_object) with the collision object data from the planning_scene for the - * requested object*/ - bool getCollisionObjectMsg(moveit_msgs::msg::CollisionObject& collision_obj, const std::string& ns) const; - - /** \brief Construct a vector of messages (\e collision_objects) with the collision object data for all objects in - * planning_scene */ - void getCollisionObjectMsgs(std::vector& collision_objs) const; - - /** \brief Construct a message (\e attached_collision_object) with the attached collision object data from the - * planning_scene for the requested object*/ - bool getAttachedCollisionObjectMsg(moveit_msgs::msg::AttachedCollisionObject& attached_collision_obj, - const std::string& ns) const; - - /** \brief Construct a vector of messages (\e attached_collision_objects) with the attached collision object data for - * all objects in planning_scene */ - void - getAttachedCollisionObjectMsgs(std::vector& attached_collision_objs) const; - - /** \brief Construct a message (\e octomap) with the octomap data from the planning_scene */ - bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const; - - /** \brief Construct a vector of messages (\e object_colors) with the colors of the objects from the planning_scene */ - void getObjectColorMsgs(std::vector& object_colors) const; - - /** \brief Apply changes to this planning scene as diffs, even if the message itself is not marked as being a diff - (is_diff - member). A parent is not required to exist. However, the existing data in the planning instance is not cleared. - Data from - the message is only appended (and in cases such as e.g., the robot state, is overwritten). */ - bool setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScene& scene); - - /** \brief Set this instance of a planning scene to be the same as the one serialized in the \e scene message, even if - * the message itself is marked as being a diff (is_diff member) */ - bool setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene); - - /** \brief Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the message - * is set */ - bool usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene); - - /** \brief Takes the object message and returns the object pose, shapes and shape poses. - * If the object pose is empty (identity) but the shape pose is set, this uses the shape - * pose as the object pose. The shape pose becomes the identity instead. - */ - bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::msg::CollisionObject& object, - Eigen::Isometry3d& object_pose_in_header_frame, - std::vector& shapes, - EigenSTL::vector_Isometry3d& shape_poses); - - bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& object); - bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& object); - - bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld& world); - - void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& map); - void processOctomapMsg(const octomap_msgs::msg::Octomap& map); - void processOctomapPtr(const std::shared_ptr& octree, const Eigen::Isometry3d& t); - - /** - * \brief Clear all collision objects in planning scene - */ - void removeAllCollisionObjects(); - - /** \brief Set the current robot state to be \e state. If not - all joint values are specified, the previously maintained - joint values are kept. */ - void setCurrentState(const moveit_msgs::msg::RobotState& state); - - /** \brief Set the current robot state */ - void setCurrentState(const moveit::core::RobotState& state); - - /** \brief Set the callback to be triggered when changes are made to the current scene state */ - void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback); - - /** \brief Set the callback to be triggered when changes are made to the current scene world */ - void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback); - - bool hasObjectColor(const std::string& id) const; - - /** - * \brief Gets the current color of an object. - * \param id The string id of the target object. - * \return The current object color. - */ - const std_msgs::msg::ColorRGBA& getObjectColor(const std::string& id) const; - - /** - * \brief Tries to get the original color of an object, if one has been set before. - * \param id The string id of the target object. - * \return The original object color, if available, otherwise std::nullopt. - */ - std::optional getOriginalObjectColor(const std::string& id) const; - - void setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color); - void removeObjectColor(const std::string& id); - void getKnownObjectColors(ObjectColorMap& kc) const; - - bool hasObjectType(const std::string& id) const; - - const object_recognition_msgs::msg::ObjectType& getObjectType(const std::string& id) const; - void setObjectType(const std::string& id, const object_recognition_msgs::msg::ObjectType& type); - void removeObjectType(const std::string& id); - void getKnownObjectTypes(ObjectTypeMap& kc) const; - - /** \brief Clear the diffs accumulated for this planning scene, with respect to: - * the parent PlanningScene (if it exists) - * the parent CollisionDetector (if it exists) - * This function is a no-op if there is no parent planning scene. */ - void clearDiffs(); - - /** \brief If there is a parent specified for this scene, then the diffs with respect to that parent are applied to a - specified planning scene, whatever - that scene may be. If there is no parent specified, this function is a no-op. */ - void pushDiffs(const PlanningScenePtr& scene); - - /** \brief Make sure that all the data maintained in this - scene is local. All unmodified data is copied from the - parent and the pointer to the parent is discarded. */ - void decoupleParent(); - - /** \brief Specify a predicate that decides whether states are considered valid or invalid for reasons beyond ones - covered by collision checking and constraint evaluation. - This is useful for setting up problem specific constraints (e.g., stability) */ - void setStateFeasibilityPredicate(const StateFeasibilityFn& fn) - { - state_feasibility_ = fn; - } - - /** \brief Get the predicate that decides whether states are considered valid or invalid for reasons beyond ones - * covered by collision checking and constraint evaluation. */ - const StateFeasibilityFn& getStateFeasibilityPredicate() const - { - return state_feasibility_; - } - - /** \brief Specify a predicate that decides whether motion segments are considered valid or invalid for reasons beyond - * ones covered by collision checking and constraint evaluation. */ - void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn) - { - motion_feasibility_ = fn; - } - - /** \brief Get the predicate that decides whether motion segments are considered valid or invalid for reasons beyond - * ones covered by collision checking and constraint evaluation. */ - const MotionFeasibilityFn& getMotionFeasibilityPredicate() const - { - return motion_feasibility_; - } - - /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by - * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */ - bool isStateFeasible(const moveit_msgs::msg::RobotState& state, bool verbose = false) const; - - /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by - * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */ - bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const; - - /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, - bool verbose = false) const; - - /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr, - bool verbose = false) const; - - /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const moveit_msgs::msg::RobotState& state, - const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; - - /** \brief Check if a given state satisfies a set of constraints */ - bool isStateConstrained(const moveit::core::RobotState& state, - const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; - - /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent - * links of \e group. */ - bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", - bool verbose = false) const; - - /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent - * links of \e group. */ - bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; - - /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well. Includes descendent links of \e group. */ - bool isStateValid(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, - const std::string& group = "", bool verbose = false) const; - - /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well. Includes descendent links of \e group. */ - bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr, - const std::string& group = "", bool verbose = false) const; - - /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well. Includes descendent links of \e group. */ - bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, - const std::string& group = "", bool verbose = false) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). - * Includes descendent links of \e group. */ - bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, - const std::string& group = "", bool verbose = false, - std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. Includes descendent links of \e group. */ - bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. Includes descendent links of \e group. */ - bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, - const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. Includes descendent links of \e group. */ - bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, - const std::vector& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. Includes descendent links of \e group. */ - bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, - const std::vector& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. Includes descendent links of \e group. */ - bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, - const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). Includes descendent links of \e group. */ - bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). - * Includes descendent links of \e group. */ - bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "", - bool verbose = false, std::vector* invalid_index = nullptr) const; - - /** \brief Get the top \e max_costs cost sources for a specified trajectory. The resulting costs are stored in \e - * costs */ - void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, - std::set& costs, double overlap_fraction = 0.9) const; - - /** \brief Get the top \e max_costs cost sources for a specified trajectory, but only for group \e group_name (plus - * descendent links). The resulting costs are stored in \e costs */ - void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, - const std::string& group_name, std::set& costs, - double overlap_fraction = 0.9) const; - - /** \brief Get the top \e max_costs cost sources for a specified state. The resulting costs are stored in \e costs */ - void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, - std::set& costs) const; - - /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name (plus - * descendent links). The resulting costs are stored in \e costs */ - void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, - std::set& costs) const; - - /** \brief Outputs debug information about the planning scene contents */ - void printKnownObjects(std::ostream& out = std::cout) const; - - /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */ - static PlanningScenePtr clone(const PlanningSceneConstPtr& scene); - - /** \brief Allocate a new collision detector and replace the previous one if there was any. - * - * The collision detector type is specified with (a shared pointer to) an - * allocator which is a subclass of CollisionDetectorAllocator. This - * identifies a combination of CollisionWorld/CollisionRobot which can be - * used together. - * - * A new PlanningScene uses an FCL collision detector by default. - * - * example: to add FCL collision detection (normally not necessary) call - * planning_scene->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); - * - * */ - void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator) - { - allocateCollisionDetector(allocator, nullptr /* no parent_detector */); - } - -private: - /* Private constructor used by the diff() methods. */ - PlanningScene(const PlanningSceneConstPtr& parent); - - /* Initialize the scene. This should only be called by the constructors. - * Requires a valid robot_model_ */ - void initialize(); - - /* Helper functions for processing collision objects */ - bool processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object); - bool processCollisionObjectRemove(const moveit_msgs::msg::CollisionObject& object); - bool processCollisionObjectMove(const moveit_msgs::msg::CollisionObject& object); - - MOVEIT_STRUCT_FORWARD(CollisionDetector); - - /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */ - void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, - const CollisionDetectorPtr& parent_detector); - - /* \brief A set of compatible collision detectors */ - struct CollisionDetector - { - collision_detection::CollisionDetectorAllocatorPtr alloc_; - collision_detection::CollisionEnvPtr cenv_; // never nullptr - collision_detection::CollisionEnvConstPtr cenv_const_; - - collision_detection::CollisionEnvPtr cenv_unpadded_; - collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; - - const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const - { - return cenv_const_; - } - const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const - { - return cenv_unpadded_const_; - } - void copyPadding(const CollisionDetector& src); - }; - friend struct CollisionDetector; - - std::string name_; // may be empty - - PlanningSceneConstPtr parent_; // Null unless this is a diff scene - - moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) - - std::optional robot_state_; // if there is no value use parent's - - // Called when changes are made to attached bodies - moveit::core::AttachedBodyCallback current_state_attached_body_callback_; - - // This variable is not necessarily used by child planning scenes - // This Transforms class is actually a SceneTransforms class - std::optional scene_transforms_; // if there is no value use parent's - - collision_detection::WorldPtr world_; // never nullptr, never shared with parent/child - collision_detection::WorldConstPtr world_const_; // copy of world_ - collision_detection::WorldDiffPtr world_diff_; // nullptr unless this is a diff scene - collision_detection::World::ObserverCallbackFn current_world_object_update_callback_; - collision_detection::World::ObserverHandle current_world_object_update_observer_handle_; - - CollisionDetectorPtr collision_detector_; // Never nullptr. - - std::optional acm_; // if there is no value use parent's - - StateFeasibilityFn state_feasibility_; - MotionFeasibilityFn motion_feasibility_; - - // Maps of current and original object colors (to manage attaching/detaching objects) - std::unique_ptr object_colors_; - std::unique_ptr original_object_colors_; - - // a map of object types - std::optional object_types_; -}; -} // namespace planning_scene +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp new file mode 100644 index 0000000000..a141b9f7e5 --- /dev/null +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.hpp @@ -0,0 +1,1003 @@ +/********************************************************************* + * 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, Acorn Pooley */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/** \brief This namespace includes the central class for representing planning contexts */ +namespace planning_scene +{ +MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc + +/** \brief This is the function signature for additional feasibility checks to be imposed on states (in addition to + respecting constraints and collision avoidance). + The first argument is the state to check the feasibility for, the second one is whether the check should be verbose + or not. */ +typedef std::function StateFeasibilityFn; + +/** \brief This is the function signature for additional feasibility checks to be imposed on motions segments between + states (in addition to respecting constraints and collision avoidance). + The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the + first state and end at the second state. The third argument indicates + whether the check should be verbose or not. */ +using MotionFeasibilityFn = std::function; + +/** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ +using ObjectColorMap = std::map; + +/** \brief A map from object names (e.g., attached bodies, collision objects) to their types */ +using ObjectTypeMap = std::map; + +/** \brief This class maintains the representation of the + environment as seen by a planning instance. The environment + geometry, the robot geometry and state are maintained. */ +class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this +{ +public: + /** + * @brief PlanningScene cannot be copy-constructed + */ + PlanningScene(const PlanningScene&) = delete; + + /** + * @brief PlanningScene cannot be copy-assigned + */ + PlanningScene& operator=(const PlanningScene&) = delete; + + /** \brief construct using an existing RobotModel */ + PlanningScene(const moveit::core::RobotModelConstPtr& robot_model, + const collision_detection::WorldPtr& world = std::make_shared()); + + /** \brief construct using a urdf and srdf. + * A RobotModel for the PlanningScene will be created using the urdf and srdf. */ + PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model, + const collision_detection::WorldPtr& world = std::make_shared()); + + static const std::string OCTOMAP_NS; + static const std::string DEFAULT_SCENE_NAME; + + ~PlanningScene(); + + /** \brief Get the name of the planning scene. This is empty by default */ + const std::string& getName() const + { + return name_; + } + + /** \brief Set the name of the planning scene */ + void setName(const std::string& name) + { + name_ = name; + } + + /** \brief Return a new child PlanningScene that uses this one as parent. + * + * The child scene has its own copy of the world. It maintains a list (in + * world_diff_) of changes made to the child world. + * + * The robot_model_, robot_state_, scene_transforms_, and acm_ are not copied. + * They are shared with the parent. So if changes to these are made in the parent they will be visible in the child. + * But if any of these is modified (i.e. if the get*NonConst functions are called) in the child then a copy is made + * and subsequent changes to the corresponding member of the parent will no longer be visible in the child. + */ + PlanningScenePtr diff() const; + + /** \brief Return a new child PlanningScene that uses this one as parent and + * has the diffs specified by \e msg applied. */ + PlanningScenePtr diff(const moveit_msgs::msg::PlanningScene& msg) const; + + /** \brief Get the parent scene (with respect to which the diffs are maintained). This may be empty */ + const PlanningSceneConstPtr& getParent() const + { + return parent_; + } + + /** \brief Get the kinematic model for which the planning scene is maintained */ + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + // the kinematic model does not change + return robot_model_; + } + + /** \brief Get the state at which the robot is assumed to be. */ + const moveit::core::RobotState& getCurrentState() const + { + // if we have an updated state, return it; otherwise, return the parent one + return robot_state_.has_value() ? robot_state_.value() : parent_->getCurrentState(); + } + /** \brief Get the state at which the robot is assumed to be. */ + moveit::core::RobotState& getCurrentStateNonConst(); + + /** \brief Get a copy of the current state with components overwritten by the state message \e update */ + moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const; + + /** + * \name Reasoning about frames + */ + /**@{*/ + + /** \brief Get the frame in which planning is performed */ + const std::string& getPlanningFrame() const + { + // if we have an updated set of transforms, return it; otherwise, return the parent one + return scene_transforms_.has_value() ? scene_transforms_.value()->getTargetFrame() : parent_->getPlanningFrame(); + } + + /** \brief Get the set of fixed transforms from known frames to the planning frame */ + const moveit::core::Transforms& getTransforms() const + { + if (scene_transforms_.has_value() || !parent_) + { + return *scene_transforms_.value(); + } + + // if this planning scene is a child of another, and doesn't have its own custom transforms + return parent_->getTransforms(); + } + + /** \brief Get the set of fixed transforms from known frames to the planning frame. This variant is non-const and also + * updates the current state */ + const moveit::core::Transforms& getTransforms(); + + /** \brief Get the set of fixed transforms from known frames to the planning frame */ + moveit::core::Transforms& getTransformsNonConst(); + + /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached + body id or a collision object. + Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be + successful or not. */ + const Eigen::Isometry3d& getFrameTransform(const std::string& id) const; + + /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached + body id or a collision object. + Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be + successful or not. + Because this function is non-const, the current state transforms are also updated, if needed. */ + const Eigen::Isometry3d& getFrameTransform(const std::string& id); + + /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached + body id or a collision object. + Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be + successful or not. This function also + updates the link transforms of \e state. */ + const Eigen::Isometry3d& getFrameTransform(moveit::core::RobotState& state, const std::string& id) const + { + state.updateLinkTransforms(); + return getFrameTransform(static_cast(state), id); + } + + /** \brief Get the transform corresponding to the frame \e id. This will be known if \e id is a link name, an attached + body id or a collision object. + Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be + successful or not. */ + const Eigen::Isometry3d& getFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; + + /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached + * body id or a collision object */ + bool knowsFrameTransform(const std::string& id) const; + + /** \brief Check if a transform to the frame \e id is known. This will be known if \e id is a link name, an attached + * body id or a collision object */ + bool knowsFrameTransform(const moveit::core::RobotState& state, const std::string& id) const; + + /**@}*/ + + /** + * \name Reasoning about the geometry of the planning scene + */ + /**@{*/ + + const std::string getCollisionDetectorName() const + { + // If no collision detector is allocated, return an empty string + return collision_detector_ ? (collision_detector_->alloc_->getName()) : ""; + } + + /** \brief Get the representation of the world */ + const collision_detection::WorldConstPtr& getWorld() const + { + // we always have a world representation + return world_const_; + } + + /** \brief Get the representation of the world */ + const collision_detection::WorldPtr& getWorldNonConst() + { + // we always have a world representation + return world_; + } + + /** \brief Get the active collision environment */ + const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const + { + return collision_detector_->getCollisionEnv(); + } + + /** \brief Get the active collision detector for the robot */ + const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const + { + return collision_detector_->getCollisionEnvUnpadded(); + } + + /** \brief Get a specific collision detector for the world. If not found return active CollisionWorld. */ + const collision_detection::CollisionEnvConstPtr& getCollisionEnv(const std::string& collision_detector_name) const; + + /** \brief Get a specific collision detector for the unpadded robot. If no found return active unpadded + * CollisionRobot. */ + const collision_detection::CollisionEnvConstPtr& + getCollisionEnvUnpadded(const std::string& collision_detector_name) const; + + /** \brief Get the representation of the collision robot + * This can be used to set padding and link scale on the active collision_robot. */ + const collision_detection::CollisionEnvPtr& getCollisionEnvNonConst(); + + /** \brief Get the allowed collision matrix */ + const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const + { + return acm_.has_value() ? acm_.value() : parent_->getAllowedCollisionMatrix(); + } + + /** \brief Get the allowed collision matrix */ + collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst(); + + /** \brief Set the allowed collision matrix */ + void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm); + + /**@}*/ + + /** + * \name Collision checking with respect to this planning scene + */ + /**@{*/ + + /** \brief Check if the current state is in collision (with the environment or self collision). + If a group name is specified, collision checking is done for that group only (plus descendent links). + Since the function is non-const, the current state transforms are updated before the collision check. */ + bool isStateColliding(const std::string& group = "", bool verbose = false); + + /** \brief Check if the current state is in collision (with the environment or self collision). If a group name is + specified, + collision checking is done for that group only (plus descendent links). It is expected the current state + transforms are up to date. */ + bool isStateColliding(const std::string& group = "", bool verbose = false) const + { + return isStateColliding(getCurrentState(), group, verbose); + } + + /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is + specified, + collision checking is done for that group only (plus descendent links). The link transforms for \e state are + updated before the collision check. */ + bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const + { + state.updateCollisionBodyTransforms(); + return isStateColliding(static_cast(state), group, verbose); + } + + /** \brief Check if a given state is in collision (with the environment or self collision) + If a group name is specified, collision checking is done for that group only (plus descendent links). It is + expected that the link transforms of \e state are up to date. */ + bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "", + bool verbose = false) const; + + /** \brief Check if a given state is in collision (with the environment or self collision) + If a group name is specified, collision checking is done for that group only (plus descendent links). */ + bool isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group = "", + bool verbose = false) const; + + /** \brief Check whether the current state is in collision, and if needed, updates the collision transforms of the + * current state before the computation. */ + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); + + /** \brief Check whether the current state is in collision. The current state is expected to be updated. */ + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes + a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */ + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e + * robot_state are + * expected to be up to date. */ + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given + allowed collision matrix (\e acm). This variant of the function takes + a non-const \e robot_state and updates its link transforms if needed. */ + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given + allowed collision matrix (\e acm). */ + void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Check whether the current state is in collision, + but use a collision_detection::CollisionRobot instance that has no padding. + Since the function is non-const, the current state transforms are also updated if needed. */ + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); + + /** \brief Check whether the current state is in collision, + but use a collision_detection::CollisionRobot instance that has no padding. */ + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision, + but use a collision_detection::CollisionRobot instance that has no padding. */ + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision, + but use a collision_detection::CollisionRobot instance that has no padding. + Update the link transforms of \e robot_state if needed. */ + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given + allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. + This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */ + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given + allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. */ + [[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void + checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Check whether the current state is in self collision */ + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res); + + /** \brief Check whether the current state is in self collision */ + void checkSelfCollision(const collision_detection::CollisionRequest& req, + collision_detection::CollisionResult& res) const; + + /** \brief Check whether a specified state (\e robot_state) is in self collision */ + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state) const; + + /** \brief Check whether a specified state (\e robot_state) is in self collision */ + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state) const; + + /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given + allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */ + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given + allowed collision matrix (\e acm) */ + void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res, + const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Get the names of the links that are involved in collisions for the current state */ + void getCollidingLinks(std::vector& links); + + /** \brief Get the names of the links that are involved in collisions for the current state */ + void getCollidingLinks(std::vector& links) const + { + getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix()); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. + Update the link transforms for \e robot_state if needed. */ + void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state) const + { + robot_state.updateCollisionBodyTransforms(); + getCollidingLinks(links, static_cast(robot_state), getAllowedCollisionMatrix()); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */ + void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state) const + { + getCollidingLinks(links, robot_state, getAllowedCollisionMatrix()); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the + allowed collision matrix (\e acm) */ + void getCollidingLinks(std::vector& links, moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const + { + robot_state.updateCollisionBodyTransforms(); + getCollidingLinks(links, static_cast(robot_state), acm); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the + allowed collision matrix (\e acm) */ + void getCollidingLinks(std::vector& links, const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const; + + /** \brief Get the names of the links that are involved in collisions for the current state. + Update the link transforms for the current state if needed. */ + void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts); + + /** \brief Get the names of the links that are involved in collisions for the current state */ + void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts) const + { + getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix()); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. + * Can be restricted to links part of or updated by \e group_name (plus descendent links) */ + void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, + const moveit::core::RobotState& robot_state, const std::string& group_name = "") const + { + getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. + Update the link transforms for \e robot_state if needed. + Can be restricted to links part of or updated by \e group_name (plus descendent links) */ + void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, + moveit::core::RobotState& robot_state, const std::string& group_name = "") const + { + robot_state.updateCollisionBodyTransforms(); + getCollidingPairs(contacts, static_cast(robot_state), getAllowedCollisionMatrix(), + group_name); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the + allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. + Can be restricted to links part of or updated by \e group_name (plus descendent links) */ + void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, + moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name = "") const + { + robot_state.updateCollisionBodyTransforms(); + getCollidingPairs(contacts, static_cast(robot_state), acm, group_name); + } + + /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the + allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name (plus descendent links) */ + void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, + const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm, + const std::string& group_name = "") const; + + /**@}*/ + + /** + * \name Distance computation + */ + /**@{*/ + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring + * self-collisions) + */ + double distanceToCollision(moveit::core::RobotState& robot_state) const + { + robot_state.updateCollisionBodyTransforms(); + return distanceToCollision(static_cast(robot_state)); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring + * self-collisions) + */ + double distanceToCollision(const moveit::core::RobotState& robot_state) const + { + return getCollisionEnv()->distanceRobot(robot_state, getAllowedCollisionMatrix()); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring + * self-collisions), if the robot has no padding */ + double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state) const + { + robot_state.updateCollisionBodyTransforms(); + return distanceToCollisionUnpadded(static_cast(robot_state)); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision (ignoring + * self-collisions), if the robot has no padding */ + double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state) const + { + return getCollisionEnvUnpadded()->distanceRobot(robot_state, getAllowedCollisionMatrix()); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring + * self-collisions + * and elements that are allowed to collide. */ + double distanceToCollision(moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const + { + robot_state.updateCollisionBodyTransforms(); + return distanceToCollision(static_cast(robot_state), acm); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring + * self-collisions + * and elements that are allowed to collide. */ + double distanceToCollision(const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const + { + return getCollisionEnv()->distanceRobot(robot_state, acm); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring + * self-collisions + * and elements that are allowed to collide, if the robot has no padding. */ + double distanceToCollisionUnpadded(moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const + { + robot_state.updateCollisionBodyTransforms(); + return distanceToCollisionUnpadded(static_cast(robot_state), acm); + } + + /** \brief The distance between the robot model at state \e robot_state to the nearest collision, ignoring + * self-collisions + * and elements that always allowed to collide, if the robot has no padding. */ + double distanceToCollisionUnpadded(const moveit::core::RobotState& robot_state, + const collision_detection::AllowedCollisionMatrix& acm) const + { + return getCollisionEnvUnpadded()->distanceRobot(robot_state, acm); + } + + /**@}*/ + + /** \brief Save the geometry of the planning scene to a stream, as plain text */ + void saveGeometryToStream(std::ostream& out) const; + + /** \brief Load the geometry of the planning scene from a stream */ + bool loadGeometryFromStream(std::istream& in); + + /** \brief Load the geometry of the planning scene from a stream at a certain location using offset*/ + bool loadGeometryFromStream(std::istream& in, const Eigen::Isometry3d& offset); + + /** \brief Fill the message \e scene with the differences between this instance of PlanningScene with respect to the + parent. + If there is no parent, everything is considered to be a diff and the function behaves like getPlanningSceneMsg() + */ + void getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& scene) const; + + /** \brief Construct a message (\e scene) with all the necessary data so that the scene can be later reconstructed to + be + exactly the same using setPlanningSceneMsg() */ + void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene) const; + + /** \brief Construct a message (\e scene) with the data requested in \e comp. If all options in \e comp are filled, + this will be a complete planning scene message */ + void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene, + const moveit_msgs::msg::PlanningSceneComponents& comp) const; + + /** \brief Construct a message (\e collision_object) with the collision object data from the planning_scene for the + * requested object*/ + bool getCollisionObjectMsg(moveit_msgs::msg::CollisionObject& collision_obj, const std::string& ns) const; + + /** \brief Construct a vector of messages (\e collision_objects) with the collision object data for all objects in + * planning_scene */ + void getCollisionObjectMsgs(std::vector& collision_objs) const; + + /** \brief Construct a message (\e attached_collision_object) with the attached collision object data from the + * planning_scene for the requested object*/ + bool getAttachedCollisionObjectMsg(moveit_msgs::msg::AttachedCollisionObject& attached_collision_obj, + const std::string& ns) const; + + /** \brief Construct a vector of messages (\e attached_collision_objects) with the attached collision object data for + * all objects in planning_scene */ + void + getAttachedCollisionObjectMsgs(std::vector& attached_collision_objs) const; + + /** \brief Construct a message (\e octomap) with the octomap data from the planning_scene */ + bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const; + + /** \brief Construct a vector of messages (\e object_colors) with the colors of the objects from the planning_scene */ + void getObjectColorMsgs(std::vector& object_colors) const; + + /** \brief Apply changes to this planning scene as diffs, even if the message itself is not marked as being a diff + (is_diff + member). A parent is not required to exist. However, the existing data in the planning instance is not cleared. + Data from + the message is only appended (and in cases such as e.g., the robot state, is overwritten). */ + bool setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScene& scene); + + /** \brief Set this instance of a planning scene to be the same as the one serialized in the \e scene message, even if + * the message itself is marked as being a diff (is_diff member) */ + bool setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene); + + /** \brief Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the message + * is set */ + bool usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene); + + /** \brief Takes the object message and returns the object pose, shapes and shape poses. + * If the object pose is empty (identity) but the shape pose is set, this uses the shape + * pose as the object pose. The shape pose becomes the identity instead. + */ + bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::msg::CollisionObject& object, + Eigen::Isometry3d& object_pose_in_header_frame, + std::vector& shapes, + EigenSTL::vector_Isometry3d& shape_poses); + + bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject& object); + bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& object); + + bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld& world); + + void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& map); + void processOctomapMsg(const octomap_msgs::msg::Octomap& map); + void processOctomapPtr(const std::shared_ptr& octree, const Eigen::Isometry3d& t); + + /** + * \brief Clear all collision objects in planning scene + */ + void removeAllCollisionObjects(); + + /** \brief Set the current robot state to be \e state. If not + all joint values are specified, the previously maintained + joint values are kept. */ + void setCurrentState(const moveit_msgs::msg::RobotState& state); + + /** \brief Set the current robot state */ + void setCurrentState(const moveit::core::RobotState& state); + + /** \brief Set the callback to be triggered when changes are made to the current scene state */ + void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback); + + /** \brief Set the callback to be triggered when changes are made to the current scene world */ + void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback); + + bool hasObjectColor(const std::string& id) const; + + /** + * \brief Gets the current color of an object. + * \param id The string id of the target object. + * \return The current object color. + */ + const std_msgs::msg::ColorRGBA& getObjectColor(const std::string& id) const; + + /** + * \brief Tries to get the original color of an object, if one has been set before. + * \param id The string id of the target object. + * \return The original object color, if available, otherwise std::nullopt. + */ + std::optional getOriginalObjectColor(const std::string& id) const; + + void setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color); + void removeObjectColor(const std::string& id); + void getKnownObjectColors(ObjectColorMap& kc) const; + + bool hasObjectType(const std::string& id) const; + + const object_recognition_msgs::msg::ObjectType& getObjectType(const std::string& id) const; + void setObjectType(const std::string& id, const object_recognition_msgs::msg::ObjectType& type); + void removeObjectType(const std::string& id); + void getKnownObjectTypes(ObjectTypeMap& kc) const; + + /** \brief Clear the diffs accumulated for this planning scene, with respect to: + * the parent PlanningScene (if it exists) + * the parent CollisionDetector (if it exists) + * This function is a no-op if there is no parent planning scene. */ + void clearDiffs(); + + /** \brief If there is a parent specified for this scene, then the diffs with respect to that parent are applied to a + specified planning scene, whatever + that scene may be. If there is no parent specified, this function is a no-op. */ + void pushDiffs(const PlanningScenePtr& scene); + + /** \brief Make sure that all the data maintained in this + scene is local. All unmodified data is copied from the + parent and the pointer to the parent is discarded. */ + void decoupleParent(); + + /** \brief Specify a predicate that decides whether states are considered valid or invalid for reasons beyond ones + covered by collision checking and constraint evaluation. + This is useful for setting up problem specific constraints (e.g., stability) */ + void setStateFeasibilityPredicate(const StateFeasibilityFn& fn) + { + state_feasibility_ = fn; + } + + /** \brief Get the predicate that decides whether states are considered valid or invalid for reasons beyond ones + * covered by collision checking and constraint evaluation. */ + const StateFeasibilityFn& getStateFeasibilityPredicate() const + { + return state_feasibility_; + } + + /** \brief Specify a predicate that decides whether motion segments are considered valid or invalid for reasons beyond + * ones covered by collision checking and constraint evaluation. */ + void setMotionFeasibilityPredicate(const MotionFeasibilityFn& fn) + { + motion_feasibility_ = fn; + } + + /** \brief Get the predicate that decides whether motion segments are considered valid or invalid for reasons beyond + * ones covered by collision checking and constraint evaluation. */ + const MotionFeasibilityFn& getMotionFeasibilityPredicate() const + { + return motion_feasibility_; + } + + /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by + * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */ + bool isStateFeasible(const moveit_msgs::msg::RobotState& state, bool verbose = false) const; + + /** \brief Check if a given state is feasible, in accordance to the feasibility predicate specified by + * setStateFeasibilityPredicate(). Returns true if no feasibility predicate was specified. */ + bool isStateFeasible(const moveit::core::RobotState& state, bool verbose = false) const; + + /** \brief Check if a given state satisfies a set of constraints */ + bool isStateConstrained(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, + bool verbose = false) const; + + /** \brief Check if a given state satisfies a set of constraints */ + bool isStateConstrained(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr, + bool verbose = false) const; + + /** \brief Check if a given state satisfies a set of constraints */ + bool isStateConstrained(const moveit_msgs::msg::RobotState& state, + const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; + + /** \brief Check if a given state satisfies a set of constraints */ + bool isStateConstrained(const moveit::core::RobotState& state, + const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; + + /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent + * links of \e group. */ + bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", + bool verbose = false) const; + + /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent + * links of \e group. */ + bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; + + /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user + * specified validity conditions hold as well. Includes descendent links of \e group. */ + bool isStateValid(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, + const std::string& group = "", bool verbose = false) const; + + /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user + * specified validity conditions hold as well. Includes descendent links of \e group. */ + bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr, + const std::string& group = "", bool verbose = false) const; + + /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user + * specified validity conditions hold as well. Includes descendent links of \e group. */ + bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, + const std::string& group = "", bool verbose = false) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). + * Includes descendent links of \e group. */ + bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::string& group = "", bool verbose = false, + std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and + * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the + * passed in trajectory. Includes descendent links of \e group. */ + bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and + * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the + * passed in trajectory. Includes descendent links of \e group. */ + bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, + const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and + * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the + * passed in trajectory. Includes descendent links of \e group. */ + bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, + const std::vector& goal_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and + * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the + * passed in trajectory. Includes descendent links of \e group. */ + bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, + const std::vector& goal_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and + * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the + * passed in trajectory. Includes descendent links of \e group. */ + bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, + const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and + * constraint satisfaction). Includes descendent links of \e group. */ + bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). + * Includes descendent links of \e group. */ + bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = nullptr) const; + + /** \brief Get the top \e max_costs cost sources for a specified trajectory. The resulting costs are stored in \e + * costs */ + void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, + std::set& costs, double overlap_fraction = 0.9) const; + + /** \brief Get the top \e max_costs cost sources for a specified trajectory, but only for group \e group_name (plus + * descendent links). The resulting costs are stored in \e costs */ + void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, + const std::string& group_name, std::set& costs, + double overlap_fraction = 0.9) const; + + /** \brief Get the top \e max_costs cost sources for a specified state. The resulting costs are stored in \e costs */ + void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, + std::set& costs) const; + + /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name (plus + * descendent links). The resulting costs are stored in \e costs */ + void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, + std::set& costs) const; + + /** \brief Outputs debug information about the planning scene contents */ + void printKnownObjects(std::ostream& out = std::cout) const; + + /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */ + static PlanningScenePtr clone(const PlanningSceneConstPtr& scene); + + /** \brief Allocate a new collision detector and replace the previous one if there was any. + * + * The collision detector type is specified with (a shared pointer to) an + * allocator which is a subclass of CollisionDetectorAllocator. This + * identifies a combination of CollisionWorld/CollisionRobot which can be + * used together. + * + * A new PlanningScene uses an FCL collision detector by default. + * + * example: to add FCL collision detection (normally not necessary) call + * planning_scene->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); + * + * */ + void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator) + { + allocateCollisionDetector(allocator, nullptr /* no parent_detector */); + } + +private: + /* Private constructor used by the diff() methods. */ + PlanningScene(const PlanningSceneConstPtr& parent); + + /* Initialize the scene. This should only be called by the constructors. + * Requires a valid robot_model_ */ + void initialize(); + + /* Helper functions for processing collision objects */ + bool processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object); + bool processCollisionObjectRemove(const moveit_msgs::msg::CollisionObject& object); + bool processCollisionObjectMove(const moveit_msgs::msg::CollisionObject& object); + + MOVEIT_STRUCT_FORWARD(CollisionDetector); + + /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */ + void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator, + const CollisionDetectorPtr& parent_detector); + + /* \brief A set of compatible collision detectors */ + struct CollisionDetector + { + collision_detection::CollisionDetectorAllocatorPtr alloc_; + collision_detection::CollisionEnvPtr cenv_; // never nullptr + collision_detection::CollisionEnvConstPtr cenv_const_; + + collision_detection::CollisionEnvPtr cenv_unpadded_; + collision_detection::CollisionEnvConstPtr cenv_unpadded_const_; + + const collision_detection::CollisionEnvConstPtr& getCollisionEnv() const + { + return cenv_const_; + } + const collision_detection::CollisionEnvConstPtr& getCollisionEnvUnpadded() const + { + return cenv_unpadded_const_; + } + void copyPadding(const CollisionDetector& src); + }; + friend struct CollisionDetector; + + std::string name_; // may be empty + + PlanningSceneConstPtr parent_; // Null unless this is a diff scene + + moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) + + std::optional robot_state_; // if there is no value use parent's + + // Called when changes are made to attached bodies + moveit::core::AttachedBodyCallback current_state_attached_body_callback_; + + // This variable is not necessarily used by child planning scenes + // This Transforms class is actually a SceneTransforms class + std::optional scene_transforms_; // if there is no value use parent's + + collision_detection::WorldPtr world_; // never nullptr, never shared with parent/child + collision_detection::WorldConstPtr world_const_; // copy of world_ + collision_detection::WorldDiffPtr world_diff_; // nullptr unless this is a diff scene + collision_detection::World::ObserverCallbackFn current_world_object_update_callback_; + collision_detection::World::ObserverHandle current_world_object_update_observer_handle_; + + CollisionDetectorPtr collision_detector_; // Never nullptr. + + std::optional acm_; // if there is no value use parent's + + StateFeasibilityFn state_feasibility_; + MotionFeasibilityFn motion_feasibility_; + + // Maps of current and original object colors (to manage attaching/detaching objects) + std::unique_ptr object_colors_; + std::unique_ptr original_object_colors_; + + // a map of object types + std::optional object_types_; +}; +} // namespace planning_scene diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 2c3241c4ae..bf8ea6df13 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -35,16 +35,16 @@ /* Author: Ioan Sucan */ #include -#include -#include -#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_core/planning_scene/test/test_collision_objects.cpp b/moveit_core/planning_scene/test/test_collision_objects.cpp index 945f6e39a4..69d45f3927 100644 --- a/moveit_core/planning_scene/test/test_collision_objects.cpp +++ b/moveit_core/planning_scene/test/test_collision_objects.cpp @@ -35,9 +35,9 @@ /* Author: Michael 'v4hn' Goerner */ #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_core/planning_scene/test/test_multi_threaded.cpp b/moveit_core/planning_scene/test/test_multi_threaded.cpp index 4d281f8b5d..a8dc71d824 100644 --- a/moveit_core/planning_scene/test/test_multi_threaded.cpp +++ b/moveit_core/planning_scene/test/test_multi_threaded.cpp @@ -34,15 +34,15 @@ /* Author: Jens Petit */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include const int TRIALS = 1000; const int THREADS = 2; diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 909f3f6576..ae697ee8a8 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -35,10 +35,10 @@ /* Author: Ioan Sucan */ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -47,8 +47,8 @@ #include #include -#include -#include +#include +#include // Test not setting the object's pose should use the shape pose as the object pose TEST(PlanningScene, TestOneShapeObjectPose) diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.h b/moveit_core/robot_model/include/moveit/robot_model/aabb.h index ce0315fdd2..e8f640bd0c 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,19 +47,5 @@ /* Author: Martin Pecka */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** \brief Represents an axis-aligned bounding box. */ -class AABB : public Eigen::AlignedBox3d -{ -public: - /** \brief Extend with a box transformed by the given transform. */ - void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.hpp b/moveit_core/robot_model/include/moveit/robot_model/aabb.hpp new file mode 100644 index 0000000000..ce0315fdd2 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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: Martin Pecka */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** \brief Represents an axis-aligned bounding box. */ +class AABB : public Eigen::AlignedBox3d +{ +public: + /** \brief Extend with a box transformed by the given transform. */ + void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index 9f3356fdcb..751e42cc37 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,35 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** \brief A fixed joint */ -class FixedJointModel : public JointModel -{ -public: - FixedJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); - - void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds) const override; - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds, const double* near, - const double distance) const override; - bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; - bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; - - void interpolate(const double* from, const double* to, const double t, double* state) const override; - unsigned int getStateSpaceDimension() const override; - double getMaximumExtent(const Bounds& other_bounds) const override; - double distance(const double* values1, const double* values2) const override; - - void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; - void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.hpp new file mode 100644 index 0000000000..c7ab04c544 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** \brief A fixed joint */ +class FixedJointModel : public JointModel +{ +public: + FixedJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); + + void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds) const override; + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds, const double* near, + const double distance) const override; + bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; + bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; + + void interpolate(const double* from, const double* to, const double t, double* state) const override; + unsigned int getStateSpaceDimension() const override; + double getMaximumExtent(const Bounds& other_bounds) const override; + double distance(const double* values1, const double* values2) const override; + + void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; + void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index 484779512d..0eba5db712 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,58 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** \brief A floating joint */ -class FloatingJointModel : public JointModel -{ -public: - FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); - - void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds) const override; - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds, const double* near, - const double distance) const override; - bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; - bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; - - void interpolate(const double* from, const double* to, const double t, double* state) const override; - unsigned int getStateSpaceDimension() const override; - double getMaximumExtent(const Bounds& other_bounds) const override; - double distance(const double* values1, const double* values2) const override; - - void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; - void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; - - double getAngularDistanceWeight() const - { - return angular_distance_weight_; - } - - void setAngularDistanceWeight(double weight) - { - angular_distance_weight_ = weight; - } - - /// Normalize the quaternion (warn if norm is 0, and set to identity); - /// Return true if any change was made - bool normalizeRotation(double* values) const; - - /// Get the distance between the rotation components of two states - double distanceRotation(const double* values1, const double* values2) const; - - /// Get the distance between the translation components of two states - double distanceTranslation(const double* values1, const double* values2) const; - -private: - double angular_distance_weight_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.hpp new file mode 100644 index 0000000000..859bcd2c7a --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.hpp @@ -0,0 +1,92 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** \brief A floating joint */ +class FloatingJointModel : public JointModel +{ +public: + FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); + + void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds) const override; + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds, const double* near, + const double distance) const override; + bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; + bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; + + void interpolate(const double* from, const double* to, const double t, double* state) const override; + unsigned int getStateSpaceDimension() const override; + double getMaximumExtent(const Bounds& other_bounds) const override; + double distance(const double* values1, const double* values2) const override; + + void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; + void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; + + double getAngularDistanceWeight() const + { + return angular_distance_weight_; + } + + void setAngularDistanceWeight(double weight) + { + angular_distance_weight_ = weight; + } + + /// Normalize the quaternion (warn if norm is 0, and set to identity); + /// Return true if any change was made + bool normalizeRotation(double* values) const; + + /// Get the distance between the rotation components of two states + double distanceRotation(const double* values1, const double* values2) const; + + /// Get the distance between the translation components of two states + double distanceTranslation(const double* values1, const double* values2) const; + +private: + double angular_distance_weight_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index d75796440f..ea06d11e55 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,511 +48,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -#undef near - -namespace moveit -{ -namespace core -{ -struct VariableBounds -{ - VariableBounds() - : min_position_(0.0) - , max_position_(0.0) - , position_bounded_(false) - , min_velocity_(0.0) - , max_velocity_(0.0) - , velocity_bounded_(false) - , min_acceleration_(0.0) - , max_acceleration_(0.0) - , acceleration_bounded_(false) - , min_jerk_(0.0) - , max_jerk_(0.0) - , jerk_bounded_(false) - { - } - - double min_position_; - double max_position_; - bool position_bounded_; - - double min_velocity_; - double max_velocity_; - bool velocity_bounded_; - - double min_acceleration_; - double max_acceleration_; - bool acceleration_bounded_; - - double min_jerk_; - double max_jerk_; - bool jerk_bounded_; -}; - -class LinkModel; -class JointModel; - -/** \brief Data type for holding mappings from variable names to their position in a state vector */ -typedef std::map VariableIndexMap; - -/** \brief Data type for holding mappings from variable names to their bounds */ -using VariableBoundsMap = std::map; - -/** \brief Map of names to instances for JointModel */ -using JointModelMap = std::map; - -/** \brief Map of names to const instances for JointModel */ -using JointModelMapConst = std::map; - -/** \brief A joint from the robot. Models the transform that - this joint applies in the kinematic chain. A joint - consists of multiple variables. In the simplest case, when - the joint is a single DOF, there is only one variable and - its name is the same as the joint's name. For multi-DOF - joints, each variable has a local name (e.g., \e x, \e y) - but the full variable name as seen from the outside of - this class is a concatenation of the "joint name"/"local - name" (e.g., a joint named 'base' with local variables 'x' - and 'y' will store its full variable names as 'base/x' and - 'base/y'). Local names are never used to reference - variables directly. */ -class JointModel -{ -public: - /** \brief The different types of joints we support */ - enum JointType - { - UNKNOWN, - REVOLUTE, - PRISMATIC, - PLANAR, - FLOATING, - FIXED - }; - - /** \brief The datatype for the joint bounds */ - using Bounds = std::vector; - - /** - * @brief Constructs a joint named \e name - * - * @param[in] name The joint name - * @param[in] index The index of the joint in the RobotModel - * @param[in] first_variable_index The index of the first variable in the RobotModel - */ - JointModel(const std::string& name, size_t joint_index, size_t first_variable_index); - - virtual ~JointModel(); - - /** \brief Get the name of the joint */ - const std::string& getName() const - { - return name_; - } - - /** \brief Get the type of joint */ - JointType getType() const - { - return type_; - } - - /** \brief Get the type of joint as a string */ - std::string getTypeName() const; - - /** \brief Get the link that this joint connects to. The - robot is assumed to start with a joint, so the root - joint will return nullptr here. */ - const LinkModel* getParentLinkModel() const - { - return parent_link_model_; - } - - /** \brief Get the link that this joint connects to. There will always be such a link */ - const LinkModel* getChildLinkModel() const - { - return child_link_model_; - } - - void setParentLinkModel(const LinkModel* link) - { - parent_link_model_ = link; - } - - void setChildLinkModel(const LinkModel* link) - { - child_link_model_ = link; - } - - /** @name Reason about the variables that make up this joint - @{ */ - - /** \brief Get the names of the variables that make up this joint, in the order they appear in corresponding states. - For single DOF joints, this will be just the joint name. For multi-DOF joints these will be the joint name - followed by "/", followed by - the local names of the variables */ - const std::vector& getVariableNames() const - { - return variable_names_; - } - - /** \brief Get the local names of the variable that make up the joint (suffixes that are attached to joint names to - construct the variable names). - For single DOF joints, this will be empty. */ - const std::vector& getLocalVariableNames() const - { - return local_variable_names_; - } - - /** \brief Check if a particular variable is known to this joint */ - bool hasVariable(const std::string& variable) const - { - return variable_index_map_.find(variable) != variable_index_map_.end(); - } - - /** \brief Get the number of variables that describe this joint */ - std::size_t getVariableCount() const - { - return variable_names_.size(); - } - - /** \brief Get the index of this joint's first variable within the full robot state */ - size_t getFirstVariableIndex() const - { - return first_variable_index_; - } - - /** \brief Get the index of this joint within the robot model */ - size_t getJointIndex() const - { - return joint_index_; - } - - /** \brief Get the index of the variable within this joint */ - size_t getLocalVariableIndex(const std::string& variable) const; - - /** @} */ - - /** @name Functionality specific to computing state values - @{ */ - - /** \brief Provide a default value for the joint given the default joint variable bounds (maintained internally). - Most joints will use the default implementation provided in this base class, but the quaternion - for example needs a different implementation. Enough memory is assumed to be allocated. */ - void getVariableDefaultPositions(double* values) const - { - getVariableDefaultPositions(values, variable_bounds_); - } - - /** \brief Provide a default value for the joint given the joint variable bounds. - Most joints will use the default implementation provided in this base class, but the quaternion - for example needs a different implementation. Enough memory is assumed to be allocated. */ - virtual void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const = 0; - - /** \brief Provide random values for the joint variables (within default bounds). Enough memory is assumed to be - * allocated. */ - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const - { - getVariableRandomPositions(rng, values, variable_bounds_); - } - - /** \brief Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be - * allocated. */ - virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds) const = 0; - - /** \brief Provide random values for the joint variables (within default bounds). Enough memory is assumed to be - * allocated. */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, - const double distance) const - { - getVariableRandomPositionsNearBy(rng, values, variable_bounds_, near, distance); - } - - /** \brief Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be - * allocated. */ - virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds, const double* near, - const double distance) const = 0; - - /** @} */ - - /** @name Functionality specific to verifying bounds - @{ */ - - /** \brief Check if the set of values for the variables of this joint are within bounds. */ - bool satisfiesPositionBounds(const double* values, double margin = 0.0) const - { - return satisfiesPositionBounds(values, variable_bounds_, margin); - } - - /** \brief Check if the set of position values for the variables of this joint are within bounds, up to some margin. - */ - virtual bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const = 0; - - /** \brief Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous - revolute joints are made between -Pi and Pi. - Returns true if changes were made. */ - bool enforcePositionBounds(double* values) const - { - return enforcePositionBounds(values, variable_bounds_); - } - - /** \brief Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous - revolute joints are made between -Pi and Pi. - Return true if changes were made. */ - virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0; - - /** Harmonize position of revolute joints, adding/subtracting multiples of 2*Pi to bring them back into bounds. - * Return true if changes were made. */ - virtual bool harmonizePosition(double* values, const Bounds& other_bounds) const; - bool harmonizePosition(double* values) const - { - return harmonizePosition(values, variable_bounds_); - } - - /** \brief Check if the set of velocities for the variables of this joint are within bounds. */ - bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const - { - return satisfiesVelocityBounds(values, variable_bounds_, margin); - } - - /** \brief Check if the set of velocities for the variables of this joint are within bounds, up to some margin. */ - virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const; - - /** \brief Force the specified velocities to be within bounds. Return true if changes were made. */ - bool enforceVelocityBounds(double* values) const - { - return enforceVelocityBounds(values, variable_bounds_); - } - - /** \brief Force the specified velocities to be inside bounds. Return true if changes were made. */ - virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const; - - /** \brief Check if the set of accelerations for the variables of this joint are within bounds. */ - bool satisfiesAccelerationBounds(const double* values, double margin = 0.0) const - { - return satisfiesAccelerationBounds(values, variable_bounds_, margin); - } - - /** \brief Check if the set of accelerations for the variables of this joint are within bounds, up to some margin. */ - virtual bool satisfiesAccelerationBounds(const double* values, const Bounds& other_bounds, double margin) const; - - /** \brief Check if the set of jerks for the variables of this joint are within bounds. */ - bool satisfiesJerkBounds(const double* values, double margin = 0.0) const - { - return satisfiesJerkBounds(values, variable_bounds_, margin); - } - - /** \brief Check if the set of jerks for the variables of this joint are within bounds, up to some margin. */ - virtual bool satisfiesJerkBounds(const double* values, const Bounds& other_bounds, double margin) const; - - /** \brief Get the bounds for a variable. Throw an exception if the variable was not found */ - const VariableBounds& getVariableBounds(const std::string& variable) const; - - /** \brief Get the variable bounds for this joint, in the same order as the names returned by getVariableNames() */ - const Bounds& getVariableBounds() const - { - return variable_bounds_; - } - - /** \brief Set the lower and upper bounds for a variable. Throw an exception if the variable was not found. */ - void setVariableBounds(const std::string& variable, const VariableBounds& bounds); - - /** \brief Override joint limits loaded from URDF. Unknown variables are ignored. */ - void setVariableBounds(const std::vector& jlim); - - /** \brief Get the joint limits known to this model, as a message. */ - const std::vector& getVariableBoundsMsg() const - { - return variable_bounds_msg_; - } - - /** @} */ - - /** \brief Compute the distance between two joint states of the same model (represented by the variable values) */ - virtual double distance(const double* value1, const double* value2) const = 0; - - /** \brief Get the factor that should be applied to the value returned by distance() when that value is used in - * compound distances */ - double getDistanceFactor() const - { - return distance_factor_; - } - - /** \brief Set the factor that should be applied to the value returned by distance() when that value is used in - * compound distances */ - void setDistanceFactor(double factor) - { - distance_factor_ = factor; - } - - /** \brief Get the dimension of the state space that corresponds to this joint */ - virtual unsigned int getStateSpaceDimension() const = 0; - - /** \brief Get the joint this one is mimicking */ - const JointModel* getMimic() const - { - return mimic_; - } - - /** \brief If mimicking a joint, this is the offset added to that joint's value */ - double getMimicOffset() const - { - return mimic_offset_; - } - - /** \brief If mimicking a joint, this is the multiplicative factor for that joint's value */ - double getMimicFactor() const - { - return mimic_factor_; - } - - /** \brief Mark this joint as mimicking \e mimic using \e factor and \e offset */ - void setMimic(const JointModel* mimic, double factor, double offset); - - /** \brief The joint models whose values would be modified if the value of this joint changed */ - const std::vector& getMimicRequests() const - { - return mimic_requests_; - } - - /** \brief Notify this joint that there is another joint that mimics it */ - void addMimicRequest(const JointModel* joint); - void addDescendantJointModel(const JointModel* joint); - void addDescendantLinkModel(const LinkModel* link); - - /** \brief Get all the link models that descend from this joint, in the kinematic tree */ - const std::vector& getDescendantLinkModels() const - { - return descendant_link_models_; - } - - /** \brief Get all the joint models that descend from this joint, in the kinematic tree */ - const std::vector& getDescendantJointModels() const - { - return descendant_joint_models_; - } - - /** \brief Get all the non-fixed joint models that descend from this joint, in the kinematic tree */ - const std::vector& getNonFixedDescendantJointModels() const - { - return non_fixed_descendant_joint_models_; - } - - /** \brief Check if this joint is passive */ - bool isPassive() const - { - return passive_; - } - - void setPassive(bool flag) - { - passive_ = flag; - } - - /** \brief Computes the state that lies at time @e t in [0, 1] on the segment that connects @e from state to @e to - state. - The memory location of @e state is not required to be different from the memory of either - @e from or @e to. */ - virtual void interpolate(const double* from, const double* to, const double t, double* state) const = 0; - - /** \brief Get the extent of the state space (the maximum value distance() can ever report) */ - virtual double getMaximumExtent(const Bounds& other_bounds) const = 0; - - double getMaximumExtent() const - { - return getMaximumExtent(variable_bounds_); - } - - /** @name Computing transforms - @{ */ - - /** \brief Given the joint values for a joint, compute the corresponding transform. The computed transform is - * guaranteed to be a valid isometry. */ - virtual void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const = 0; - - /** \brief Given the transform generated by joint, compute the corresponding joint values. Make sure the passed - * transform is a valid isometry. */ - virtual void computeVariablePositions(const Eigen::Isometry3d& transform, double* joint_values) const = 0; - - /** @} */ - -private: - /** \brief Name of the joint */ - std::string name_; - - /** \brief Index for this joint in the array of joints of the complete model */ - size_t joint_index_; - - /** \brief The index of this joint's first variable, in the complete robot state */ - size_t first_variable_index_; - -protected: - void computeVariableBoundsMsg(); - - /** \brief The type of joint */ - JointType type_; - - /** \brief The local names to use for the variables that make up this joint */ - std::vector local_variable_names_; - - /** \brief The full names to use for the variables that make up this joint */ - std::vector variable_names_; - - /** \brief The bounds for each variable (low, high) in the same order as variable_names_ */ - Bounds variable_bounds_; - - std::vector variable_bounds_msg_; - - /** \brief Map from variable names to the corresponding index in variable_names_ (indexing makes sense within the - * JointModel only) */ - VariableIndexMap variable_index_map_; - - /** \brief The link before this joint */ - const LinkModel* parent_link_model_; - - /** \brief The link after this joint */ - const LinkModel* child_link_model_; - - /** \brief The joint this one mimics (nullptr for joints that do not mimic) */ - const JointModel* mimic_; - - /** \brief The multiplier to the mimic joint */ - double mimic_factor_; - - /** \brief The offset to the mimic joint */ - double mimic_offset_; - - /** \brief The set of joints that should get a value copied to them when this joint changes */ - std::vector mimic_requests_; - - /** \brief Pointers to all the links that will be moved if this joint changes value */ - std::vector descendant_link_models_; - - /** \brief Pointers to all the joints that follow this one in the kinematic tree (including mimic joints) */ - std::vector descendant_joint_models_; - - /** \brief Pointers to all the joints that follow this one in the kinematic tree, including mimic joints, but - * excluding fixed joints */ - std::vector non_fixed_descendant_joint_models_; - - /** \brief Specify whether this joint is marked as passive in the SRDF */ - bool passive_; - - /** \brief The factor applied to the distance between two joint states */ - double distance_factor_; -}; - -/** \brief Operator overload for printing variable bounds to a stream */ -std::ostream& operator<<(std::ostream& out, const VariableBounds& b); -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/joint_model.hpp new file mode 100644 index 0000000000..d75796440f --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.hpp @@ -0,0 +1,546 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#undef near + +namespace moveit +{ +namespace core +{ +struct VariableBounds +{ + VariableBounds() + : min_position_(0.0) + , max_position_(0.0) + , position_bounded_(false) + , min_velocity_(0.0) + , max_velocity_(0.0) + , velocity_bounded_(false) + , min_acceleration_(0.0) + , max_acceleration_(0.0) + , acceleration_bounded_(false) + , min_jerk_(0.0) + , max_jerk_(0.0) + , jerk_bounded_(false) + { + } + + double min_position_; + double max_position_; + bool position_bounded_; + + double min_velocity_; + double max_velocity_; + bool velocity_bounded_; + + double min_acceleration_; + double max_acceleration_; + bool acceleration_bounded_; + + double min_jerk_; + double max_jerk_; + bool jerk_bounded_; +}; + +class LinkModel; +class JointModel; + +/** \brief Data type for holding mappings from variable names to their position in a state vector */ +typedef std::map VariableIndexMap; + +/** \brief Data type for holding mappings from variable names to their bounds */ +using VariableBoundsMap = std::map; + +/** \brief Map of names to instances for JointModel */ +using JointModelMap = std::map; + +/** \brief Map of names to const instances for JointModel */ +using JointModelMapConst = std::map; + +/** \brief A joint from the robot. Models the transform that + this joint applies in the kinematic chain. A joint + consists of multiple variables. In the simplest case, when + the joint is a single DOF, there is only one variable and + its name is the same as the joint's name. For multi-DOF + joints, each variable has a local name (e.g., \e x, \e y) + but the full variable name as seen from the outside of + this class is a concatenation of the "joint name"/"local + name" (e.g., a joint named 'base' with local variables 'x' + and 'y' will store its full variable names as 'base/x' and + 'base/y'). Local names are never used to reference + variables directly. */ +class JointModel +{ +public: + /** \brief The different types of joints we support */ + enum JointType + { + UNKNOWN, + REVOLUTE, + PRISMATIC, + PLANAR, + FLOATING, + FIXED + }; + + /** \brief The datatype for the joint bounds */ + using Bounds = std::vector; + + /** + * @brief Constructs a joint named \e name + * + * @param[in] name The joint name + * @param[in] index The index of the joint in the RobotModel + * @param[in] first_variable_index The index of the first variable in the RobotModel + */ + JointModel(const std::string& name, size_t joint_index, size_t first_variable_index); + + virtual ~JointModel(); + + /** \brief Get the name of the joint */ + const std::string& getName() const + { + return name_; + } + + /** \brief Get the type of joint */ + JointType getType() const + { + return type_; + } + + /** \brief Get the type of joint as a string */ + std::string getTypeName() const; + + /** \brief Get the link that this joint connects to. The + robot is assumed to start with a joint, so the root + joint will return nullptr here. */ + const LinkModel* getParentLinkModel() const + { + return parent_link_model_; + } + + /** \brief Get the link that this joint connects to. There will always be such a link */ + const LinkModel* getChildLinkModel() const + { + return child_link_model_; + } + + void setParentLinkModel(const LinkModel* link) + { + parent_link_model_ = link; + } + + void setChildLinkModel(const LinkModel* link) + { + child_link_model_ = link; + } + + /** @name Reason about the variables that make up this joint + @{ */ + + /** \brief Get the names of the variables that make up this joint, in the order they appear in corresponding states. + For single DOF joints, this will be just the joint name. For multi-DOF joints these will be the joint name + followed by "/", followed by + the local names of the variables */ + const std::vector& getVariableNames() const + { + return variable_names_; + } + + /** \brief Get the local names of the variable that make up the joint (suffixes that are attached to joint names to + construct the variable names). + For single DOF joints, this will be empty. */ + const std::vector& getLocalVariableNames() const + { + return local_variable_names_; + } + + /** \brief Check if a particular variable is known to this joint */ + bool hasVariable(const std::string& variable) const + { + return variable_index_map_.find(variable) != variable_index_map_.end(); + } + + /** \brief Get the number of variables that describe this joint */ + std::size_t getVariableCount() const + { + return variable_names_.size(); + } + + /** \brief Get the index of this joint's first variable within the full robot state */ + size_t getFirstVariableIndex() const + { + return first_variable_index_; + } + + /** \brief Get the index of this joint within the robot model */ + size_t getJointIndex() const + { + return joint_index_; + } + + /** \brief Get the index of the variable within this joint */ + size_t getLocalVariableIndex(const std::string& variable) const; + + /** @} */ + + /** @name Functionality specific to computing state values + @{ */ + + /** \brief Provide a default value for the joint given the default joint variable bounds (maintained internally). + Most joints will use the default implementation provided in this base class, but the quaternion + for example needs a different implementation. Enough memory is assumed to be allocated. */ + void getVariableDefaultPositions(double* values) const + { + getVariableDefaultPositions(values, variable_bounds_); + } + + /** \brief Provide a default value for the joint given the joint variable bounds. + Most joints will use the default implementation provided in this base class, but the quaternion + for example needs a different implementation. Enough memory is assumed to be allocated. */ + virtual void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const = 0; + + /** \brief Provide random values for the joint variables (within default bounds). Enough memory is assumed to be + * allocated. */ + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const + { + getVariableRandomPositions(rng, values, variable_bounds_); + } + + /** \brief Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be + * allocated. */ + virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds) const = 0; + + /** \brief Provide random values for the joint variables (within default bounds). Enough memory is assumed to be + * allocated. */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, + const double distance) const + { + getVariableRandomPositionsNearBy(rng, values, variable_bounds_, near, distance); + } + + /** \brief Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be + * allocated. */ + virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds, const double* near, + const double distance) const = 0; + + /** @} */ + + /** @name Functionality specific to verifying bounds + @{ */ + + /** \brief Check if the set of values for the variables of this joint are within bounds. */ + bool satisfiesPositionBounds(const double* values, double margin = 0.0) const + { + return satisfiesPositionBounds(values, variable_bounds_, margin); + } + + /** \brief Check if the set of position values for the variables of this joint are within bounds, up to some margin. + */ + virtual bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const = 0; + + /** \brief Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous + revolute joints are made between -Pi and Pi. + Returns true if changes were made. */ + bool enforcePositionBounds(double* values) const + { + return enforcePositionBounds(values, variable_bounds_); + } + + /** \brief Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous + revolute joints are made between -Pi and Pi. + Return true if changes were made. */ + virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0; + + /** Harmonize position of revolute joints, adding/subtracting multiples of 2*Pi to bring them back into bounds. + * Return true if changes were made. */ + virtual bool harmonizePosition(double* values, const Bounds& other_bounds) const; + bool harmonizePosition(double* values) const + { + return harmonizePosition(values, variable_bounds_); + } + + /** \brief Check if the set of velocities for the variables of this joint are within bounds. */ + bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const + { + return satisfiesVelocityBounds(values, variable_bounds_, margin); + } + + /** \brief Check if the set of velocities for the variables of this joint are within bounds, up to some margin. */ + virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const; + + /** \brief Force the specified velocities to be within bounds. Return true if changes were made. */ + bool enforceVelocityBounds(double* values) const + { + return enforceVelocityBounds(values, variable_bounds_); + } + + /** \brief Force the specified velocities to be inside bounds. Return true if changes were made. */ + virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const; + + /** \brief Check if the set of accelerations for the variables of this joint are within bounds. */ + bool satisfiesAccelerationBounds(const double* values, double margin = 0.0) const + { + return satisfiesAccelerationBounds(values, variable_bounds_, margin); + } + + /** \brief Check if the set of accelerations for the variables of this joint are within bounds, up to some margin. */ + virtual bool satisfiesAccelerationBounds(const double* values, const Bounds& other_bounds, double margin) const; + + /** \brief Check if the set of jerks for the variables of this joint are within bounds. */ + bool satisfiesJerkBounds(const double* values, double margin = 0.0) const + { + return satisfiesJerkBounds(values, variable_bounds_, margin); + } + + /** \brief Check if the set of jerks for the variables of this joint are within bounds, up to some margin. */ + virtual bool satisfiesJerkBounds(const double* values, const Bounds& other_bounds, double margin) const; + + /** \brief Get the bounds for a variable. Throw an exception if the variable was not found */ + const VariableBounds& getVariableBounds(const std::string& variable) const; + + /** \brief Get the variable bounds for this joint, in the same order as the names returned by getVariableNames() */ + const Bounds& getVariableBounds() const + { + return variable_bounds_; + } + + /** \brief Set the lower and upper bounds for a variable. Throw an exception if the variable was not found. */ + void setVariableBounds(const std::string& variable, const VariableBounds& bounds); + + /** \brief Override joint limits loaded from URDF. Unknown variables are ignored. */ + void setVariableBounds(const std::vector& jlim); + + /** \brief Get the joint limits known to this model, as a message. */ + const std::vector& getVariableBoundsMsg() const + { + return variable_bounds_msg_; + } + + /** @} */ + + /** \brief Compute the distance between two joint states of the same model (represented by the variable values) */ + virtual double distance(const double* value1, const double* value2) const = 0; + + /** \brief Get the factor that should be applied to the value returned by distance() when that value is used in + * compound distances */ + double getDistanceFactor() const + { + return distance_factor_; + } + + /** \brief Set the factor that should be applied to the value returned by distance() when that value is used in + * compound distances */ + void setDistanceFactor(double factor) + { + distance_factor_ = factor; + } + + /** \brief Get the dimension of the state space that corresponds to this joint */ + virtual unsigned int getStateSpaceDimension() const = 0; + + /** \brief Get the joint this one is mimicking */ + const JointModel* getMimic() const + { + return mimic_; + } + + /** \brief If mimicking a joint, this is the offset added to that joint's value */ + double getMimicOffset() const + { + return mimic_offset_; + } + + /** \brief If mimicking a joint, this is the multiplicative factor for that joint's value */ + double getMimicFactor() const + { + return mimic_factor_; + } + + /** \brief Mark this joint as mimicking \e mimic using \e factor and \e offset */ + void setMimic(const JointModel* mimic, double factor, double offset); + + /** \brief The joint models whose values would be modified if the value of this joint changed */ + const std::vector& getMimicRequests() const + { + return mimic_requests_; + } + + /** \brief Notify this joint that there is another joint that mimics it */ + void addMimicRequest(const JointModel* joint); + void addDescendantJointModel(const JointModel* joint); + void addDescendantLinkModel(const LinkModel* link); + + /** \brief Get all the link models that descend from this joint, in the kinematic tree */ + const std::vector& getDescendantLinkModels() const + { + return descendant_link_models_; + } + + /** \brief Get all the joint models that descend from this joint, in the kinematic tree */ + const std::vector& getDescendantJointModels() const + { + return descendant_joint_models_; + } + + /** \brief Get all the non-fixed joint models that descend from this joint, in the kinematic tree */ + const std::vector& getNonFixedDescendantJointModels() const + { + return non_fixed_descendant_joint_models_; + } + + /** \brief Check if this joint is passive */ + bool isPassive() const + { + return passive_; + } + + void setPassive(bool flag) + { + passive_ = flag; + } + + /** \brief Computes the state that lies at time @e t in [0, 1] on the segment that connects @e from state to @e to + state. + The memory location of @e state is not required to be different from the memory of either + @e from or @e to. */ + virtual void interpolate(const double* from, const double* to, const double t, double* state) const = 0; + + /** \brief Get the extent of the state space (the maximum value distance() can ever report) */ + virtual double getMaximumExtent(const Bounds& other_bounds) const = 0; + + double getMaximumExtent() const + { + return getMaximumExtent(variable_bounds_); + } + + /** @name Computing transforms + @{ */ + + /** \brief Given the joint values for a joint, compute the corresponding transform. The computed transform is + * guaranteed to be a valid isometry. */ + virtual void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const = 0; + + /** \brief Given the transform generated by joint, compute the corresponding joint values. Make sure the passed + * transform is a valid isometry. */ + virtual void computeVariablePositions(const Eigen::Isometry3d& transform, double* joint_values) const = 0; + + /** @} */ + +private: + /** \brief Name of the joint */ + std::string name_; + + /** \brief Index for this joint in the array of joints of the complete model */ + size_t joint_index_; + + /** \brief The index of this joint's first variable, in the complete robot state */ + size_t first_variable_index_; + +protected: + void computeVariableBoundsMsg(); + + /** \brief The type of joint */ + JointType type_; + + /** \brief The local names to use for the variables that make up this joint */ + std::vector local_variable_names_; + + /** \brief The full names to use for the variables that make up this joint */ + std::vector variable_names_; + + /** \brief The bounds for each variable (low, high) in the same order as variable_names_ */ + Bounds variable_bounds_; + + std::vector variable_bounds_msg_; + + /** \brief Map from variable names to the corresponding index in variable_names_ (indexing makes sense within the + * JointModel only) */ + VariableIndexMap variable_index_map_; + + /** \brief The link before this joint */ + const LinkModel* parent_link_model_; + + /** \brief The link after this joint */ + const LinkModel* child_link_model_; + + /** \brief The joint this one mimics (nullptr for joints that do not mimic) */ + const JointModel* mimic_; + + /** \brief The multiplier to the mimic joint */ + double mimic_factor_; + + /** \brief The offset to the mimic joint */ + double mimic_offset_; + + /** \brief The set of joints that should get a value copied to them when this joint changes */ + std::vector mimic_requests_; + + /** \brief Pointers to all the links that will be moved if this joint changes value */ + std::vector descendant_link_models_; + + /** \brief Pointers to all the joints that follow this one in the kinematic tree (including mimic joints) */ + std::vector descendant_joint_models_; + + /** \brief Pointers to all the joints that follow this one in the kinematic tree, including mimic joints, but + * excluding fixed joints */ + std::vector non_fixed_descendant_joint_models_; + + /** \brief Specify whether this joint is marked as passive in the SRDF */ + bool passive_; + + /** \brief The factor applied to the distance between two joint states */ + double distance_factor_; +}; + +/** \brief Operator overload for printing variable bounds to a stream */ +std::ostream& operator<<(std::ostream& out, const VariableBounds& b); +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index c63112fde4..d9e53b3061 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,738 +48,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace core -{ -class RobotModel; -class JointModelGroup; - -/** \brief Function type that allocates a kinematics solver for a particular group */ -typedef std::function SolverAllocatorFn; - -/** \brief Map from group instances to allocator functions & bijections */ -using SolverAllocatorMapFn = std::map; - -/** \brief Map of names to instances for JointModelGroup */ -using JointModelGroupMap = std::map; - -/** \brief Map of names to const instances for JointModelGroup */ -using JointModelGroupMapConst = std::map; - -using JointBoundsVector = std::vector; - -class JointModelGroup -{ -public: - struct KinematicsSolver - { - KinematicsSolver() : default_ik_timeout_(0.5) - { - } - - /// Return a flag indicating whether the state of the solver is initialized - operator bool() const - { - return allocator_ && !bijection_.empty() && solver_instance_; - } - - void reset() - { - solver_instance_.reset(); - bijection_.clear(); - } - - /// Function type that allocates a kinematics solver for a particular group - SolverAllocatorFn allocator_; - - /** \brief The mapping between the order of the joints in the group and the order of the joints in the kinematics - solver. - An element bijection[i] at index \e i in this array, maps the variable at index bijection[i] in this group to - the variable at index - i in the kinematic solver. */ - std::vector bijection_; - - kinematics::KinematicsBasePtr solver_instance_; - - double default_ik_timeout_; - }; - - /// Map from group instances to allocator functions & bijections - using KinematicsSolverMap = std::map; - - JointModelGroup(const std::string& name, const srdf::Model::Group& config, - const std::vector& joint_vector, const RobotModel* parent_model); - - ~JointModelGroup(); - - /** \brief Get the kinematic model this group is part of */ - const RobotModel& getParentModel() const - { - return *parent_model_; - } - - /** \brief Get the name of the joint group */ - const std::string& getName() const - { - return name_; - } - - /** \brief get the SRDF configuration this group is based on */ - const srdf::Model::Group& getConfig() const - { - return config_; - } - - /** \brief Check if a joint is part of this group */ - bool hasJointModel(const std::string& joint) const; - - /** \brief Check if a link is part of this group */ - bool hasLinkModel(const std::string& link) const; - - /** \brief Get a joint by its name. Throw an exception if the joint is not part of this group. */ - const JointModel* getJointModel(const std::string& joint) const; - - /** \brief Get a link by its name. Throw an exception if the link is not part of this group. */ - const LinkModel* getLinkModel(const std::string& link) const; - - /** \brief Get all the joints in this group (including fixed and mimic joints). */ - const std::vector& getJointModels() const - { - return joint_model_vector_; - } - - /** \brief Get the names of the joints in this group. These are the names of the joints returned by getJointModels(). - */ - const std::vector& getJointModelNames() const - { - return joint_model_name_vector_; - } - - /** \brief Get the active joints in this group (that have controllable DOF). This does not include mimic joints. */ - const std::vector& getActiveJointModels() const - { - return active_joint_model_vector_; - } - - /** \brief Get the names of the active joints in this group. These are the names of the joints returned by - * getJointModels(). */ - const std::vector& getActiveJointModelNames() const - { - return active_joint_model_name_vector_; - } - - /** \brief Get the fixed joints that are part of this group */ - const std::vector& getFixedJointModels() const - { - return fixed_joints_; - } - - /** \brief Get the mimic joints that are part of this group */ - const std::vector& getMimicJointModels() const - { - return mimic_joints_; - } - - /** \brief Get the array of continuous joints used in this group (may include mimic joints). */ - const std::vector& getContinuousJointModels() const - { - return continuous_joint_model_vector_; - } - - /** \brief Get the names of the variables that make up the joints included in this group. The number of - returned elements is always equal to getVariableCount(). This includes mimic joints. */ - const std::vector& getVariableNames() const - { - return variable_names_; - } - - /** \brief Unlike a complete kinematic model, a group may - contain disconnected parts of the kinematic tree -- a - set of smaller trees. This function gives the roots of - those smaller trees. Furthermore, it is ensured that - the roots are on different branches in the kinematic - tree. This means that in following any root in the given - list, none of the other returned roots will be encountered. */ - const std::vector& getJointRoots() const - { - return joint_roots_; - } - - /** \brief Get the common root of all joint roots; not necessarily part of this group */ - const JointModel* getCommonRoot() const - { - return common_root_; - } - - /** \brief Get the links that are part of this joint group */ - const std::vector& getLinkModels() const - { - return link_model_vector_; - } - - /** \brief Get the names of the links that are part of this joint group */ - const std::vector& getLinkModelNames() const - { - return link_model_name_vector_; - } - - /** \brief Get the names of the links that are part of this joint group and also have geometry associated with them */ - const std::vector& getLinkModelNamesWithCollisionGeometry() const - { - return link_model_with_geometry_name_vector_; - } - - /** \brief Get the names of the links that are to be updated when the state of this group changes. This - includes links that are in the kinematic model but outside this group, if those links are descendants of - joints in this group that have their values updated. The order is the correct order for updating the corresponding - states. */ - const std::vector& getUpdatedLinkModels() const - { - return updated_link_model_vector_; - } - - /** \brief Return the same data as getUpdatedLinkModels() but as a set */ - const std::set& getUpdatedLinkModelsSet() const - { - return updated_link_model_set_; - } - - /** \brief Get the names of the links returned by getUpdatedLinkModels() */ - const std::vector& getUpdatedLinkModelNames() const - { - return updated_link_model_name_vector_; - } - - /** \brief Get the names of the links that are to be updated when the state of this group changes. This - includes links that are in the kinematic model but outside this group, if those links are descendants of - joints in this group that have their values updated. */ - const std::vector& getUpdatedLinkModelsWithGeometry() const - { - return updated_link_model_with_geometry_vector_; - } - - /** \brief Return the same data as getUpdatedLinkModelsWithGeometry() but as a set */ - const std::set& getUpdatedLinkModelsWithGeometrySet() const - { - return updated_link_model_with_geometry_set_; - } - - /** \brief Get the names of the links returned by getUpdatedLinkModels() */ - const std::vector& getUpdatedLinkModelsWithGeometryNames() const - { - return updated_link_model_with_geometry_name_vector_; - } - - /** \brief Get the names of the links returned by getUpdatedLinkModels() */ - const std::set& getUpdatedLinkModelsWithGeometryNamesSet() const - { - return updated_link_model_with_geometry_name_set_; - } - - /** \brief True if this name is in the set of links that are to be updated when the state of this group changes. This - includes links that are in the kinematic model but outside this group, if those links are descendants of - joints in this group that have their values updated. */ - bool isLinkUpdated(const std::string& name) const - { - return updated_link_model_name_set_.find(name) != updated_link_model_name_set_.end(); - } - - /** \brief Get the index locations in the complete robot state for all the variables in this group */ - const std::vector& getVariableIndexList() const - { - return variable_index_list_; - } - - /** \brief Get the index of a variable within the group. Return -1 on error. */ - int getVariableGroupIndex(const std::string& variable) const; - - /** \brief Get the names of the known default states (as specified in the SRDF) */ - const std::vector& getDefaultStateNames() const - { - return default_states_names_; - } - - void addDefaultState(const std::string& name, const std::map& default_state); - - /** \brief Get the values that correspond to a named state as read from the URDF. Return false on failure. */ - bool getVariableDefaultPositions(const std::string& name, std::map& values) const; - - /** \brief Compute the default values for the joint group */ - void getVariableDefaultPositions(std::map& values) const; - - /** \brief Compute the default values for the joint group */ - void getVariableDefaultPositions(std::vector& values) const - { - values.resize(variable_count_); - getVariableDefaultPositions(&values[0]); - } - - /** \brief Compute the default values for the joint group */ - void getVariableDefaultPositions(double* values) const; - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const - { - getVariableRandomPositions(rng, values, active_joint_models_bounds_); - } - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const - { - values.resize(variable_count_); - getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_); - } - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, - const double distance) const - { - getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance); - } - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, - const std::vector& near, double distance) const - { - values.resize(variable_count_); - getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance); - } - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, - const std::vector& near, - const std::map& distance_map) const - { - values.resize(variable_count_); - getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance_map); - } - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, - const std::vector& distances) const - { - getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distances); - } - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, - const std::vector& near, const std::vector& distances) const - { - values.resize(variable_count_); - getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distances); - } - - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const JointBoundsVector& active_joint_bounds) const; - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const JointBoundsVector& active_joint_bounds, const double* near, - const double distance) const; - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const JointBoundsVector& active_joint_bounds, const double* near, - const std::map& distance_map) const; - - /** \brief Compute random values for the state of the joint group */ - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const JointBoundsVector& active_joint_bounds, const double* near, - const std::vector& distances) const; - - bool enforcePositionBounds(double* state) const - { - return enforcePositionBounds(state, active_joint_models_bounds_); - } - - bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const; - bool satisfiesPositionBounds(const double* state, double margin = 0.0) const - { - return satisfiesPositionBounds(state, active_joint_models_bounds_, margin); - } - bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds, - double margin = 0.0) const; - - double getMaximumExtent() const - { - return getMaximumExtent(active_joint_models_bounds_); - } - double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const; - - double distance(const double* state1, const double* state2) const; - void interpolate(const double* from, const double* to, double t, double* state) const; - - /** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic - joints, so will always be >= getActiveVariableCount() */ - unsigned int getVariableCount() const - { - return variable_count_; - } - - /** \brief Get the number of variables that describe the active joints in this joint group. This excludes variables - necessary for mimic joints. */ - unsigned int getActiveVariableCount() const - { - return active_variable_count_; - } - - /** \brief Set the names of the subgroups for this group */ - void setSubgroupNames(const std::vector& subgroups); - - /** \brief Get the names of the groups that are subsets of this one (in terms of joints set) */ - const std::vector& getSubgroupNames() const - { - return subgroup_names_; - } - - /** \brief Get the groups that are subsets of this one (in terms of joints set) */ - void getSubgroups(std::vector& sub_groups) const; - - /** \brief Check if the joints of group \e group are a subset of the joints in this group */ - bool isSubgroup(const std::string& group) const - { - return subgroup_names_set_.find(group) != subgroup_names_set_.end(); - } - - /** \brief Check if this group is a linear chain */ - bool isChain() const - { - return is_chain_; - } - - /** \brief Return true if the group consists only of joints that are single DOF */ - bool isSingleDOFJoints() const - { - return is_single_dof_; - } - - /** \brief Check if this group was designated as an end-effector in the SRDF */ - bool isEndEffector() const - { - return !end_effector_name_.empty(); - } - - bool isContiguousWithinState() const - { - return is_contiguous_index_list_; - } - - /** \brief Return the name of the end effector, if this group is an end-effector */ - const std::string& getEndEffectorName() const - { - return end_effector_name_; - } - - /** \brief Set the name of the end-effector, and remember this group is indeed an end-effector. */ - void setEndEffectorName(const std::string& name); - - /** \brief If this group is an end-effector, specify the parent group (e.g., the arm holding the eef) and the link the - end - effector connects to */ - void setEndEffectorParent(const std::string& group, const std::string& link); - - /** \brief Notify this group that there is an end-effector attached to it */ - void attachEndEffector(const std::string& eef_name); - - /** \brief Get the name of the group this end-effector attaches to (first) and the name of the link in that group - * (second) */ - const std::pair& getEndEffectorParentGroup() const - { - return end_effector_parent_; - } - - /** \brief Get the names of the end effectors attached to this group */ - const std::vector& getAttachedEndEffectorNames() const - { - return attached_end_effector_names_; - } - - /** - * \brief Get the unique set of end effector tips included in a particular joint model group - * as defined by the SRDF end effector elements - * e.g. for a humanoid robot this would return 4 tips for the hands and feet - * \param tips - the output vector of link models of the tips - * \return true on success - */ - bool getEndEffectorTips(std::vector& tips) const; - - /** - * \brief Get the unique set of end effector tips included in a particular joint model group - * as defined by the SRDF end effector elements - * e.g. for a humanoid robot this would return 4 tips for the hands and feet - * \param tips - the output vector of link names of the tips - * \return true on success - */ - bool getEndEffectorTips(std::vector& tips) const; - - /** - * \brief Get one end effector tip, throwing an error if there ends up being more in the joint model group - * This is a useful helper function because most planning groups (almost all) only have one tip - * \return pointer to LinkModel, or nullptr on failure - */ - const moveit::core::LinkModel* getOnlyOneEndEffectorTip() const; - - /** \brief Get the bounds for all the active joints */ - const JointBoundsVector& getActiveJointModelsBounds() const - { - return active_joint_models_bounds_; - } - - const std::pair& getGroupKinematics() const - { - return group_kinematics_; - } - - void setSolverAllocators(const SolverAllocatorFn& solver, - const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn()) - { - setSolverAllocators(std::make_pair(solver, solver_map)); - } - - void setSolverAllocators(const std::pair& solvers); - - const kinematics::KinematicsBaseConstPtr getSolverInstance() const - { - return group_kinematics_.first.solver_instance_; - } - - const kinematics::KinematicsBasePtr& getSolverInstance() - { - return group_kinematics_.first.solver_instance_; - } - - bool canSetStateFromIK(const std::string& tip) const; - - bool setRedundantJoints(const std::vector& joints) - { - if (group_kinematics_.first.solver_instance_) - return group_kinematics_.first.solver_instance_->setRedundantJoints(joints); - return false; - } - - /** \brief Get the default IK timeout */ - double getDefaultIKTimeout() const - { - return group_kinematics_.first.default_ik_timeout_; - } - - /** \brief Set the default IK timeout */ - void setDefaultIKTimeout(double ik_timeout); - - /** \brief Return the mapping between the order of the joints in this group and the order of the joints in the - kinematics solver. - An element bijection[i] at index \e i in this array, maps the variable at index bijection[i] in this group to - the variable at index i in the kinematic solver. */ - const std::vector& getKinematicsSolverJointBijection() const - { - return group_kinematics_.first.bijection_; - } - - /** \brief Print information about the constructed model */ - void printGroupInfo(std::ostream& out = std::cout) const; - - /** \brief Check that the time to move between two waypoints is sufficient given velocity limits */ - bool isValidVelocityMove(const std::vector& from_joint_pose, const std::vector& to_joint_pose, - double dt) const; - - /** \brief Check that the time to move between two waypoints is sufficient given velocity limits */ - bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size, - double dt) const; - - /** \brief Computes the indices of joint variables given a vector of joint names to look up */ - bool computeJointVariableIndices(const std::vector& joint_names, - std::vector& joint_bijection) const; - - /** - * @brief Get the lower and upper position limits of all active variables in the group. - * - * @return std::pair Containing the lower and upper joint limits for all active variables. - */ - [[nodiscard]] std::pair getLowerAndUpperLimits() const; - - /** - * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains - * only single-variable joints, - * @details In case of asymmetric velocity or acceleration limits, this function will return the most limiting component. - * @return std::pair Containing the velocity and acceleration limits - */ - [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds() const; - -protected: - /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates - mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, - there are no values to be read (\e values is only the group state) */ - void updateMimicJoints(double* values) const; - - /** \brief Owner model */ - const RobotModel* parent_model_; - - /** \brief Name of group */ - std::string name_; - - /** \brief Joint instances in the order they appear in the group state */ - std::vector joint_model_vector_; - - /** \brief Names of joints in the order they appear in the group state */ - std::vector joint_model_name_vector_; - - /** \brief Active joint instances in the order they appear in the group state */ - std::vector active_joint_model_vector_; - - /** \brief Names of active joints in the order they appear in the group state */ - std::vector active_joint_model_name_vector_; - - /** \brief The joints that have no DOF (fixed) */ - std::vector fixed_joints_; - - /** \brief Joints that mimic other joints */ - std::vector mimic_joints_; - - /** \brief The set of continuous joints this group contains */ - std::vector continuous_joint_model_vector_; - - /** \brief The names of the DOF that make up this group (this is just a sequence of joint variable names; not - necessarily joint names!) */ - std::vector variable_names_; - - /** \brief The names of the DOF that make up this group (this is just a sequence of joint variable names; not - necessarily joint names!) */ - std::set variable_names_set_; - - /** \brief A map from joint names to their instances. This includes all joints in the group. */ - JointModelMapConst joint_model_map_; - - /** \brief The list of active joint models that are roots in this group */ - std::vector joint_roots_; - - /** \brief The joint that is a common root for all joints in this group (not necessarily part of this group) */ - const JointModel* common_root_; - - /** \brief The group includes all the joint variables that make up the joints the group consists of. - This map gives the position in the state vector of the group for each of these variables. - Additionally, it includes the names of the joints and the index for the first variable of that joint. */ - VariableIndexMap joint_variables_index_map_; - - /** \brief The bounds for all the active joint models */ - JointBoundsVector active_joint_models_bounds_; - - /** \brief The list of index values this group includes, with respect to a full robot state; this includes mimic - * joints. */ - std::vector variable_index_list_; - - /** \brief For each active joint model in this group, hold the index at which the corresponding joint state starts in - the group state */ - std::vector active_joint_model_start_index_; - - /** \brief The links that are on the direct lineage between joints - and joint_roots_, as well as the children of the joint leafs. - May not be in any particular order */ - std::vector link_model_vector_; - - /** \brief A map from link names to their instances */ - LinkModelMapConst link_model_map_; - - /** \brief The names of the links in this group */ - std::vector link_model_name_vector_; - - std::vector link_model_with_geometry_vector_; - - /** \brief The names of the links in this group that also have geometry */ - std::vector link_model_with_geometry_name_vector_; - - /** \brief The list of downstream link models in the order they should be updated (may include links that are not in - * this group) */ - std::vector updated_link_model_vector_; - - /** \brief The list of downstream link models in the order they should be updated (may include links that are not in - * this group) */ - std::set updated_link_model_set_; - - /** \brief The list of downstream link names in the order they should be updated (may include links that are not in - * this group) */ - std::vector updated_link_model_name_vector_; - - /** \brief The list of downstream link names in the order they should be updated (may include links that are not in - * this group) */ - std::set updated_link_model_name_set_; - - /** \brief The list of downstream link models in the order they should be updated (may include links that are not in - * this group) */ - std::vector updated_link_model_with_geometry_vector_; - - /** \brief The list of downstream link models in the order they should be updated (may include links that are not in - * this group) */ - std::set updated_link_model_with_geometry_set_; - - /** \brief The list of downstream link names in the order they should be updated (may include links that are not in - * this group) */ - std::vector updated_link_model_with_geometry_name_vector_; - - /** \brief The list of downstream link names in the order they should be updated (may include links that are not in - * this group) */ - std::set updated_link_model_with_geometry_name_set_; - - /** \brief The number of variables necessary to describe this group of joints */ - unsigned int variable_count_; - - /** \brief The number of variables necessary to describe the active joints in this group of joints */ - unsigned int active_variable_count_; - - /** \brief True if the state of this group is contiguous within the full robot state; this also means that - the index values in variable_index_list_ are consecutive integers */ - bool is_contiguous_index_list_; - - /** \brief The set of labelled subgroups that are included in this group */ - std::vector subgroup_names_; - - /** \brief The set of labelled subgroups that are included in this group */ - std::set subgroup_names_set_; - - /** \brief If an end-effector is attached to this group, the name of that end-effector is stored in this variable */ - std::vector attached_end_effector_names_; - - /** \brief First: name of the group that is parent to this end-effector group; Second: the link this in the parent - group - that this group attaches to */ - std::pair end_effector_parent_; - - /** \brief The name of the end effector, if this group is an end-effector */ - std::string end_effector_name_; - - bool is_chain_; - - bool is_single_dof_; - - struct GroupMimicUpdate - { - GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o) - { - } - int src; - int dest; - double factor; - double offset; - }; - - std::vector group_mimic_update_; - - std::pair group_kinematics_; - - srdf::Model::Group config_; - - /** \brief The set of default states specified for this group in the SRDF */ - std::map > default_states_; - - /** \brief The names of the default states specified for this group in the SRDF */ - std::vector default_states_names_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp new file mode 100644 index 0000000000..54299304c1 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp @@ -0,0 +1,773 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, 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, Inc. 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +class RobotModel; +class JointModelGroup; + +/** \brief Function type that allocates a kinematics solver for a particular group */ +typedef std::function SolverAllocatorFn; + +/** \brief Map from group instances to allocator functions & bijections */ +using SolverAllocatorMapFn = std::map; + +/** \brief Map of names to instances for JointModelGroup */ +using JointModelGroupMap = std::map; + +/** \brief Map of names to const instances for JointModelGroup */ +using JointModelGroupMapConst = std::map; + +using JointBoundsVector = std::vector; + +class JointModelGroup +{ +public: + struct KinematicsSolver + { + KinematicsSolver() : default_ik_timeout_(0.5) + { + } + + /// Return a flag indicating whether the state of the solver is initialized + operator bool() const + { + return allocator_ && !bijection_.empty() && solver_instance_; + } + + void reset() + { + solver_instance_.reset(); + bijection_.clear(); + } + + /// Function type that allocates a kinematics solver for a particular group + SolverAllocatorFn allocator_; + + /** \brief The mapping between the order of the joints in the group and the order of the joints in the kinematics + solver. + An element bijection[i] at index \e i in this array, maps the variable at index bijection[i] in this group to + the variable at index + i in the kinematic solver. */ + std::vector bijection_; + + kinematics::KinematicsBasePtr solver_instance_; + + double default_ik_timeout_; + }; + + /// Map from group instances to allocator functions & bijections + using KinematicsSolverMap = std::map; + + JointModelGroup(const std::string& name, const srdf::Model::Group& config, + const std::vector& joint_vector, const RobotModel* parent_model); + + ~JointModelGroup(); + + /** \brief Get the kinematic model this group is part of */ + const RobotModel& getParentModel() const + { + return *parent_model_; + } + + /** \brief Get the name of the joint group */ + const std::string& getName() const + { + return name_; + } + + /** \brief get the SRDF configuration this group is based on */ + const srdf::Model::Group& getConfig() const + { + return config_; + } + + /** \brief Check if a joint is part of this group */ + bool hasJointModel(const std::string& joint) const; + + /** \brief Check if a link is part of this group */ + bool hasLinkModel(const std::string& link) const; + + /** \brief Get a joint by its name. Throw an exception if the joint is not part of this group. */ + const JointModel* getJointModel(const std::string& joint) const; + + /** \brief Get a link by its name. Throw an exception if the link is not part of this group. */ + const LinkModel* getLinkModel(const std::string& link) const; + + /** \brief Get all the joints in this group (including fixed and mimic joints). */ + const std::vector& getJointModels() const + { + return joint_model_vector_; + } + + /** \brief Get the names of the joints in this group. These are the names of the joints returned by getJointModels(). + */ + const std::vector& getJointModelNames() const + { + return joint_model_name_vector_; + } + + /** \brief Get the active joints in this group (that have controllable DOF). This does not include mimic joints. */ + const std::vector& getActiveJointModels() const + { + return active_joint_model_vector_; + } + + /** \brief Get the names of the active joints in this group. These are the names of the joints returned by + * getJointModels(). */ + const std::vector& getActiveJointModelNames() const + { + return active_joint_model_name_vector_; + } + + /** \brief Get the fixed joints that are part of this group */ + const std::vector& getFixedJointModels() const + { + return fixed_joints_; + } + + /** \brief Get the mimic joints that are part of this group */ + const std::vector& getMimicJointModels() const + { + return mimic_joints_; + } + + /** \brief Get the array of continuous joints used in this group (may include mimic joints). */ + const std::vector& getContinuousJointModels() const + { + return continuous_joint_model_vector_; + } + + /** \brief Get the names of the variables that make up the joints included in this group. The number of + returned elements is always equal to getVariableCount(). This includes mimic joints. */ + const std::vector& getVariableNames() const + { + return variable_names_; + } + + /** \brief Unlike a complete kinematic model, a group may + contain disconnected parts of the kinematic tree -- a + set of smaller trees. This function gives the roots of + those smaller trees. Furthermore, it is ensured that + the roots are on different branches in the kinematic + tree. This means that in following any root in the given + list, none of the other returned roots will be encountered. */ + const std::vector& getJointRoots() const + { + return joint_roots_; + } + + /** \brief Get the common root of all joint roots; not necessarily part of this group */ + const JointModel* getCommonRoot() const + { + return common_root_; + } + + /** \brief Get the links that are part of this joint group */ + const std::vector& getLinkModels() const + { + return link_model_vector_; + } + + /** \brief Get the names of the links that are part of this joint group */ + const std::vector& getLinkModelNames() const + { + return link_model_name_vector_; + } + + /** \brief Get the names of the links that are part of this joint group and also have geometry associated with them */ + const std::vector& getLinkModelNamesWithCollisionGeometry() const + { + return link_model_with_geometry_name_vector_; + } + + /** \brief Get the names of the links that are to be updated when the state of this group changes. This + includes links that are in the kinematic model but outside this group, if those links are descendants of + joints in this group that have their values updated. The order is the correct order for updating the corresponding + states. */ + const std::vector& getUpdatedLinkModels() const + { + return updated_link_model_vector_; + } + + /** \brief Return the same data as getUpdatedLinkModels() but as a set */ + const std::set& getUpdatedLinkModelsSet() const + { + return updated_link_model_set_; + } + + /** \brief Get the names of the links returned by getUpdatedLinkModels() */ + const std::vector& getUpdatedLinkModelNames() const + { + return updated_link_model_name_vector_; + } + + /** \brief Get the names of the links that are to be updated when the state of this group changes. This + includes links that are in the kinematic model but outside this group, if those links are descendants of + joints in this group that have their values updated. */ + const std::vector& getUpdatedLinkModelsWithGeometry() const + { + return updated_link_model_with_geometry_vector_; + } + + /** \brief Return the same data as getUpdatedLinkModelsWithGeometry() but as a set */ + const std::set& getUpdatedLinkModelsWithGeometrySet() const + { + return updated_link_model_with_geometry_set_; + } + + /** \brief Get the names of the links returned by getUpdatedLinkModels() */ + const std::vector& getUpdatedLinkModelsWithGeometryNames() const + { + return updated_link_model_with_geometry_name_vector_; + } + + /** \brief Get the names of the links returned by getUpdatedLinkModels() */ + const std::set& getUpdatedLinkModelsWithGeometryNamesSet() const + { + return updated_link_model_with_geometry_name_set_; + } + + /** \brief True if this name is in the set of links that are to be updated when the state of this group changes. This + includes links that are in the kinematic model but outside this group, if those links are descendants of + joints in this group that have their values updated. */ + bool isLinkUpdated(const std::string& name) const + { + return updated_link_model_name_set_.find(name) != updated_link_model_name_set_.end(); + } + + /** \brief Get the index locations in the complete robot state for all the variables in this group */ + const std::vector& getVariableIndexList() const + { + return variable_index_list_; + } + + /** \brief Get the index of a variable within the group. Return -1 on error. */ + int getVariableGroupIndex(const std::string& variable) const; + + /** \brief Get the names of the known default states (as specified in the SRDF) */ + const std::vector& getDefaultStateNames() const + { + return default_states_names_; + } + + void addDefaultState(const std::string& name, const std::map& default_state); + + /** \brief Get the values that correspond to a named state as read from the URDF. Return false on failure. */ + bool getVariableDefaultPositions(const std::string& name, std::map& values) const; + + /** \brief Compute the default values for the joint group */ + void getVariableDefaultPositions(std::map& values) const; + + /** \brief Compute the default values for the joint group */ + void getVariableDefaultPositions(std::vector& values) const + { + values.resize(variable_count_); + getVariableDefaultPositions(&values[0]); + } + + /** \brief Compute the default values for the joint group */ + void getVariableDefaultPositions(double* values) const; + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const + { + getVariableRandomPositions(rng, values, active_joint_models_bounds_); + } + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const + { + values.resize(variable_count_); + getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_); + } + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, + const double distance) const + { + getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance); + } + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, + const std::vector& near, double distance) const + { + values.resize(variable_count_); + getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance); + } + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, + const std::vector& near, + const std::map& distance_map) const + { + values.resize(variable_count_); + getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance_map); + } + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, + const std::vector& distances) const + { + getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distances); + } + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, + const std::vector& near, const std::vector& distances) const + { + values.resize(variable_count_); + getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distances); + } + + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const JointBoundsVector& active_joint_bounds) const; + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const JointBoundsVector& active_joint_bounds, const double* near, + const double distance) const; + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const JointBoundsVector& active_joint_bounds, const double* near, + const std::map& distance_map) const; + + /** \brief Compute random values for the state of the joint group */ + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const JointBoundsVector& active_joint_bounds, const double* near, + const std::vector& distances) const; + + bool enforcePositionBounds(double* state) const + { + return enforcePositionBounds(state, active_joint_models_bounds_); + } + + bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const; + bool satisfiesPositionBounds(const double* state, double margin = 0.0) const + { + return satisfiesPositionBounds(state, active_joint_models_bounds_, margin); + } + bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds, + double margin = 0.0) const; + + double getMaximumExtent() const + { + return getMaximumExtent(active_joint_models_bounds_); + } + double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const; + + double distance(const double* state1, const double* state2) const; + void interpolate(const double* from, const double* to, double t, double* state) const; + + /** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic + joints, so will always be >= getActiveVariableCount() */ + unsigned int getVariableCount() const + { + return variable_count_; + } + + /** \brief Get the number of variables that describe the active joints in this joint group. This excludes variables + necessary for mimic joints. */ + unsigned int getActiveVariableCount() const + { + return active_variable_count_; + } + + /** \brief Set the names of the subgroups for this group */ + void setSubgroupNames(const std::vector& subgroups); + + /** \brief Get the names of the groups that are subsets of this one (in terms of joints set) */ + const std::vector& getSubgroupNames() const + { + return subgroup_names_; + } + + /** \brief Get the groups that are subsets of this one (in terms of joints set) */ + void getSubgroups(std::vector& sub_groups) const; + + /** \brief Check if the joints of group \e group are a subset of the joints in this group */ + bool isSubgroup(const std::string& group) const + { + return subgroup_names_set_.find(group) != subgroup_names_set_.end(); + } + + /** \brief Check if this group is a linear chain */ + bool isChain() const + { + return is_chain_; + } + + /** \brief Return true if the group consists only of joints that are single DOF */ + bool isSingleDOFJoints() const + { + return is_single_dof_; + } + + /** \brief Check if this group was designated as an end-effector in the SRDF */ + bool isEndEffector() const + { + return !end_effector_name_.empty(); + } + + bool isContiguousWithinState() const + { + return is_contiguous_index_list_; + } + + /** \brief Return the name of the end effector, if this group is an end-effector */ + const std::string& getEndEffectorName() const + { + return end_effector_name_; + } + + /** \brief Set the name of the end-effector, and remember this group is indeed an end-effector. */ + void setEndEffectorName(const std::string& name); + + /** \brief If this group is an end-effector, specify the parent group (e.g., the arm holding the eef) and the link the + end + effector connects to */ + void setEndEffectorParent(const std::string& group, const std::string& link); + + /** \brief Notify this group that there is an end-effector attached to it */ + void attachEndEffector(const std::string& eef_name); + + /** \brief Get the name of the group this end-effector attaches to (first) and the name of the link in that group + * (second) */ + const std::pair& getEndEffectorParentGroup() const + { + return end_effector_parent_; + } + + /** \brief Get the names of the end effectors attached to this group */ + const std::vector& getAttachedEndEffectorNames() const + { + return attached_end_effector_names_; + } + + /** + * \brief Get the unique set of end effector tips included in a particular joint model group + * as defined by the SRDF end effector elements + * e.g. for a humanoid robot this would return 4 tips for the hands and feet + * \param tips - the output vector of link models of the tips + * \return true on success + */ + bool getEndEffectorTips(std::vector& tips) const; + + /** + * \brief Get the unique set of end effector tips included in a particular joint model group + * as defined by the SRDF end effector elements + * e.g. for a humanoid robot this would return 4 tips for the hands and feet + * \param tips - the output vector of link names of the tips + * \return true on success + */ + bool getEndEffectorTips(std::vector& tips) const; + + /** + * \brief Get one end effector tip, throwing an error if there ends up being more in the joint model group + * This is a useful helper function because most planning groups (almost all) only have one tip + * \return pointer to LinkModel, or nullptr on failure + */ + const moveit::core::LinkModel* getOnlyOneEndEffectorTip() const; + + /** \brief Get the bounds for all the active joints */ + const JointBoundsVector& getActiveJointModelsBounds() const + { + return active_joint_models_bounds_; + } + + const std::pair& getGroupKinematics() const + { + return group_kinematics_; + } + + void setSolverAllocators(const SolverAllocatorFn& solver, + const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn()) + { + setSolverAllocators(std::make_pair(solver, solver_map)); + } + + void setSolverAllocators(const std::pair& solvers); + + const kinematics::KinematicsBaseConstPtr getSolverInstance() const + { + return group_kinematics_.first.solver_instance_; + } + + const kinematics::KinematicsBasePtr& getSolverInstance() + { + return group_kinematics_.first.solver_instance_; + } + + bool canSetStateFromIK(const std::string& tip) const; + + bool setRedundantJoints(const std::vector& joints) + { + if (group_kinematics_.first.solver_instance_) + return group_kinematics_.first.solver_instance_->setRedundantJoints(joints); + return false; + } + + /** \brief Get the default IK timeout */ + double getDefaultIKTimeout() const + { + return group_kinematics_.first.default_ik_timeout_; + } + + /** \brief Set the default IK timeout */ + void setDefaultIKTimeout(double ik_timeout); + + /** \brief Return the mapping between the order of the joints in this group and the order of the joints in the + kinematics solver. + An element bijection[i] at index \e i in this array, maps the variable at index bijection[i] in this group to + the variable at index i in the kinematic solver. */ + const std::vector& getKinematicsSolverJointBijection() const + { + return group_kinematics_.first.bijection_; + } + + /** \brief Print information about the constructed model */ + void printGroupInfo(std::ostream& out = std::cout) const; + + /** \brief Check that the time to move between two waypoints is sufficient given velocity limits */ + bool isValidVelocityMove(const std::vector& from_joint_pose, const std::vector& to_joint_pose, + double dt) const; + + /** \brief Check that the time to move between two waypoints is sufficient given velocity limits */ + bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size, + double dt) const; + + /** \brief Computes the indices of joint variables given a vector of joint names to look up */ + bool computeJointVariableIndices(const std::vector& joint_names, + std::vector& joint_bijection) const; + + /** + * @brief Get the lower and upper position limits of all active variables in the group. + * + * @return std::pair Containing the lower and upper joint limits for all active variables. + */ + [[nodiscard]] std::pair getLowerAndUpperLimits() const; + + /** + * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains + * only single-variable joints, + * @details In case of asymmetric velocity or acceleration limits, this function will return the most limiting component. + * @return std::pair Containing the velocity and acceleration limits + */ + [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds() const; + +protected: + /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates + mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, + there are no values to be read (\e values is only the group state) */ + void updateMimicJoints(double* values) const; + + /** \brief Owner model */ + const RobotModel* parent_model_; + + /** \brief Name of group */ + std::string name_; + + /** \brief Joint instances in the order they appear in the group state */ + std::vector joint_model_vector_; + + /** \brief Names of joints in the order they appear in the group state */ + std::vector joint_model_name_vector_; + + /** \brief Active joint instances in the order they appear in the group state */ + std::vector active_joint_model_vector_; + + /** \brief Names of active joints in the order they appear in the group state */ + std::vector active_joint_model_name_vector_; + + /** \brief The joints that have no DOF (fixed) */ + std::vector fixed_joints_; + + /** \brief Joints that mimic other joints */ + std::vector mimic_joints_; + + /** \brief The set of continuous joints this group contains */ + std::vector continuous_joint_model_vector_; + + /** \brief The names of the DOF that make up this group (this is just a sequence of joint variable names; not + necessarily joint names!) */ + std::vector variable_names_; + + /** \brief The names of the DOF that make up this group (this is just a sequence of joint variable names; not + necessarily joint names!) */ + std::set variable_names_set_; + + /** \brief A map from joint names to their instances. This includes all joints in the group. */ + JointModelMapConst joint_model_map_; + + /** \brief The list of active joint models that are roots in this group */ + std::vector joint_roots_; + + /** \brief The joint that is a common root for all joints in this group (not necessarily part of this group) */ + const JointModel* common_root_; + + /** \brief The group includes all the joint variables that make up the joints the group consists of. + This map gives the position in the state vector of the group for each of these variables. + Additionally, it includes the names of the joints and the index for the first variable of that joint. */ + VariableIndexMap joint_variables_index_map_; + + /** \brief The bounds for all the active joint models */ + JointBoundsVector active_joint_models_bounds_; + + /** \brief The list of index values this group includes, with respect to a full robot state; this includes mimic + * joints. */ + std::vector variable_index_list_; + + /** \brief For each active joint model in this group, hold the index at which the corresponding joint state starts in + the group state */ + std::vector active_joint_model_start_index_; + + /** \brief The links that are on the direct lineage between joints + and joint_roots_, as well as the children of the joint leafs. + May not be in any particular order */ + std::vector link_model_vector_; + + /** \brief A map from link names to their instances */ + LinkModelMapConst link_model_map_; + + /** \brief The names of the links in this group */ + std::vector link_model_name_vector_; + + std::vector link_model_with_geometry_vector_; + + /** \brief The names of the links in this group that also have geometry */ + std::vector link_model_with_geometry_name_vector_; + + /** \brief The list of downstream link models in the order they should be updated (may include links that are not in + * this group) */ + std::vector updated_link_model_vector_; + + /** \brief The list of downstream link models in the order they should be updated (may include links that are not in + * this group) */ + std::set updated_link_model_set_; + + /** \brief The list of downstream link names in the order they should be updated (may include links that are not in + * this group) */ + std::vector updated_link_model_name_vector_; + + /** \brief The list of downstream link names in the order they should be updated (may include links that are not in + * this group) */ + std::set updated_link_model_name_set_; + + /** \brief The list of downstream link models in the order they should be updated (may include links that are not in + * this group) */ + std::vector updated_link_model_with_geometry_vector_; + + /** \brief The list of downstream link models in the order they should be updated (may include links that are not in + * this group) */ + std::set updated_link_model_with_geometry_set_; + + /** \brief The list of downstream link names in the order they should be updated (may include links that are not in + * this group) */ + std::vector updated_link_model_with_geometry_name_vector_; + + /** \brief The list of downstream link names in the order they should be updated (may include links that are not in + * this group) */ + std::set updated_link_model_with_geometry_name_set_; + + /** \brief The number of variables necessary to describe this group of joints */ + unsigned int variable_count_; + + /** \brief The number of variables necessary to describe the active joints in this group of joints */ + unsigned int active_variable_count_; + + /** \brief True if the state of this group is contiguous within the full robot state; this also means that + the index values in variable_index_list_ are consecutive integers */ + bool is_contiguous_index_list_; + + /** \brief The set of labelled subgroups that are included in this group */ + std::vector subgroup_names_; + + /** \brief The set of labelled subgroups that are included in this group */ + std::set subgroup_names_set_; + + /** \brief If an end-effector is attached to this group, the name of that end-effector is stored in this variable */ + std::vector attached_end_effector_names_; + + /** \brief First: name of the group that is parent to this end-effector group; Second: the link this in the parent + group + that this group attaches to */ + std::pair end_effector_parent_; + + /** \brief The name of the end effector, if this group is an end-effector */ + std::string end_effector_name_; + + bool is_chain_; + + bool is_single_dof_; + + struct GroupMimicUpdate + { + GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o) + { + } + int src; + int dest; + double factor; + double offset; + }; + + std::vector group_mimic_update_; + + std::pair group_kinematics_; + + srdf::Model::Group config_; + + /** \brief The set of default states specified for this group in the SRDF */ + std::map > default_states_; + + /** \brief The names of the default states specified for this group in the SRDF */ + std::vector default_states_names_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index a9cd17df07..4337b6ba77 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,255 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace shapes -{ -MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc -} - -namespace moveit -{ -namespace core -{ -class JointModel; -class LinkModel; - -/** \brief Map of names to instances for LinkModel */ -typedef std::map LinkModelMap; - -/** \brief Map of names to const instances for LinkModel */ -using LinkModelMapConst = std::map; - -/** \brief Map from link model instances to Eigen transforms */ -using LinkTransformMap = std::map, - Eigen::aligned_allocator > >; - -/** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */ -class LinkModel -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * @brief Construct a link model named \e name - * - * @param[in] name The name of the link - * @param[in] link_index The link index in the RobotModel - */ - LinkModel(const std::string& name, size_t link_index); - ~LinkModel(); - - /** \brief The name of this link */ - const std::string& getName() const - { - return name_; - } - - /** \brief The index of this joint when traversing the kinematic tree in depth first fashion */ - size_t getLinkIndex() const - { - return link_index_; - } - - int getFirstCollisionBodyTransformIndex() const - { - return first_collision_body_transform_index_; - } - - void setFirstCollisionBodyTransformIndex(int index) - { - first_collision_body_transform_index_ = index; - } - - /** \brief Get the joint model whose child this link is. There will always be a parent joint */ - const JointModel* getParentJointModel() const - { - return parent_joint_model_; - } - - void setParentJointModel(const JointModel* joint); - - /** \brief Get the link model whose child this link is (through some joint). There may not always be a parent link - * (nullptr is returned for the root link) */ - const LinkModel* getParentLinkModel() const - { - return parent_link_model_; - } - - void setParentLinkModel(const LinkModel* link) - { - parent_link_model_ = link; - } - - /** \brief A link may have 0 or more child joints. From those joints there will certainly be other descendant links */ - const std::vector& getChildJointModels() const - { - return child_joint_models_; - } - - void addChildJointModel(const JointModel* joint) - { - child_joint_models_.push_back(joint); - } - - /** \brief When transforms are computed for this link, - they are usually applied to the link's origin. The - joint origin transform acts as an offset -- it is - pre-applied before any other transform. The - transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getJointOriginTransform() const - { - return joint_origin_transform_; - } - - bool jointOriginTransformIsIdentity() const - { - return joint_origin_transform_is_identity_; - } - - bool parentJointIsFixed() const - { - return is_parent_joint_fixed_; - } - - void setJointOriginTransform(const Eigen::Isometry3d& transform); - - /** \brief In addition to the link transform, the geometry - of a link that is used for collision checking may have - a different offset itself, with respect to the origin. - The transform is guaranteed to be a valid isometry. */ - const EigenSTL::vector_Isometry3d& getCollisionOriginTransforms() const - { - return collision_origin_transform_; - } - - /** \brief Return flags for each transform specifying whether they are identity or not */ - const std::vector& areCollisionOriginTransformsIdentity() const - { - return collision_origin_transform_is_identity_; - } - - /** \brief Get shape associated to the collision geometry for this link */ - const std::vector& getShapes() const - { - return shapes_; - } - - void setGeometry(const std::vector& shapes, const EigenSTL::vector_Isometry3d& origins); - - /** \brief Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes that make - up the - link, when the link is positioned at origin -- only collision origin transforms are considered) */ - const Eigen::Vector3d& getShapeExtentsAtOrigin() const - { - return shape_extents_; - } - - /** \brief Get the offset of the center of the bounding box of this link when the link is positioned at origin. */ - const Eigen::Vector3d& getCenteredBoundingBoxOffset() const - { - return centered_bounding_box_offset_; - } - - /** \brief Get the set of links that are attached to this one via fixed transforms. The returned transforms are - * guaranteed to be valid isometries. */ - const LinkTransformMap& getAssociatedFixedTransforms() const - { - return associated_fixed_transforms_; - } - - /** \brief Remember that \e link_model is attached to this link using a fixed transform */ - void addAssociatedFixedTransform(const LinkModel* link_model, const Eigen::Isometry3d& transform) - { - ASSERT_ISOMETRY(transform); // unsanitized input, could contain a non-isometry - associated_fixed_transforms_[link_model] = transform; - } - - /** \brief Get the filename of the mesh resource used for visual display of this link */ - const std::string& getVisualMeshFilename() const - { - return visual_mesh_filename_; - } - - /** \brief Get the scale of the mesh resource for this link */ - const Eigen::Vector3d& getVisualMeshScale() const - { - return visual_mesh_scale_; - } - - /** \brief Get the transform for the visual mesh origin */ - const Eigen::Isometry3d& getVisualMeshOrigin() const - { - return visual_mesh_origin_; - } - - void setVisualMesh(const std::string& visual_mesh, const Eigen::Isometry3d& origin, const Eigen::Vector3d& scale); - -private: - /** \brief Name of the link */ - std::string name_; - - /** \brief Index of the transform for this link in the full robot frame */ - size_t link_index_; - - /** \brief JointModel that connects this link to the parent link */ - const JointModel* parent_joint_model_; - - /** \brief The parent link model (nullptr for the root link) */ - const LinkModel* parent_link_model_; - - /** \brief List of directly descending joints (each connects to a child link) */ - std::vector child_joint_models_; - - /** \brief True if the parent joint of this link is fixed */ - bool is_parent_joint_fixed_; - - /** \brief True of the joint origin transform is identity */ - bool joint_origin_transform_is_identity_; - - /** \brief The constant transform applied to the link (local) */ - Eigen::Isometry3d joint_origin_transform_; - - /** \brief The constant transform applied to the collision geometry of the link (local) */ - EigenSTL::vector_Isometry3d collision_origin_transform_; - - /** \brief Flag indicating if the constant transform applied to the collision geometry of the link (local) is - * identity; use int instead of bool to avoid bit operations */ - std::vector collision_origin_transform_is_identity_; - - /** \brief The set of links that are attached to this one via fixed transforms */ - LinkTransformMap associated_fixed_transforms_; - - /** \brief The collision geometry of the link */ - std::vector shapes_; - - /** \brief The extents of shape (dimensions of axis aligned bounding box when shape is at origin). */ - Eigen::Vector3d shape_extents_; - - /** \brief Center of the axis aligned bounding box with size shape_extents_ (zero if symmetric along all axes). */ - Eigen::Vector3d centered_bounding_box_offset_; - - /** \brief Filename associated with the visual geometry mesh of this link. If empty, no mesh was used. */ - std::string visual_mesh_filename_; - - /** \brief The additional origin transform for the mesh */ - Eigen::Isometry3d visual_mesh_origin_; - - /** \brief Scale factor associated with the visual geometry mesh of this link. */ - Eigen::Vector3d visual_mesh_scale_; - - /** \brief Index of the transform for the first shape that makes up the geometry of this link in the full robot state - */ - int first_collision_body_transform_index_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/link_model.hpp new file mode 100644 index 0000000000..323e4dca67 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.hpp @@ -0,0 +1,289 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace shapes +{ +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc +} + +namespace moveit +{ +namespace core +{ +class JointModel; +class LinkModel; + +/** \brief Map of names to instances for LinkModel */ +typedef std::map LinkModelMap; + +/** \brief Map of names to const instances for LinkModel */ +using LinkModelMapConst = std::map; + +/** \brief Map from link model instances to Eigen transforms */ +using LinkTransformMap = std::map, + Eigen::aligned_allocator > >; + +/** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */ +class LinkModel +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief Construct a link model named \e name + * + * @param[in] name The name of the link + * @param[in] link_index The link index in the RobotModel + */ + LinkModel(const std::string& name, size_t link_index); + ~LinkModel(); + + /** \brief The name of this link */ + const std::string& getName() const + { + return name_; + } + + /** \brief The index of this joint when traversing the kinematic tree in depth first fashion */ + size_t getLinkIndex() const + { + return link_index_; + } + + int getFirstCollisionBodyTransformIndex() const + { + return first_collision_body_transform_index_; + } + + void setFirstCollisionBodyTransformIndex(int index) + { + first_collision_body_transform_index_ = index; + } + + /** \brief Get the joint model whose child this link is. There will always be a parent joint */ + const JointModel* getParentJointModel() const + { + return parent_joint_model_; + } + + void setParentJointModel(const JointModel* joint); + + /** \brief Get the link model whose child this link is (through some joint). There may not always be a parent link + * (nullptr is returned for the root link) */ + const LinkModel* getParentLinkModel() const + { + return parent_link_model_; + } + + void setParentLinkModel(const LinkModel* link) + { + parent_link_model_ = link; + } + + /** \brief A link may have 0 or more child joints. From those joints there will certainly be other descendant links */ + const std::vector& getChildJointModels() const + { + return child_joint_models_; + } + + void addChildJointModel(const JointModel* joint) + { + child_joint_models_.push_back(joint); + } + + /** \brief When transforms are computed for this link, + they are usually applied to the link's origin. The + joint origin transform acts as an offset -- it is + pre-applied before any other transform. The + transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getJointOriginTransform() const + { + return joint_origin_transform_; + } + + bool jointOriginTransformIsIdentity() const + { + return joint_origin_transform_is_identity_; + } + + bool parentJointIsFixed() const + { + return is_parent_joint_fixed_; + } + + void setJointOriginTransform(const Eigen::Isometry3d& transform); + + /** \brief In addition to the link transform, the geometry + of a link that is used for collision checking may have + a different offset itself, with respect to the origin. + The transform is guaranteed to be a valid isometry. */ + const EigenSTL::vector_Isometry3d& getCollisionOriginTransforms() const + { + return collision_origin_transform_; + } + + /** \brief Return flags for each transform specifying whether they are identity or not */ + const std::vector& areCollisionOriginTransformsIdentity() const + { + return collision_origin_transform_is_identity_; + } + + /** \brief Get shape associated to the collision geometry for this link */ + const std::vector& getShapes() const + { + return shapes_; + } + + void setGeometry(const std::vector& shapes, const EigenSTL::vector_Isometry3d& origins); + + /** \brief Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes that make + up the + link, when the link is positioned at origin -- only collision origin transforms are considered) */ + const Eigen::Vector3d& getShapeExtentsAtOrigin() const + { + return shape_extents_; + } + + /** \brief Get the offset of the center of the bounding box of this link when the link is positioned at origin. */ + const Eigen::Vector3d& getCenteredBoundingBoxOffset() const + { + return centered_bounding_box_offset_; + } + + /** \brief Get the set of links that are attached to this one via fixed transforms. The returned transforms are + * guaranteed to be valid isometries. */ + const LinkTransformMap& getAssociatedFixedTransforms() const + { + return associated_fixed_transforms_; + } + + /** \brief Remember that \e link_model is attached to this link using a fixed transform */ + void addAssociatedFixedTransform(const LinkModel* link_model, const Eigen::Isometry3d& transform) + { + ASSERT_ISOMETRY(transform); // unsanitized input, could contain a non-isometry + associated_fixed_transforms_[link_model] = transform; + } + + /** \brief Get the filename of the mesh resource used for visual display of this link */ + const std::string& getVisualMeshFilename() const + { + return visual_mesh_filename_; + } + + /** \brief Get the scale of the mesh resource for this link */ + const Eigen::Vector3d& getVisualMeshScale() const + { + return visual_mesh_scale_; + } + + /** \brief Get the transform for the visual mesh origin */ + const Eigen::Isometry3d& getVisualMeshOrigin() const + { + return visual_mesh_origin_; + } + + void setVisualMesh(const std::string& visual_mesh, const Eigen::Isometry3d& origin, const Eigen::Vector3d& scale); + +private: + /** \brief Name of the link */ + std::string name_; + + /** \brief Index of the transform for this link in the full robot frame */ + size_t link_index_; + + /** \brief JointModel that connects this link to the parent link */ + const JointModel* parent_joint_model_; + + /** \brief The parent link model (nullptr for the root link) */ + const LinkModel* parent_link_model_; + + /** \brief List of directly descending joints (each connects to a child link) */ + std::vector child_joint_models_; + + /** \brief True if the parent joint of this link is fixed */ + bool is_parent_joint_fixed_; + + /** \brief True of the joint origin transform is identity */ + bool joint_origin_transform_is_identity_; + + /** \brief The constant transform applied to the link (local) */ + Eigen::Isometry3d joint_origin_transform_; + + /** \brief The constant transform applied to the collision geometry of the link (local) */ + EigenSTL::vector_Isometry3d collision_origin_transform_; + + /** \brief Flag indicating if the constant transform applied to the collision geometry of the link (local) is + * identity; use int instead of bool to avoid bit operations */ + std::vector collision_origin_transform_is_identity_; + + /** \brief The set of links that are attached to this one via fixed transforms */ + LinkTransformMap associated_fixed_transforms_; + + /** \brief The collision geometry of the link */ + std::vector shapes_; + + /** \brief The extents of shape (dimensions of axis aligned bounding box when shape is at origin). */ + Eigen::Vector3d shape_extents_; + + /** \brief Center of the axis aligned bounding box with size shape_extents_ (zero if symmetric along all axes). */ + Eigen::Vector3d centered_bounding_box_offset_; + + /** \brief Filename associated with the visual geometry mesh of this link. If empty, no mesh was used. */ + std::string visual_mesh_filename_; + + /** \brief The additional origin transform for the mesh */ + Eigen::Isometry3d visual_mesh_origin_; + + /** \brief Scale factor associated with the visual geometry mesh of this link. */ + Eigen::Vector3d visual_mesh_scale_; + + /** \brief Index of the transform for the first shape that makes up the geometry of this link in the full robot state + */ + int first_collision_body_transform_index_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index cc95677872..b2c269981b 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,99 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** \brief A planar joint */ -class PlanarJointModel : public JointModel -{ -public: - /** \brief different types of planar joints we support */ - enum MotionModel - { - HOLONOMIC, // default - DIFF_DRIVE - }; - - PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); - - void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds) const override; - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds, const double* near, - const double distance) const override; - bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; - bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; - - void interpolate(const double* from, const double* to, const double t, double* state) const override; - unsigned int getStateSpaceDimension() const override; - double getMaximumExtent(const Bounds& other_bounds) const override; - double distance(const double* values1, const double* values2) const override; - - void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; - void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; - - double getAngularDistanceWeight() const - { - return angular_distance_weight_; - } - - void setAngularDistanceWeight(double weight) - { - angular_distance_weight_ = weight; - } - - double getMinTranslationalDistance() const - { - return min_translational_distance_; - } - - void setMinTranslationalDistance(double min_translational_distance) - { - min_translational_distance_ = min_translational_distance; - } - - MotionModel getMotionModel() const - { - return motion_model_; - } - - void setMotionModel(MotionModel model) - { - motion_model_ = model; - } - - /// Make the yaw component of a state's value vector be in the range [-Pi, Pi]. enforceBounds() also calls this - /// function; - /// Return true if a change is actually made - bool normalizeRotation(double* values) const; - -private: - double angular_distance_weight_; - MotionModel motion_model_; - /// Only used for the differential drive motion model @see computeTurnDriveTurnGeometry - double min_translational_distance_; -}; - -/** - * @brief Compute the geometry to turn toward the target point, drive straight and then turn to target orientation - * @param[in] from A vector representing the initial position [x0, y0, theta0] - * @param[in] to A vector representing the target position [x1, y1, theta1] - * @param[in] min_translational_distance If the translational distance between \p from and \p to is less than this - * value the motion will be pure rotation (meters) - * @param[out] dx x1 - x0 (meters) - * @param[out] dy y1 - y0 (meters) - * @param[out] initial_turn The initial turn in radians to face the target - * @param[out] drive_angle The orientation in radians that the robot will be driving straight at - * @param[out] final_turn The final turn in radians to the target orientation - */ -void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance, - double& dx, double& dy, double& initial_turn, double& drive_angle, - double& final_turn); -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.hpp new file mode 100644 index 0000000000..88cf4eb010 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.hpp @@ -0,0 +1,133 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** \brief A planar joint */ +class PlanarJointModel : public JointModel +{ +public: + /** \brief different types of planar joints we support */ + enum MotionModel + { + HOLONOMIC, // default + DIFF_DRIVE + }; + + PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); + + void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds) const override; + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds, const double* near, + const double distance) const override; + bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; + bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; + + void interpolate(const double* from, const double* to, const double t, double* state) const override; + unsigned int getStateSpaceDimension() const override; + double getMaximumExtent(const Bounds& other_bounds) const override; + double distance(const double* values1, const double* values2) const override; + + void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; + void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; + + double getAngularDistanceWeight() const + { + return angular_distance_weight_; + } + + void setAngularDistanceWeight(double weight) + { + angular_distance_weight_ = weight; + } + + double getMinTranslationalDistance() const + { + return min_translational_distance_; + } + + void setMinTranslationalDistance(double min_translational_distance) + { + min_translational_distance_ = min_translational_distance; + } + + MotionModel getMotionModel() const + { + return motion_model_; + } + + void setMotionModel(MotionModel model) + { + motion_model_ = model; + } + + /// Make the yaw component of a state's value vector be in the range [-Pi, Pi]. enforceBounds() also calls this + /// function; + /// Return true if a change is actually made + bool normalizeRotation(double* values) const; + +private: + double angular_distance_weight_; + MotionModel motion_model_; + /// Only used for the differential drive motion model @see computeTurnDriveTurnGeometry + double min_translational_distance_; +}; + +/** + * @brief Compute the geometry to turn toward the target point, drive straight and then turn to target orientation + * @param[in] from A vector representing the initial position [x0, y0, theta0] + * @param[in] to A vector representing the target position [x1, y1, theta1] + * @param[in] min_translational_distance If the translational distance between \p from and \p to is less than this + * value the motion will be pure rotation (meters) + * @param[out] dx x1 - x0 (meters) + * @param[out] dy y1 - y0 (meters) + * @param[out] initial_turn The initial turn in radians to face the target + * @param[out] drive_angle The orientation in radians that the robot will be driving straight at + * @param[out] final_turn The final turn in radians to the target orientation + */ +void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance, + double& dx, double& dy, double& initial_turn, double& drive_angle, + double& final_turn); +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index c69e523a55..86908dd1d9 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,53 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** \brief A prismatic joint */ -class PrismaticJointModel : public JointModel -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PrismaticJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); - - void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds) const override; - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds, const double* near, - const double distance) const override; - bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; - bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; - - void interpolate(const double* from, const double* to, const double t, double* state) const override; - unsigned int getStateSpaceDimension() const override; - double getMaximumExtent(const Bounds& other_bounds) const override; - double distance(const double* values1, const double* values2) const override; - - void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; - void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; - - /** \brief Get the axis of translation */ - const Eigen::Vector3d& getAxis() const - { - return axis_; - } - - /** \brief Set the axis of translation */ - void setAxis(const Eigen::Vector3d& axis) - { - axis_ = axis; - } - -protected: - /** \brief The axis of the joint */ - Eigen::Vector3d axis_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.hpp new file mode 100644 index 0000000000..85ac165ef5 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.hpp @@ -0,0 +1,87 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** \brief A prismatic joint */ +class PrismaticJointModel : public JointModel +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PrismaticJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); + + void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds) const override; + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds, const double* near, + const double distance) const override; + bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; + bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; + + void interpolate(const double* from, const double* to, const double t, double* state) const override; + unsigned int getStateSpaceDimension() const override; + double getMaximumExtent(const Bounds& other_bounds) const override; + double distance(const double* values1, const double* values2) const override; + + void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; + void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; + + /** \brief Get the axis of translation */ + const Eigen::Vector3d& getAxis() const + { + return axis_; + } + + /** \brief Set the axis of translation */ + void setAxis(const Eigen::Vector3d& axis) + { + axis_ = axis; + } + +protected: + /** \brief The axis of the joint */ + Eigen::Vector3d axis_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 935a12a697..bdf9e1475a 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,64 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** \brief A revolute joint */ -class RevoluteJointModel : public JointModel -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - RevoluteJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); - void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds) const override; - void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, - const Bounds& other_bounds, const double* near, - const double distance) const override; - bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; - bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; - bool harmonizePosition(double* values, const Bounds& other_bounds) const override; - - void interpolate(const double* from, const double* to, const double t, double* state) const override; - unsigned int getStateSpaceDimension() const override; - double getMaximumExtent(const Bounds& other_bounds) const override; - double distance(const double* values1, const double* values2) const override; - - void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; - void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; - - void setContinuous(bool flag); - - /** \brief Check if this joint wraps around */ - bool isContinuous() const - { - return continuous_; - } - - /** \brief Get the axis of rotation */ - const Eigen::Vector3d& getAxis() const - { - return axis_; - } - - /** \brief Set the axis of rotation */ - void setAxis(const Eigen::Vector3d& axis); - -protected: - /** \brief The axis of the joint */ - Eigen::Vector3d axis_; - - /** \brief Flag indicating whether this joint wraps around */ - bool continuous_; - -private: - double x2_, y2_, z2_, xy_, xz_, yz_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.hpp new file mode 100644 index 0000000000..90798d407a --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.hpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** \brief A revolute joint */ +class RevoluteJointModel : public JointModel +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + RevoluteJointModel(const std::string& name, size_t joint_index, size_t first_variable_index); + void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds) const override; + void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, + const Bounds& other_bounds, const double* near, + const double distance) const override; + bool enforcePositionBounds(double* values, const Bounds& other_bounds) const override; + bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const override; + bool harmonizePosition(double* values, const Bounds& other_bounds) const override; + + void interpolate(const double* from, const double* to, const double t, double* state) const override; + unsigned int getStateSpaceDimension() const override; + double getMaximumExtent(const Bounds& other_bounds) const override; + double distance(const double* values1, const double* values2) const override; + + void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; + void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; + + void setContinuous(bool flag); + + /** \brief Check if this joint wraps around */ + bool isContinuous() const + { + return continuous_; + } + + /** \brief Get the axis of rotation */ + const Eigen::Vector3d& getAxis() const + { + return axis_; + } + + /** \brief Set the axis of rotation */ + void setAxis(const Eigen::Vector3d& axis); + +protected: + /** \brief The axis of the joint */ + Eigen::Vector3d axis_; + + /** \brief Flag indicating whether this joint wraps around */ + bool continuous_; + +private: + double x2_, y2_, z2_, xy_, xz_, yz_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 6c4d1aad1b..91625baf21 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,611 +48,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -// joint types -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** \brief Main namespace for MoveIt */ -namespace moveit -{ -/** \brief Core components of MoveIt */ -namespace core -{ -MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc - -static inline void checkInterpolationParamBounds(const rclcpp::Logger& logger, double t) -{ - if (std::isnan(t) || std::isinf(t)) - { - throw Exception("Interpolation parameter is NaN or inf."); - } - - RCLCPP_WARN_STREAM_EXPRESSION(logger, t < 0. || t > 1., "Interpolation parameter is not in the range [0, 1]: " << t); -} - -/** \brief Definition of a kinematic model. This class is not thread - safe, however multiple instances can be created */ -class RobotModel -{ -public: - /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ - RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model); - - /** \brief Destructor. Clear all memory. */ - ~RobotModel(); - - /** \brief Get the model name. */ - const std::string& getName() const - { - return model_name_; - } - - /** \brief Get the frame in which the transforms for this model are - computed (when using a RobotState). This frame depends on the - root joint. As such, the frame is either extracted from SRDF, or - it is assumed to be the name of the root link */ - const std::string& getModelFrame() const - { - return model_frame_; - } - - /** \brief Return true if the model is empty (has no root link, no joints) */ - bool isEmpty() const - { - return root_link_ == nullptr; - } - - /** \brief Get the parsed URDF model */ - const urdf::ModelInterfaceSharedPtr& getURDF() const - { - return urdf_; - } - - /** \brief Get the parsed SRDF model */ - const srdf::ModelConstSharedPtr& getSRDF() const - { - return srdf_; - } - - /** \brief Print information about the constructed model */ - void printModelInfo(std::ostream& out) const; - - /** \name Access to joint models - * @{ - */ - - /** \brief Get the root joint. There will be one root joint - unless the model is empty. This is either extracted from the - SRDF, or a fixed joint is assumed, if no specification is - given. */ - const JointModel* getRootJoint() const; - - /** \brief Return the name of the root joint. Throws an exception if there is no root joint. */ - const std::string& getRootJointName() const - { - return getRootJoint()->getName(); - } - - /** \brief Check if a joint exists. Return true if it does. */ - bool hasJointModel(const std::string& name) const; - - /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ - const JointModel* getJointModel(const std::string& joint) const; - - /** \brief Get a joint by its index. Output error and return nullptr when the link is missing. */ - const JointModel* getJointModel(size_t index) const; - - /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ - JointModel* getJointModel(const std::string& joint); - - /** \brief Get the array of joints, in the order they appear - in the robot state. */ - const std::vector& getJointModels() const - { - return joint_model_vector_const_; - } - - /** \brief Get the array of joints, in the order they appear in the - robot state. This includes all types of joints (including mimic - & fixed), as opposed to JointModelGroup::getJointModels(). */ - const std::vector& getJointModels() - { - return joint_model_vector_; - } - - /** \brief Get the array of joint names, in the order they appear in the robot state. */ - const std::vector& getJointModelNames() const - { - return joint_model_names_vector_; - } - - /** \brief Get the array of joints that are active (not fixed, not mimic) in this model. */ - const std::vector& getActiveJointModels() const - { - return active_joint_model_vector_const_; - } - - /** \brief Get the array of active joint names, in the order they appear in the robot state. */ - const std::vector& getActiveJointModelNames() const - { - return active_joint_model_names_vector_; - } - - /** \brief Get the array of joints that are active (not fixed, not mimic) in this model */ - const std::vector& getActiveJointModels() - { - return active_joint_model_vector_; - } - - /** \brief This is a list of all single-dof joints (including mimic joints) */ - const std::vector& getSingleDOFJointModels() const - { - return single_dof_joints_; - } - - /** \brief This is a list of all multi-dof joints */ - const std::vector& getMultiDOFJointModels() const - { - return multi_dof_joints_; - } - - /** \brief Get the array of continuous joints, in the order they appear - in the robot state. */ - const std::vector& getContinuousJointModels() const - { - return continuous_joint_model_vector_; - } - - /** \brief Get the array of mimic joints, in the order they appear - in the robot state. */ - const std::vector& getMimicJointModels() const - { - return mimic_joints_; - } - - const JointModel* getJointOfVariable(int variable_index) const - { - return joints_of_variable_[variable_index]; - } - - const JointModel* getJointOfVariable(const std::string& variable) const - { - return joints_of_variable_[getVariableIndex(variable)]; - } - - std::size_t getJointModelCount() const - { - return joint_model_vector_.size(); - } - - /** @} */ - - /** \name Access to link models - * @{ - */ - - /** \brief Get the physical root link of the robot. */ - const LinkModel* getRootLink() const; - - /** \brief Get the name of the root link of the robot. */ - const std::string& getRootLinkName() const - { - return getRootLink()->getName(); - } - - /** \brief Check if a link exists. Return true if it does. - * - * If this is followed by a call to getLinkModel(), better use the latter with the has_link argument */ - bool hasLinkModel(const std::string& name) const; - - /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ - const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const; - - /** \brief Get a link by its index. Output error and return nullptr when the link is missing. */ - const LinkModel* getLinkModel(size_t index) const; - - /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ - LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr); - - /** \brief Get the latest link upwards the kinematic tree, which is only connected via fixed joints - * - * This is useful, if the link should be warped to a specific pose using updateStateWithLinkAt(). - * As updateStateWithLinkAt() warps only the specified link and its descendants, you might not - * achieve what you expect, if link is an abstract frame name. Considering the following example: - * root -> arm0 -> ... -> armN -> wrist -- grasp_frame - * -- palm -> end effector ... - * Calling updateStateWithLinkAt(grasp_frame), will not warp the end effector, which is probably - * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...) - * will actually warp wrist (and all its descendants). - */ - static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link); - - /** \brief Get the array of links */ - const std::vector& getLinkModels() const - { - return link_model_vector_const_; - } - - /** \brief Get the array of links */ - const std::vector& getLinkModels() - { - return link_model_vector_; - } - - /** \brief Get the link names (of all links) */ - const std::vector& getLinkModelNames() const - { - return link_model_names_vector_; - } - - /** \brief Get the link models that have some collision geometry associated to themselves */ - const std::vector& getLinkModelsWithCollisionGeometry() const - { - return link_models_with_collision_geometry_vector_; - } - - /** \brief Get the names of the link models that have some collision geometry associated to themselves */ - const std::vector& getLinkModelNamesWithCollisionGeometry() const - { - return link_model_names_with_collision_geometry_vector_; - } - - std::size_t getLinkModelCount() const - { - return link_model_vector_.size(); - } - - std::size_t getLinkGeometryCount() const - { - return link_geometry_count_; - } - - /** @} */ - - /** \brief Compute the random values for a RobotState */ - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const; - - /** \brief Compute the default values for a RobotState */ - void getVariableDefaultPositions(double* values) const; - - /** \brief Compute the random values for a RobotState */ - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const - { - values.resize(variable_count_); - getVariableRandomPositions(rng, &values[0]); - } - - /** \brief Compute the default values for a RobotState */ - void getVariableDefaultPositions(std::vector& values) const - { - values.resize(variable_count_); - getVariableDefaultPositions(&values[0]); - } - - /** \brief Compute the random values for a RobotState */ - void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, - std::map& values) const; - - /** \brief Compute the default values for a RobotState */ - void getVariableDefaultPositions(std::map& values) const; - - bool enforcePositionBounds(double* state) const - { - return enforcePositionBounds(state, active_joint_models_bounds_); - } - bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const; - bool satisfiesPositionBounds(const double* state, double margin = 0.0) const - { - return satisfiesPositionBounds(state, active_joint_models_bounds_, margin); - } - bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds, - double margin = 0.0) const; - double getMaximumExtent() const - { - return getMaximumExtent(active_joint_models_bounds_); - } - double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const; - - /** \brief Return the sum of joint distances between two states. An L1 norm. Only considers active joints. */ - double distance(const double* state1, const double* state2) const; - - /** - * Interpolate between "from" state, to "to" state. Mimic joints are correctly updated. - * - * @param from interpolate from this state - * @param to to this state - * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. - * @param state holds the result - */ - void interpolate(const double* from, const double* to, double t, double* state) const; - - /** \name Access to joint groups - * @{ - */ - - /** \brief Check if the JointModelGroup \e group exists */ - bool hasJointModelGroup(const std::string& group) const; - - /** \brief Get a joint group from this model (by name) */ - const JointModelGroup* getJointModelGroup(const std::string& name) const; - - /** \brief Get a joint group from this model (by name) */ - JointModelGroup* getJointModelGroup(const std::string& name); - - /** \brief Get the available joint groups */ - const std::vector& getJointModelGroups() const - { - return joint_model_groups_const_; - } - - /** \brief Get the available joint groups */ - const std::vector& getJointModelGroups() - { - return joint_model_groups_; - } - - /** \brief Get the names of all groups that are defined for this model */ - const std::vector& getJointModelGroupNames() const - { - return joint_model_group_names_; - } - - /** \brief Check if an end effector exists */ - bool hasEndEffector(const std::string& eef) const; - - /** \brief Get the joint group that corresponds to a given end-effector name */ - const JointModelGroup* getEndEffector(const std::string& name) const; - - /** \brief Get the joint group that corresponds to a given end-effector name */ - JointModelGroup* getEndEffector(const std::string& name); - - /** \brief Get the map between end effector names and the groups they correspond to */ - const std::vector& getEndEffectors() const - { - return end_effectors_; - } - - /** @} */ - - /** \brief Get the number of variables that describe this model */ - std::size_t getVariableCount() const - { - return variable_count_; - } - - /** \brief Get the names of the variables that make up the joints that form this state. Fixed joints have no DOF, so - they are not here, - but the variables for mimic joints are included. The number of returned elements is always equal to - getVariableCount() */ - const std::vector& getVariableNames() const - { - return variable_names_; - } - - /** \brief Get the bounds for a specific variable. Throw an exception of variable is not found. */ - const VariableBounds& getVariableBounds(const std::string& variable) const - { - return getJointOfVariable(variable)->getVariableBounds(variable); - } - - /** \brief Get the bounds for all the active joints */ - const JointBoundsVector& getActiveJointModelsBounds() const - { - return active_joint_models_bounds_; - } - - void getMissingVariableNames(const std::vector& variables, - std::vector& missing_variables) const; - - /** \brief Get the index of a variable in the robot state */ - size_t getVariableIndex(const std::string& variable) const; - - /** \brief Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument */ - const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const - { - if (!a) - return b; - if (!b) - return a; - return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]]; - } - - /// A map of known kinematics solvers (associated to their group name) - void setKinematicsAllocators(const std::map& allocators); - -protected: - /** \brief Get the transforms between link and all its rigidly attached descendants */ - void computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform, - LinkTransformMap& associated_transforms); - - /** \brief Given two joints, find their common root */ - const JointModel* computeCommonRoot(const JointModel* a, const JointModel* b) const; - - /** \brief Update the variable values for the state of a group with respect to the mimic joints. */ - void updateMimicJoints(double* values) const; - - // GENERIC INFO - - /** \brief The name of the robot */ - std::string model_name_; - - /** \brief The reference (base) frame for this model. The frame is either extracted from the SRDF as a virtual joint, - * or it is assumed to be the name of the root link in the URDF */ - std::string model_frame_; - - srdf::ModelConstSharedPtr srdf_; - - urdf::ModelInterfaceSharedPtr urdf_; - - // LINKS - - /** \brief The first physical link for the robot */ - const LinkModel* root_link_; - - /** \brief A map from link names to their instances */ - LinkModelMap link_model_map_; - - /** \brief The vector of links that are updated when computeTransforms() is called, in the order they are updated */ - std::vector link_model_vector_; - - /** \brief The vector of links that are updated when computeTransforms() is called, in the order they are updated */ - std::vector link_model_vector_const_; - - /** \brief The vector of link names that corresponds to link_model_vector_ */ - std::vector link_model_names_vector_; - - /** \brief Only links that have collision geometry specified */ - std::vector link_models_with_collision_geometry_vector_; - - /** \brief The vector of link names that corresponds to link_models_with_collision_geometry_vector_ */ - std::vector link_model_names_with_collision_geometry_vector_; - - /** \brief Total number of geometric shapes in this model */ - std::size_t link_geometry_count_; - - // JOINTS - - /** \brief The root joint */ - const JointModel* root_joint_; - - /** \brief A map from joint names to their instances */ - JointModelMap joint_model_map_; - - /** \brief The vector of joints in the model, in the order they appear in the state vector */ - std::vector joint_model_vector_; - - /** \brief The vector of joints in the model, in the order they appear in the state vector */ - std::vector joint_model_vector_const_; - - /** \brief The vector of joint names that corresponds to joint_model_vector_ */ - std::vector joint_model_names_vector_; - - /** \brief The vector of joints in the model, in the order they appear in the state vector */ - std::vector active_joint_model_vector_; - - /** \brief The vector of joint names that corresponds to active_joint_model_vector_ */ - std::vector active_joint_model_names_vector_; - - /** \brief The vector of joints in the model, in the order they appear in the state vector */ - std::vector active_joint_model_vector_const_; - - /** \brief The set of continuous joints this model contains */ - std::vector continuous_joint_model_vector_; - - /** \brief The set of mimic joints this model contains */ - std::vector mimic_joints_; - - std::vector single_dof_joints_; - - std::vector multi_dof_joints_; - - /** \brief For every two joints, the index of the common root for the joints is stored. - - for jointA, jointB - the index of the common root is located in the array at location - jointA->getJointIndex() * nr.joints + jointB->getJointIndex(). - The size of this array is nr.joints * nr.joints - */ - std::vector common_joint_roots_; - - // INDEXING - - /** \brief The names of the DOF that make up this state (this is just a sequence of joint variable names; not - * necessarily joint names!) */ - std::vector variable_names_; - - /** \brief Get the number of variables necessary to describe this model */ - std::size_t variable_count_; - - /** \brief The state includes all the joint variables that make up the joints the state consists of. - This map gives the position in the state vector of the group for each of these variables. - Additionally, it includes the names of the joints and the index for the first variable of that joint. */ - VariableIndexMap joint_variables_index_map_; - - std::vector active_joint_model_start_index_; - - /** \brief The bounds for all the active joint models */ - JointBoundsVector active_joint_models_bounds_; - - /** \brief The joints that correspond to each variable index */ - std::vector joints_of_variable_; - - // GROUPS - - /** \brief A map from group names to joint groups */ - JointModelGroupMap joint_model_group_map_; - - /** \brief The known end effectors */ - JointModelGroupMap end_effectors_map_; - - /** \brief The array of joint model groups, in alphabetical order */ - std::vector joint_model_groups_; - - /** \brief The array of joint model groups, in alphabetical order */ - std::vector joint_model_groups_const_; - - /** \brief A vector of all group names, in alphabetical order */ - std::vector joint_model_group_names_; - - /** \brief The array of end-effectors, in alphabetical order */ - std::vector end_effectors_; - -private: - /** \brief Given an URDF model and a SRDF model, build a full kinematic model */ - void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model); - - /** \brief Given a SRDF model describing the groups, build up the groups in this kinematic model */ - void buildGroups(const srdf::Model& srdf_model); - - /** \brief Compute helpful information about groups (that can be queried later) */ - void buildGroupsInfoSubgroups(); - - /** \brief Compute helpful information about groups (that can be queried later) */ - void buildGroupsInfoEndEffectors(const srdf::Model& srdf_model); - - /** \brief Given the URDF model, build up the mimic joints (mutually constrained joints) */ - void buildMimic(const urdf::ModelInterface& urdf_model); - - /** \brief Given a SRDF model describing the groups, build the default states defined in the SRDF */ - void buildGroupStates(const srdf::Model& srdf_model); - - /** \brief Compute helpful information about joints */ - void buildJointInfo(); - - /** \brief For every joint, pre-compute the list of descendant joints & links */ - void computeDescendants(); - - /** \brief For every pair of joints, pre-compute the common roots of the joints */ - void computeCommonRoots(); - - /** \brief (This function is mostly intended for internal use). Given a parent link, build up (recursively), - the kinematic model by walking down the tree*/ - JointModel* buildRecursive(LinkModel* parent, const urdf::Link* link, const srdf::Model& srdf_model); - - /** \brief Construct a JointModelGroup given a SRDF description \e group */ - bool addJointModelGroup(const srdf::Model::Group& group); - - /** \brief Given a child link and a srdf model, - build up the corresponding JointModel object*/ - JointModel* constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model); - - /** \brief Given a urdf link, build the corresponding LinkModel object*/ - LinkModel* constructLinkModel(const urdf::Link* urdf_link); - - /** \brief Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape object*/ - shapes::ShapePtr constructShape(const urdf::Geometry* geom); -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp b/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp new file mode 100644 index 0000000000..2cc06f984d --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp @@ -0,0 +1,646 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, 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, Inc. 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 */ + +#pragma once + +#include +#include +#include + +// joint types +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** \brief Main namespace for MoveIt */ +namespace moveit +{ +/** \brief Core components of MoveIt */ +namespace core +{ +MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc + +static inline void checkInterpolationParamBounds(const rclcpp::Logger& logger, double t) +{ + if (std::isnan(t) || std::isinf(t)) + { + throw Exception("Interpolation parameter is NaN or inf."); + } + + RCLCPP_WARN_STREAM_EXPRESSION(logger, t < 0. || t > 1., "Interpolation parameter is not in the range [0, 1]: " << t); +} + +/** \brief Definition of a kinematic model. This class is not thread + safe, however multiple instances can be created */ +class RobotModel +{ +public: + /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ + RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model); + + /** \brief Destructor. Clear all memory. */ + ~RobotModel(); + + /** \brief Get the model name. */ + const std::string& getName() const + { + return model_name_; + } + + /** \brief Get the frame in which the transforms for this model are + computed (when using a RobotState). This frame depends on the + root joint. As such, the frame is either extracted from SRDF, or + it is assumed to be the name of the root link */ + const std::string& getModelFrame() const + { + return model_frame_; + } + + /** \brief Return true if the model is empty (has no root link, no joints) */ + bool isEmpty() const + { + return root_link_ == nullptr; + } + + /** \brief Get the parsed URDF model */ + const urdf::ModelInterfaceSharedPtr& getURDF() const + { + return urdf_; + } + + /** \brief Get the parsed SRDF model */ + const srdf::ModelConstSharedPtr& getSRDF() const + { + return srdf_; + } + + /** \brief Print information about the constructed model */ + void printModelInfo(std::ostream& out) const; + + /** \name Access to joint models + * @{ + */ + + /** \brief Get the root joint. There will be one root joint + unless the model is empty. This is either extracted from the + SRDF, or a fixed joint is assumed, if no specification is + given. */ + const JointModel* getRootJoint() const; + + /** \brief Return the name of the root joint. Throws an exception if there is no root joint. */ + const std::string& getRootJointName() const + { + return getRootJoint()->getName(); + } + + /** \brief Check if a joint exists. Return true if it does. */ + bool hasJointModel(const std::string& name) const; + + /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ + const JointModel* getJointModel(const std::string& joint) const; + + /** \brief Get a joint by its index. Output error and return nullptr when the link is missing. */ + const JointModel* getJointModel(size_t index) const; + + /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */ + JointModel* getJointModel(const std::string& joint); + + /** \brief Get the array of joints, in the order they appear + in the robot state. */ + const std::vector& getJointModels() const + { + return joint_model_vector_const_; + } + + /** \brief Get the array of joints, in the order they appear in the + robot state. This includes all types of joints (including mimic + & fixed), as opposed to JointModelGroup::getJointModels(). */ + const std::vector& getJointModels() + { + return joint_model_vector_; + } + + /** \brief Get the array of joint names, in the order they appear in the robot state. */ + const std::vector& getJointModelNames() const + { + return joint_model_names_vector_; + } + + /** \brief Get the array of joints that are active (not fixed, not mimic) in this model. */ + const std::vector& getActiveJointModels() const + { + return active_joint_model_vector_const_; + } + + /** \brief Get the array of active joint names, in the order they appear in the robot state. */ + const std::vector& getActiveJointModelNames() const + { + return active_joint_model_names_vector_; + } + + /** \brief Get the array of joints that are active (not fixed, not mimic) in this model */ + const std::vector& getActiveJointModels() + { + return active_joint_model_vector_; + } + + /** \brief This is a list of all single-dof joints (including mimic joints) */ + const std::vector& getSingleDOFJointModels() const + { + return single_dof_joints_; + } + + /** \brief This is a list of all multi-dof joints */ + const std::vector& getMultiDOFJointModels() const + { + return multi_dof_joints_; + } + + /** \brief Get the array of continuous joints, in the order they appear + in the robot state. */ + const std::vector& getContinuousJointModels() const + { + return continuous_joint_model_vector_; + } + + /** \brief Get the array of mimic joints, in the order they appear + in the robot state. */ + const std::vector& getMimicJointModels() const + { + return mimic_joints_; + } + + const JointModel* getJointOfVariable(int variable_index) const + { + return joints_of_variable_[variable_index]; + } + + const JointModel* getJointOfVariable(const std::string& variable) const + { + return joints_of_variable_[getVariableIndex(variable)]; + } + + std::size_t getJointModelCount() const + { + return joint_model_vector_.size(); + } + + /** @} */ + + /** \name Access to link models + * @{ + */ + + /** \brief Get the physical root link of the robot. */ + const LinkModel* getRootLink() const; + + /** \brief Get the name of the root link of the robot. */ + const std::string& getRootLinkName() const + { + return getRootLink()->getName(); + } + + /** \brief Check if a link exists. Return true if it does. + * + * If this is followed by a call to getLinkModel(), better use the latter with the has_link argument */ + bool hasLinkModel(const std::string& name) const; + + /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ + const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const; + + /** \brief Get a link by its index. Output error and return nullptr when the link is missing. */ + const LinkModel* getLinkModel(size_t index) const; + + /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */ + LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr); + + /** \brief Get the latest link upwards the kinematic tree, which is only connected via fixed joints + * + * This is useful, if the link should be warped to a specific pose using updateStateWithLinkAt(). + * As updateStateWithLinkAt() warps only the specified link and its descendants, you might not + * achieve what you expect, if link is an abstract frame name. Considering the following example: + * root -> arm0 -> ... -> armN -> wrist -- grasp_frame + * -- palm -> end effector ... + * Calling updateStateWithLinkAt(grasp_frame), will not warp the end effector, which is probably + * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...) + * will actually warp wrist (and all its descendants). + */ + static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link); + + /** \brief Get the array of links */ + const std::vector& getLinkModels() const + { + return link_model_vector_const_; + } + + /** \brief Get the array of links */ + const std::vector& getLinkModels() + { + return link_model_vector_; + } + + /** \brief Get the link names (of all links) */ + const std::vector& getLinkModelNames() const + { + return link_model_names_vector_; + } + + /** \brief Get the link models that have some collision geometry associated to themselves */ + const std::vector& getLinkModelsWithCollisionGeometry() const + { + return link_models_with_collision_geometry_vector_; + } + + /** \brief Get the names of the link models that have some collision geometry associated to themselves */ + const std::vector& getLinkModelNamesWithCollisionGeometry() const + { + return link_model_names_with_collision_geometry_vector_; + } + + std::size_t getLinkModelCount() const + { + return link_model_vector_.size(); + } + + std::size_t getLinkGeometryCount() const + { + return link_geometry_count_; + } + + /** @} */ + + /** \brief Compute the random values for a RobotState */ + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const; + + /** \brief Compute the default values for a RobotState */ + void getVariableDefaultPositions(double* values) const; + + /** \brief Compute the random values for a RobotState */ + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const + { + values.resize(variable_count_); + getVariableRandomPositions(rng, &values[0]); + } + + /** \brief Compute the default values for a RobotState */ + void getVariableDefaultPositions(std::vector& values) const + { + values.resize(variable_count_); + getVariableDefaultPositions(&values[0]); + } + + /** \brief Compute the random values for a RobotState */ + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, + std::map& values) const; + + /** \brief Compute the default values for a RobotState */ + void getVariableDefaultPositions(std::map& values) const; + + bool enforcePositionBounds(double* state) const + { + return enforcePositionBounds(state, active_joint_models_bounds_); + } + bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const; + bool satisfiesPositionBounds(const double* state, double margin = 0.0) const + { + return satisfiesPositionBounds(state, active_joint_models_bounds_, margin); + } + bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds, + double margin = 0.0) const; + double getMaximumExtent() const + { + return getMaximumExtent(active_joint_models_bounds_); + } + double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const; + + /** \brief Return the sum of joint distances between two states. An L1 norm. Only considers active joints. */ + double distance(const double* state1, const double* state2) const; + + /** + * Interpolate between "from" state, to "to" state. Mimic joints are correctly updated. + * + * @param from interpolate from this state + * @param to to this state + * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. + * @param state holds the result + */ + void interpolate(const double* from, const double* to, double t, double* state) const; + + /** \name Access to joint groups + * @{ + */ + + /** \brief Check if the JointModelGroup \e group exists */ + bool hasJointModelGroup(const std::string& group) const; + + /** \brief Get a joint group from this model (by name) */ + const JointModelGroup* getJointModelGroup(const std::string& name) const; + + /** \brief Get a joint group from this model (by name) */ + JointModelGroup* getJointModelGroup(const std::string& name); + + /** \brief Get the available joint groups */ + const std::vector& getJointModelGroups() const + { + return joint_model_groups_const_; + } + + /** \brief Get the available joint groups */ + const std::vector& getJointModelGroups() + { + return joint_model_groups_; + } + + /** \brief Get the names of all groups that are defined for this model */ + const std::vector& getJointModelGroupNames() const + { + return joint_model_group_names_; + } + + /** \brief Check if an end effector exists */ + bool hasEndEffector(const std::string& eef) const; + + /** \brief Get the joint group that corresponds to a given end-effector name */ + const JointModelGroup* getEndEffector(const std::string& name) const; + + /** \brief Get the joint group that corresponds to a given end-effector name */ + JointModelGroup* getEndEffector(const std::string& name); + + /** \brief Get the map between end effector names and the groups they correspond to */ + const std::vector& getEndEffectors() const + { + return end_effectors_; + } + + /** @} */ + + /** \brief Get the number of variables that describe this model */ + std::size_t getVariableCount() const + { + return variable_count_; + } + + /** \brief Get the names of the variables that make up the joints that form this state. Fixed joints have no DOF, so + they are not here, + but the variables for mimic joints are included. The number of returned elements is always equal to + getVariableCount() */ + const std::vector& getVariableNames() const + { + return variable_names_; + } + + /** \brief Get the bounds for a specific variable. Throw an exception of variable is not found. */ + const VariableBounds& getVariableBounds(const std::string& variable) const + { + return getJointOfVariable(variable)->getVariableBounds(variable); + } + + /** \brief Get the bounds for all the active joints */ + const JointBoundsVector& getActiveJointModelsBounds() const + { + return active_joint_models_bounds_; + } + + void getMissingVariableNames(const std::vector& variables, + std::vector& missing_variables) const; + + /** \brief Get the index of a variable in the robot state */ + size_t getVariableIndex(const std::string& variable) const; + + /** \brief Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument */ + const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const + { + if (!a) + return b; + if (!b) + return a; + return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]]; + } + + /// A map of known kinematics solvers (associated to their group name) + void setKinematicsAllocators(const std::map& allocators); + +protected: + /** \brief Get the transforms between link and all its rigidly attached descendants */ + void computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform, + LinkTransformMap& associated_transforms); + + /** \brief Given two joints, find their common root */ + const JointModel* computeCommonRoot(const JointModel* a, const JointModel* b) const; + + /** \brief Update the variable values for the state of a group with respect to the mimic joints. */ + void updateMimicJoints(double* values) const; + + // GENERIC INFO + + /** \brief The name of the robot */ + std::string model_name_; + + /** \brief The reference (base) frame for this model. The frame is either extracted from the SRDF as a virtual joint, + * or it is assumed to be the name of the root link in the URDF */ + std::string model_frame_; + + srdf::ModelConstSharedPtr srdf_; + + urdf::ModelInterfaceSharedPtr urdf_; + + // LINKS + + /** \brief The first physical link for the robot */ + const LinkModel* root_link_; + + /** \brief A map from link names to their instances */ + LinkModelMap link_model_map_; + + /** \brief The vector of links that are updated when computeTransforms() is called, in the order they are updated */ + std::vector link_model_vector_; + + /** \brief The vector of links that are updated when computeTransforms() is called, in the order they are updated */ + std::vector link_model_vector_const_; + + /** \brief The vector of link names that corresponds to link_model_vector_ */ + std::vector link_model_names_vector_; + + /** \brief Only links that have collision geometry specified */ + std::vector link_models_with_collision_geometry_vector_; + + /** \brief The vector of link names that corresponds to link_models_with_collision_geometry_vector_ */ + std::vector link_model_names_with_collision_geometry_vector_; + + /** \brief Total number of geometric shapes in this model */ + std::size_t link_geometry_count_; + + // JOINTS + + /** \brief The root joint */ + const JointModel* root_joint_; + + /** \brief A map from joint names to their instances */ + JointModelMap joint_model_map_; + + /** \brief The vector of joints in the model, in the order they appear in the state vector */ + std::vector joint_model_vector_; + + /** \brief The vector of joints in the model, in the order they appear in the state vector */ + std::vector joint_model_vector_const_; + + /** \brief The vector of joint names that corresponds to joint_model_vector_ */ + std::vector joint_model_names_vector_; + + /** \brief The vector of joints in the model, in the order they appear in the state vector */ + std::vector active_joint_model_vector_; + + /** \brief The vector of joint names that corresponds to active_joint_model_vector_ */ + std::vector active_joint_model_names_vector_; + + /** \brief The vector of joints in the model, in the order they appear in the state vector */ + std::vector active_joint_model_vector_const_; + + /** \brief The set of continuous joints this model contains */ + std::vector continuous_joint_model_vector_; + + /** \brief The set of mimic joints this model contains */ + std::vector mimic_joints_; + + std::vector single_dof_joints_; + + std::vector multi_dof_joints_; + + /** \brief For every two joints, the index of the common root for the joints is stored. + + for jointA, jointB + the index of the common root is located in the array at location + jointA->getJointIndex() * nr.joints + jointB->getJointIndex(). + The size of this array is nr.joints * nr.joints + */ + std::vector common_joint_roots_; + + // INDEXING + + /** \brief The names of the DOF that make up this state (this is just a sequence of joint variable names; not + * necessarily joint names!) */ + std::vector variable_names_; + + /** \brief Get the number of variables necessary to describe this model */ + std::size_t variable_count_; + + /** \brief The state includes all the joint variables that make up the joints the state consists of. + This map gives the position in the state vector of the group for each of these variables. + Additionally, it includes the names of the joints and the index for the first variable of that joint. */ + VariableIndexMap joint_variables_index_map_; + + std::vector active_joint_model_start_index_; + + /** \brief The bounds for all the active joint models */ + JointBoundsVector active_joint_models_bounds_; + + /** \brief The joints that correspond to each variable index */ + std::vector joints_of_variable_; + + // GROUPS + + /** \brief A map from group names to joint groups */ + JointModelGroupMap joint_model_group_map_; + + /** \brief The known end effectors */ + JointModelGroupMap end_effectors_map_; + + /** \brief The array of joint model groups, in alphabetical order */ + std::vector joint_model_groups_; + + /** \brief The array of joint model groups, in alphabetical order */ + std::vector joint_model_groups_const_; + + /** \brief A vector of all group names, in alphabetical order */ + std::vector joint_model_group_names_; + + /** \brief The array of end-effectors, in alphabetical order */ + std::vector end_effectors_; + +private: + /** \brief Given an URDF model and a SRDF model, build a full kinematic model */ + void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model); + + /** \brief Given a SRDF model describing the groups, build up the groups in this kinematic model */ + void buildGroups(const srdf::Model& srdf_model); + + /** \brief Compute helpful information about groups (that can be queried later) */ + void buildGroupsInfoSubgroups(); + + /** \brief Compute helpful information about groups (that can be queried later) */ + void buildGroupsInfoEndEffectors(const srdf::Model& srdf_model); + + /** \brief Given the URDF model, build up the mimic joints (mutually constrained joints) */ + void buildMimic(const urdf::ModelInterface& urdf_model); + + /** \brief Given a SRDF model describing the groups, build the default states defined in the SRDF */ + void buildGroupStates(const srdf::Model& srdf_model); + + /** \brief Compute helpful information about joints */ + void buildJointInfo(); + + /** \brief For every joint, pre-compute the list of descendant joints & links */ + void computeDescendants(); + + /** \brief For every pair of joints, pre-compute the common roots of the joints */ + void computeCommonRoots(); + + /** \brief (This function is mostly intended for internal use). Given a parent link, build up (recursively), + the kinematic model by walking down the tree*/ + JointModel* buildRecursive(LinkModel* parent, const urdf::Link* link, const srdf::Model& srdf_model); + + /** \brief Construct a JointModelGroup given a SRDF description \e group */ + bool addJointModelGroup(const srdf::Model::Group& group); + + /** \brief Given a child link and a srdf model, + build up the corresponding JointModel object*/ + JointModel* constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model); + + /** \brief Given a urdf link, build the corresponding LinkModel object*/ + LinkModel* constructLinkModel(const urdf::Link* urdf_link); + + /** \brief Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape object*/ + shapes::ShapePtr constructShape(const urdf::Geometry* geom); +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index 91552b0fa2..04e1372f15 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -34,7 +34,7 @@ /* Author: Martin Pecka */ -#include +#include #include void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp index 5873f0c951..9a230e0dab 100644 --- a/moveit_core/robot_model/src/fixed_joint_model.cpp +++ b/moveit_core/robot_model/src/fixed_joint_model.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include namespace moveit { diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 776ed5efd9..e3b6e3255b 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index 933c65e5ab..5da2403476 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -35,9 +35,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include namespace moveit diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 2776bb0c9d..a9d23c33ea 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -35,10 +35,10 @@ /* Author: Ioan Sucan, Dave Coleman */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index da3e447c14..98150b935f 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -34,11 +34,11 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include #include -#include +#include namespace moveit { diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 6bede60c14..deb7b04951 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace moveit { diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index 81e9128053..7a486288d0 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include namespace moveit diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 524632159c..515cc3ea3a 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index f65ff86a11..c6634f5b5d 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index db76212b2e..5ed7236fd4 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -34,12 +34,12 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include -#include +#include class LoadPlanningModelsPr2 : public testing::Test { diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index a7c74d0905..0bf8a61f8d 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,210 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace core -{ -class AttachedBody; -typedef std::function AttachedBodyCallback; - -/** @brief Object defining bodies that can be attached to robot links. - * - * This is useful when handling objects picked up by the robot. */ -class AttachedBody -{ -public: - /** \brief Construct an attached body for a specified \e link. - * - * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms - * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links. - * detach_posture may describe a detach motion for the gripper when placing the object. - * The shape and subframe poses are relative to the \e pose, and \e pose is relative to the parent link. */ - AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose, - const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, - const std::set& touch_links, const trajectory_msgs::msg::JointTrajectory& detach_posture, - const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); - - ~AttachedBody(); - - /** \brief Get the name of the attached body */ - const std::string& getName() const - { - return id_; - } - - /** \brief Get the pose of the attached body relative to the parent link */ - const Eigen::Isometry3d& getPose() const - { - return pose_; - } - - /** \brief Get the pose of the attached body, relative to the world */ - const Eigen::Isometry3d& getGlobalPose() const - { - return global_pose_; - } - - /** \brief Get the name of the link this body is attached to */ - const std::string& getAttachedLinkName() const - { - return parent_link_model_->getName(); - } - - /** \brief Get the model of the link this body is attached to */ - const LinkModel* getAttachedLink() const - { - return parent_link_model_; - } - - /** \brief Get the shapes that make up this attached body */ - const std::vector& getShapes() const - { - return shapes_; - } - - /** \brief Get the shape poses (the transforms to the shapes of this body, relative to the pose). The returned - * transforms are guaranteed to be valid isometries. */ - const EigenSTL::vector_Isometry3d& getShapePoses() const - { - return shape_poses_; - } - - /** \brief Get the links that the attached body is allowed to touch */ - const std::set& getTouchLinks() const - { - return touch_links_; - } - - /** \brief Return the posture that is necessary for the object to be released, (if any). This is useful for example - when storing the configuration of a gripper holding an object */ - const trajectory_msgs::msg::JointTrajectory& getDetachPosture() const - { - return detach_posture_; - } - - /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link). The returned - * transforms are guaranteed to be valid isometries. */ - const EigenSTL::vector_Isometry3d& getShapePosesInLinkFrame() const - { - return shape_poses_in_link_frame_; - } - - /** \brief Get subframes of this object (relative to the object pose). The returned transforms are guaranteed to be - * valid isometries. */ - const moveit::core::FixedTransformsMap& getSubframes() const - { - return subframe_poses_; - } - - /** \brief Get subframes of this object (in the world frame) */ - const moveit::core::FixedTransformsMap& getGlobalSubframeTransforms() const - { - return global_subframe_poses_; - } - - /** \brief Set all subframes of this object. - * - * Use these to define points of interest on the object to plan with - * (e.g. screwdriver/tip, kettle/spout, mug/base). - */ - void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses) - { - for (const auto& t : subframe_poses) - { - ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry - } - subframe_poses_ = subframe_poses; - } - - /** \brief Get the fixed transform to a named subframe on this body (relative to the body's pose) - * - * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). - * The returned transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; - - /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) - * - * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). - * The returned transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const; - - /** \brief Get the fixed transform to a named subframe on this body, relative to the world frame. - * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). - * The returned transform is guaranteed to be a valid isometry. */ - const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; - - /** \brief Check whether a subframe of given @frame_name is present in this object. - * - * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). */ - bool hasSubframeTransform(const std::string& frame_name) const; - - /** \brief Get the global transforms (in world frame) for the collision bodies. The returned transforms are - * guaranteed to be valid isometries. */ - const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const - { - return global_collision_body_transforms_; - } - - /** \brief Set the padding for the shapes of this attached object */ - void setPadding(double padding); - - /** \brief Set the scale for the shapes of this attached object */ - void setScale(double scale); - - /** \brief Recompute global_collision_body_transform given the transform of the parent link */ - void computeTransform(const Eigen::Isometry3d& parent_link_global_transform); - -private: - /** \brief The link that owns this attached body */ - const LinkModel* parent_link_model_; - - /** \brief string id for reference */ - std::string id_; - - /** \brief The transform from the parent link to the attached body's pose*/ - Eigen::Isometry3d pose_; - - /** \brief The transform from the model frame to the attached body's pose */ - Eigen::Isometry3d global_pose_; - - /** \brief The geometries of the attached body */ - std::vector shapes_; - - /** \brief The transforms from the object's pose to the object's geometries*/ - EigenSTL::vector_Isometry3d shape_poses_; - - /** \brief The transforms from the link to the object's geometries*/ - EigenSTL::vector_Isometry3d shape_poses_in_link_frame_; - - /** \brief The global transforms for the attached bodies (computed by forward kinematics) */ - EigenSTL::vector_Isometry3d global_collision_body_transforms_; - - /** \brief The set of links this body is allowed to touch */ - std::set touch_links_; - - /** \brief Posture of links for releasing the object (if any). This is useful for example when storing - the configuration of a gripper holding an object */ - trajectory_msgs::msg::JointTrajectory detach_posture_; - - /** \brief Transforms to subframes on the object, relative to the object's pose. */ - moveit::core::FixedTransformsMap subframe_poses_; - - /** \brief Transforms to subframes on the object, relative to the model frame. */ - moveit::core::FixedTransformsMap global_subframe_poses_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp b/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp new file mode 100644 index 0000000000..4ef09a2518 --- /dev/null +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.hpp @@ -0,0 +1,244 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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, Inc. 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +class AttachedBody; +typedef std::function AttachedBodyCallback; + +/** @brief Object defining bodies that can be attached to robot links. + * + * This is useful when handling objects picked up by the robot. */ +class AttachedBody +{ +public: + /** \brief Construct an attached body for a specified \e link. + * + * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms + * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links. + * detach_posture may describe a detach motion for the gripper when placing the object. + * The shape and subframe poses are relative to the \e pose, and \e pose is relative to the parent link. */ + AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose, + const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, + const std::set& touch_links, const trajectory_msgs::msg::JointTrajectory& detach_posture, + const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); + + ~AttachedBody(); + + /** \brief Get the name of the attached body */ + const std::string& getName() const + { + return id_; + } + + /** \brief Get the pose of the attached body relative to the parent link */ + const Eigen::Isometry3d& getPose() const + { + return pose_; + } + + /** \brief Get the pose of the attached body, relative to the world */ + const Eigen::Isometry3d& getGlobalPose() const + { + return global_pose_; + } + + /** \brief Get the name of the link this body is attached to */ + const std::string& getAttachedLinkName() const + { + return parent_link_model_->getName(); + } + + /** \brief Get the model of the link this body is attached to */ + const LinkModel* getAttachedLink() const + { + return parent_link_model_; + } + + /** \brief Get the shapes that make up this attached body */ + const std::vector& getShapes() const + { + return shapes_; + } + + /** \brief Get the shape poses (the transforms to the shapes of this body, relative to the pose). The returned + * transforms are guaranteed to be valid isometries. */ + const EigenSTL::vector_Isometry3d& getShapePoses() const + { + return shape_poses_; + } + + /** \brief Get the links that the attached body is allowed to touch */ + const std::set& getTouchLinks() const + { + return touch_links_; + } + + /** \brief Return the posture that is necessary for the object to be released, (if any). This is useful for example + when storing the configuration of a gripper holding an object */ + const trajectory_msgs::msg::JointTrajectory& getDetachPosture() const + { + return detach_posture_; + } + + /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link). The returned + * transforms are guaranteed to be valid isometries. */ + const EigenSTL::vector_Isometry3d& getShapePosesInLinkFrame() const + { + return shape_poses_in_link_frame_; + } + + /** \brief Get subframes of this object (relative to the object pose). The returned transforms are guaranteed to be + * valid isometries. */ + const moveit::core::FixedTransformsMap& getSubframes() const + { + return subframe_poses_; + } + + /** \brief Get subframes of this object (in the world frame) */ + const moveit::core::FixedTransformsMap& getGlobalSubframeTransforms() const + { + return global_subframe_poses_; + } + + /** \brief Set all subframes of this object. + * + * Use these to define points of interest on the object to plan with + * (e.g. screwdriver/tip, kettle/spout, mug/base). + */ + void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses) + { + for (const auto& t : subframe_poses) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } + subframe_poses_ = subframe_poses; + } + + /** \brief Get the fixed transform to a named subframe on this body (relative to the body's pose) + * + * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; + + /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) + * + * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const; + + /** \brief Get the fixed transform to a named subframe on this body, relative to the world frame. + * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ + const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; + + /** \brief Check whether a subframe of given @frame_name is present in this object. + * + * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's + * name is "screwdriver"). */ + bool hasSubframeTransform(const std::string& frame_name) const; + + /** \brief Get the global transforms (in world frame) for the collision bodies. The returned transforms are + * guaranteed to be valid isometries. */ + const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const + { + return global_collision_body_transforms_; + } + + /** \brief Set the padding for the shapes of this attached object */ + void setPadding(double padding); + + /** \brief Set the scale for the shapes of this attached object */ + void setScale(double scale); + + /** \brief Recompute global_collision_body_transform given the transform of the parent link */ + void computeTransform(const Eigen::Isometry3d& parent_link_global_transform); + +private: + /** \brief The link that owns this attached body */ + const LinkModel* parent_link_model_; + + /** \brief string id for reference */ + std::string id_; + + /** \brief The transform from the parent link to the attached body's pose*/ + Eigen::Isometry3d pose_; + + /** \brief The transform from the model frame to the attached body's pose */ + Eigen::Isometry3d global_pose_; + + /** \brief The geometries of the attached body */ + std::vector shapes_; + + /** \brief The transforms from the object's pose to the object's geometries*/ + EigenSTL::vector_Isometry3d shape_poses_; + + /** \brief The transforms from the link to the object's geometries*/ + EigenSTL::vector_Isometry3d shape_poses_in_link_frame_; + + /** \brief The global transforms for the attached bodies (computed by forward kinematics) */ + EigenSTL::vector_Isometry3d global_collision_body_transforms_; + + /** \brief The set of links this body is allowed to touch */ + std::set touch_links_; + + /** \brief Posture of links for releasing the object (if any). This is useful for example when storing + the configuration of a gripper holding an object */ + trajectory_msgs::msg::JointTrajectory detach_posture_; + + /** \brief Transforms to subframes on the object, relative to the object's pose. */ + moveit::core::FixedTransformsMap subframe_poses_; + + /** \brief Transforms to subframes on the object, relative to the model frame. */ + moveit::core::FixedTransformsMap global_subframe_poses_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index 089eba6b8f..d84fe72c67 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,297 +49,5 @@ /* Author: Ioan Sucan, Mike Lautman */ #pragma once - -#include - -namespace moveit -{ -namespace core -{ -/** Struct defining linear and rotational precision */ -struct CartesianPrecision -{ - double translational = 0.001; //< max deviation in translation (meters) - double rotational = 0.01; //< max deviation in rotation (radians) - double max_resolution = 1e-5; //< max resolution for waypoints (fraction of total path) -}; - -/** \brief Struct with options for defining joint-space jump thresholds. */ -struct JumpThreshold -{ - JumpThreshold() = default; // default is equivalent to disabled(). - - /** \brief Do not define any jump threshold, i.e., disable joint-space jump detection. */ - static JumpThreshold disabled(); - - /** \brief Detect joint-space jumps relative to the average joint-space displacement along the path. - - The average joint-space distance between consecutive points in the path is computed. If any individual joint-space - motion delta is larger than the average distance by a factor of `relative_factor`, it is considered the path has a - jump. For instance, a `relative_factor` of 10.0 will detect joint increments larger than 10x the average increment - */ - static JumpThreshold relative(double relative_factor); - - /** \brief Detect joint-space jumps greater than the given absolute thresholds. - - `revolute` and `prismatic` are absolute joint displacement thresholds, in radians and meters respectively. - If any two consecutive waypoints have a joint-space distance larger than these values, the path has a jump. */ - static JumpThreshold absolute(double revolute, double prismatic); - - double relative_factor = 0.0; - double revolute = 0.0; // Radians - double prismatic = 0.0; // Meters - - // Deprecated constructors. Construct using the builder methods above. - [[deprecated("Use JumpThreshold::relative() instead.")]] JumpThreshold(double relative_factor); - [[deprecated("Use JumpThreshold::absolute() instead.")]] JumpThreshold(double revolute, double prismatic); -}; - -/** \brief Struct for containing max_step for computeCartesianPath - - Setting translation to zero will disable checking for translations. The same goes for rotation. - Initializing with only one value (translation) sets the rotation such that - 1 cm of allowed translation = 2 degrees of allowed rotation. */ -struct MaxEEFStep -{ - MaxEEFStep(double translation, double rotation) : translation(translation), rotation(rotation) - { - } - - MaxEEFStep(double step_size) : translation(step_size), rotation(3.5 * step_size) // 0.035 rad = 2 deg - { - } - - double translation; // Meters - double rotation; // Radians -}; - -class CartesianInterpolator -{ - // TODO(mlautman): Eventually, this planner should be moved out of robot_state - -public: - struct Percentage - { - // value must be in [0,1] - Percentage(double value) : value(value) - { - if (value < 0.0 || value > 1.0) - throw std::runtime_error("Percentage values must be between 0 and 1, inclusive"); - } - operator double() - { - return value; - } - double operator*() - { - return value; - } - Percentage operator*(const Percentage& p) - { - Percentage res(value * p.value); - return res; - } - double value; - }; - - struct Distance - { - Distance(double meters) : meters(meters) - { - } - operator double() - { - return meters; - } - double operator*() - { - return meters; - } - Distance operator*(const Percentage& p) - { - Distance res(meters * p.value); - return res; - } - double meters; - }; - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. - - The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. - This vector is assumed to be specified either in the global reference frame or in the local - reference frame of the link. - The resulting joint values are stored in the vector \e traj, one by one. The interpolation distance in - Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which - provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal - call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to - the distance that was achieved and for which corresponding states were added to the path. - - The struct CartesianPrecision specifies the precision to which the path should follow the Cartesian straight line. - If the deviation at the mid point of two consecutive waypoints is larger than the specified precision, another waypoint - will be inserted at that mid point. The precision is specified separately for translation and rotation. - The maximal resolution to consider (as fraction of the total path length) is specified by max_resolution. - */ - static Distance computeCartesianPath( - const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, - const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, - const MaxEEFStep& max_step, const CartesianPrecision& precision, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. - - In contrast to the previous function, the translation vector is specified as a (unit) direction vector and - a distance. */ - static Distance computeCartesianPath( - const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, - const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, - const MaxEEFStep& max_step, const CartesianPrecision& precision, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) - { - return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, - precision, validCallback, options, cost_function); - } - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. - - In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) - for a virtual frame attached to the robot \e link with the given \e link_offset. - The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame. - This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */ - static Percentage computeCartesianPath( - const RobotState* start_state, const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, - const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options, - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), - const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); - - /** \brief Compute the sequence of joint values that perform a general Cartesian path. - - In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially - reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global - reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs - to move in a straight line between two consecutive waypoints. All other comments apply. */ - static Percentage computeCartesianPath( - const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, - const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, - const MaxEEFStep& max_step, const CartesianPrecision& precision, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), - const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. - - The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. - This vector is assumed to be specified either in the global reference frame or in the local - reference frame of the link (\e global_reference_frame is false). - The resulting joint values are stored in the vector \e path, one by one. The maximum distance in - Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which - provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal - call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to - the distance that was achieved and for which corresponding states were added to the path. At the end of the - function call, the state of the group corresponds to the last attempted Cartesian pose. - - During the computation of the path, it is usually preferred if consecutive joint values do not 'jump' by a - large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold struct is provided, which comprises three fields: - \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. - If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. - If \e jump_threshold_factor is non-zero, we test for relative jumps. To this end, the average joint-space distance - between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger than - this average distance multiplied by \e jump_threshold_factor, this step is considered a jump. - - Otherwise (if all params are zero), jump detection is disabled. - If a jump is detected, the path is truncated up to just before the jump. - - Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e - cost_function. */ - [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& path, - const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, - const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. - - In contrast to the previous function, the translation vector is specified as a (unit) direction vector and - a distance. */ - [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& path, - const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, - const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) - { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step, - jump_threshold, validCallback, options, cost_function); -#pragma GCC diagnostic pop - } - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. - - In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) - for a virtual frame attached to the robot \e link with the given \e link_offset. - The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame - (\e global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved. - All other comments from the previous function apply. */ - [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& path, - const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, - const JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), - const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); - - /** \brief Compute the sequence of joint values that perform a general Cartesian path. - - In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially - reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the - global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move - in a straight line between two consecutive waypoints. All other comments apply. */ - [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& path, - const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, - const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), - const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); - - /** \brief Checks if a path has a joint-space jump, and truncates the path at the jump. - - Checks if a path has a jump larger than `jump_threshold` and truncates the path to the waypoint right before the - jump. Returns the percentage of the path that doesn't have jumps. - - @param group The joint model group of the robot state. - @param path The path that should be tested. - @param jump_threshold The struct holding jump thresholds to determine if a joint space jump has occurred. - @return The fraction of the path that passed. - */ - static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector>& path, - const JumpThreshold& jump_threshold); -}; - -/** \brief Checks if a joint-space path has a jump larger than the given threshold. - - This function computes the distance between every pair of adjacent waypoints (for the given group) and checks if that - distance is larger than the threshold defined by `jump_threshold`. If so, it is considered that the path has a - jump, and the path index where the jump happens is returned as output. - Otherwise the function returns a nullopt. */ -std::optional hasJointSpaceJump(const std::vector& waypoints, - const moveit::core::JointModelGroup& group, - const moveit::core::JumpThreshold& jump_threshold); - -} // end of namespace core -} // end of namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp new file mode 100644 index 0000000000..f7843c8996 --- /dev/null +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp @@ -0,0 +1,333 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * Copyright (c) 2019, PickNik 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, Inc. 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, Mike Lautman */ + +#pragma once + +#include + +namespace moveit +{ +namespace core +{ +/** Struct defining linear and rotational precision */ +struct CartesianPrecision +{ + double translational = 0.001; //< max deviation in translation (meters) + double rotational = 0.01; //< max deviation in rotation (radians) + double max_resolution = 1e-5; //< max resolution for waypoints (fraction of total path) +}; + +/** \brief Struct with options for defining joint-space jump thresholds. */ +struct JumpThreshold +{ + JumpThreshold() = default; // default is equivalent to disabled(). + + /** \brief Do not define any jump threshold, i.e., disable joint-space jump detection. */ + static JumpThreshold disabled(); + + /** \brief Detect joint-space jumps relative to the average joint-space displacement along the path. + + The average joint-space distance between consecutive points in the path is computed. If any individual joint-space + motion delta is larger than the average distance by a factor of `relative_factor`, it is considered the path has a + jump. For instance, a `relative_factor` of 10.0 will detect joint increments larger than 10x the average increment + */ + static JumpThreshold relative(double relative_factor); + + /** \brief Detect joint-space jumps greater than the given absolute thresholds. + + `revolute` and `prismatic` are absolute joint displacement thresholds, in radians and meters respectively. + If any two consecutive waypoints have a joint-space distance larger than these values, the path has a jump. */ + static JumpThreshold absolute(double revolute, double prismatic); + + double relative_factor = 0.0; + double revolute = 0.0; // Radians + double prismatic = 0.0; // Meters + + // Deprecated constructors. Construct using the builder methods above. + [[deprecated("Use JumpThreshold::relative() instead.")]] JumpThreshold(double relative_factor); + [[deprecated("Use JumpThreshold::absolute() instead.")]] JumpThreshold(double revolute, double prismatic); +}; + +/** \brief Struct for containing max_step for computeCartesianPath + + Setting translation to zero will disable checking for translations. The same goes for rotation. + Initializing with only one value (translation) sets the rotation such that + 1 cm of allowed translation = 2 degrees of allowed rotation. */ +struct MaxEEFStep +{ + MaxEEFStep(double translation, double rotation) : translation(translation), rotation(rotation) + { + } + + MaxEEFStep(double step_size) : translation(step_size), rotation(3.5 * step_size) // 0.035 rad = 2 deg + { + } + + double translation; // Meters + double rotation; // Radians +}; + +class CartesianInterpolator +{ + // TODO(mlautman): Eventually, this planner should be moved out of robot_state + +public: + struct Percentage + { + // value must be in [0,1] + Percentage(double value) : value(value) + { + if (value < 0.0 || value > 1.0) + throw std::runtime_error("Percentage values must be between 0 and 1, inclusive"); + } + operator double() + { + return value; + } + double operator*() + { + return value; + } + Percentage operator*(const Percentage& p) + { + Percentage res(value * p.value); + return res; + } + double value; + }; + + struct Distance + { + Distance(double meters) : meters(meters) + { + } + operator double() + { + return meters; + } + double operator*() + { + return meters; + } + Distance operator*(const Percentage& p) + { + Distance res(meters * p.value); + return res; + } + double meters; + }; + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. + + The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. + This vector is assumed to be specified either in the global reference frame or in the local + reference frame of the link. + The resulting joint values are stored in the vector \e traj, one by one. The interpolation distance in + Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which + provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal + call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to + the distance that was achieved and for which corresponding states were added to the path. + + The struct CartesianPrecision specifies the precision to which the path should follow the Cartesian straight line. + If the deviation at the mid point of two consecutive waypoints is larger than the specified precision, another waypoint + will be inserted at that mid point. The precision is specified separately for translation and rotation. + The maximal resolution to consider (as fraction of the total path length) is specified by max_resolution. + */ + static Distance computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. + + In contrast to the previous function, the translation vector is specified as a (unit) direction vector and + a distance. */ + static Distance computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) + { + return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + precision, validCallback, options, cost_function); + } + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. + + In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) + for a virtual frame attached to the robot \e link with the given \e link_offset. + The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame. + This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */ + static Percentage computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Compute the sequence of joint values that perform a general Cartesian path. + + In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially + reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global + reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs + to move in a straight line between two consecutive waypoints. All other comments apply. */ + static Percentage computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. + + The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. + This vector is assumed to be specified either in the global reference frame or in the local + reference frame of the link (\e global_reference_frame is false). + The resulting joint values are stored in the vector \e path, one by one. The maximum distance in + Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which + provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal + call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to + the distance that was achieved and for which corresponding states were added to the path. At the end of the + function call, the state of the group corresponds to the last attempted Cartesian pose. + + During the computation of the path, it is usually preferred if consecutive joint values do not 'jump' by a + large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. + To account for this, the \e jump_threshold struct is provided, which comprises three fields: + \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. + If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. + If \e jump_threshold_factor is non-zero, we test for relative jumps. To this end, the average joint-space distance + between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger than + this average distance multiplied by \e jump_threshold_factor, this step is considered a jump. + + Otherwise (if all params are zero), jump detection is disabled. + If a jump is detected, the path is truncated up to just before the jump. + + Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e + cost_function. */ + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( + RobotState* start_state, const JointModelGroup* group, std::vector>& path, + const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, + const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. + + In contrast to the previous function, the translation vector is specified as a (unit) direction vector and + a distance. */ + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( + RobotState* start_state, const JointModelGroup* group, std::vector>& path, + const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, + const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) + { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step, + jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop + } + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. + + In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) + for a virtual frame attached to the robot \e link with the given \e link_offset. + The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame + (\e global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved. + All other comments from the previous function apply. */ + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( + RobotState* start_state, const JointModelGroup* group, std::vector>& path, + const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, + const JumpThreshold& jump_threshold, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Compute the sequence of joint values that perform a general Cartesian path. + + In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially + reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the + global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move + in a straight line between two consecutive waypoints. All other comments apply. */ + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( + RobotState* start_state, const JointModelGroup* group, std::vector>& path, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Checks if a path has a joint-space jump, and truncates the path at the jump. + + Checks if a path has a jump larger than `jump_threshold` and truncates the path to the waypoint right before the + jump. Returns the percentage of the path that doesn't have jumps. + + @param group The joint model group of the robot state. + @param path The path that should be tested. + @param jump_threshold The struct holding jump thresholds to determine if a joint space jump has occurred. + @return The fraction of the path that passed. + */ + static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector>& path, + const JumpThreshold& jump_threshold); +}; + +/** \brief Checks if a joint-space path has a jump larger than the given threshold. + + This function computes the distance between every pair of adjacent waypoints (for the given group) and checks if that + distance is larger than the threshold defined by `jump_threshold`. If so, it is considered that the path has a + jump, and the path index where the jump happens is returned as output. + Otherwise the function returns a nullopt. */ +std::optional hasJointSpaceJump(const std::vector& waypoints, + const moveit::core::JointModelGroup& group, + const moveit::core::JumpThreshold& jump_threshold); + +} // end of namespace core +} // end of namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h index 833ad3e464..a6817510c8 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,109 +47,5 @@ /* Author: Ioan Sucan, Dave Coleman */ #pragma once - -#include -#include -#include - -namespace moveit -{ -namespace core -{ -/** - * @brief Convert a joint state to a MoveIt robot state - * @param joint_state The input joint state to be converted - * @param state The resultant MoveIt robot state - * @return True if successful, false if failed for any reason - */ -bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state); - -/** - * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state - * @param tf An instance of a transforms object - * @param robot_state The input robot state msg - * @param state The resultant MoveIt robot state - * @param copy_attached_bodies Flag to include attached objects in robot state copy - * @return True if successful, false if failed for any reason - */ -bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::RobotState& robot_state, RobotState& state, - bool copy_attached_bodies = true); - -/** - * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state - * @param robot_state The input robot state msg - * @param state The resultant MoveIt robot state - * @param copy_attached_bodies Flag to include attached objects in robot state copy - * @return True if successful, false if failed for any reason - */ -bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, - bool copy_attached_bodies = true); - -/** - * @brief Convert a MoveIt robot state to a robot state message - * @param state The input MoveIt robot state object - * @param robot_state The resultant RobotState *message - * @param copy_attached_bodies Flag to include attached objects in robot state copy - */ -void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotState& robot_state, - bool copy_attached_bodies = true); - -/** - * @brief Convert AttachedBodies to AttachedCollisionObjects - * @param attached_bodies The input MoveIt attached body objects - * @param attached_collision_objs The resultant AttachedCollisionObject messages - */ -void attachedBodiesToAttachedCollisionObjectMsgs( - const std::vector& attached_bodies, - std::vector& attached_collision_objs); -/** - * @brief Convert a MoveIt robot state to a joint state message - * @param state The input MoveIt robot state object - * @param robot_state The resultant JointState message - */ -void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state); - -/** - * @brief Convert a joint trajectory point to a MoveIt robot state - * @param joint_trajectory The input msg - * @param point_id The index of the trajectory point in the joint trajectory. - * @param state The resultant MoveIt robot state - * @return True if successful, false if failed for any reason - */ -bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id, - RobotState& state); - -/** - * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is - * outputted to a stream e.g. for file saving - * @param state - The input MoveIt robot state object - * @param out - a file stream, or any other stream - * @param include_header - flag to prefix the output with a line of joint names. - * @param separator - allows to override the comma separator with any symbol, such as a white space - */ -void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header = true, - const std::string& separator = ","); - -/** - * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is - * outputted to a stream e.g. for file saving. This version can order by joint model groups - * @param state - The input MoveIt robot state object - * @param out - a file stream, or any other stream - * @param joint_group_ordering - output joints based on ordering of joint groups - * @param include_header - flag to prefix the output with a line of joint names. - * @param separator - allows to override the comma separator with any symbol, such as a white space - */ -void robotStateToStream(const RobotState& state, std::ostream& out, - const std::vector& joint_groups_ordering, bool include_header = true, - const std::string& separator = ","); - -/** - * \brief Convert a string of joint values from a file (CSV) or input source into a RobotState - * @param state - the output MoveIt robot state object - * @param line - the input string of joint values - * @param separator - allows to override the comma separator with any symbol, such as a white space - * \return true on success - */ -void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator = ","); -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.hpp b/moveit_core/robot_state/include/moveit/robot_state/conversions.hpp new file mode 100644 index 0000000000..4f369659a4 --- /dev/null +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.hpp @@ -0,0 +1,143 @@ +/********************************************************************* + * 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 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, Dave Coleman */ + +#pragma once + +#include +#include +#include + +namespace moveit +{ +namespace core +{ +/** + * @brief Convert a joint state to a MoveIt robot state + * @param joint_state The input joint state to be converted + * @param state The resultant MoveIt robot state + * @return True if successful, false if failed for any reason + */ +bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state); + +/** + * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state + * @param tf An instance of a transforms object + * @param robot_state The input robot state msg + * @param state The resultant MoveIt robot state + * @param copy_attached_bodies Flag to include attached objects in robot state copy + * @return True if successful, false if failed for any reason + */ +bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::RobotState& robot_state, RobotState& state, + bool copy_attached_bodies = true); + +/** + * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state + * @param robot_state The input robot state msg + * @param state The resultant MoveIt robot state + * @param copy_attached_bodies Flag to include attached objects in robot state copy + * @return True if successful, false if failed for any reason + */ +bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, + bool copy_attached_bodies = true); + +/** + * @brief Convert a MoveIt robot state to a robot state message + * @param state The input MoveIt robot state object + * @param robot_state The resultant RobotState *message + * @param copy_attached_bodies Flag to include attached objects in robot state copy + */ +void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotState& robot_state, + bool copy_attached_bodies = true); + +/** + * @brief Convert AttachedBodies to AttachedCollisionObjects + * @param attached_bodies The input MoveIt attached body objects + * @param attached_collision_objs The resultant AttachedCollisionObject messages + */ +void attachedBodiesToAttachedCollisionObjectMsgs( + const std::vector& attached_bodies, + std::vector& attached_collision_objs); +/** + * @brief Convert a MoveIt robot state to a joint state message + * @param state The input MoveIt robot state object + * @param robot_state The resultant JointState message + */ +void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state); + +/** + * @brief Convert a joint trajectory point to a MoveIt robot state + * @param joint_trajectory The input msg + * @param point_id The index of the trajectory point in the joint trajectory. + * @param state The resultant MoveIt robot state + * @return True if successful, false if failed for any reason + */ +bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id, + RobotState& state); + +/** + * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is + * outputted to a stream e.g. for file saving + * @param state - The input MoveIt robot state object + * @param out - a file stream, or any other stream + * @param include_header - flag to prefix the output with a line of joint names. + * @param separator - allows to override the comma separator with any symbol, such as a white space + */ +void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header = true, + const std::string& separator = ","); + +/** + * @brief Convert a MoveIt robot state to common separated values (CSV) on a single line that is + * outputted to a stream e.g. for file saving. This version can order by joint model groups + * @param state - The input MoveIt robot state object + * @param out - a file stream, or any other stream + * @param joint_group_ordering - output joints based on ordering of joint groups + * @param include_header - flag to prefix the output with a line of joint names. + * @param separator - allows to override the comma separator with any symbol, such as a white space + */ +void robotStateToStream(const RobotState& state, std::ostream& out, + const std::vector& joint_groups_ordering, bool include_header = true, + const std::string& separator = ","); + +/** + * \brief Convert a string of joint values from a file (CSV) or input source into a RobotState + * @param state - the output MoveIt robot state object + * @param line - the input string of joint values + * @param separator - allows to override the comma separator with any symbol, such as a white space + * \return true on success + */ +void streamToRobotState(RobotState& state, const std::string& line, const std::string& separator = ","); +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index cdac228288..5b7867273f 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,1718 +48,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/* Terminology - * Model Frame: RobotModel's root frame == PlanningScene's planning frame - If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint. - Otherwise, it is the root link of the URDF model. - * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics -*/ - -namespace moveit -{ -namespace core -{ -MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc - -/** \brief Signature for functions that can verify that if the group \e joint_group in \e robot_state is set to \e - joint_group_variable_values - the state is valid or not. Returns true if the state is valid. This call is allowed to modify \e robot_state (e.g., - set \e joint_group_variable_values) */ -typedef std::function - GroupStateValidityCallbackFn; - -/** \brief Representation of a robot's state. This includes position, - velocity, acceleration and effort. - - At the lowest level, a state is a collection of variables. Each - variable has a name and can have position, velocity, acceleration - and effort associated to it. Effort and acceleration share the - memory area for efficiency reasons (one should not set both - acceleration and effort in the same state and expect things to - work). Often variables correspond to joint names as well (joints - with one degree of freedom have one variable), but joints with - multiple degrees of freedom have more variables. Operations are - allowed at variable level, joint level (see JointModel) and joint - group level (see JointModelGroup). - - For efficiency reasons a state computes forward kinematics in a - lazy fashion. This can sometimes lead to problems if the update() - function was not called on the state. */ -class RobotState -{ -public: - /** \brief A state can be constructed from a specified robot model. No values are initialized. - Call setToDefaultValues() if a state needs to provide valid information. */ - RobotState(const RobotModelConstPtr& robot_model); - ~RobotState(); - - /** \brief Copy constructor. */ - RobotState(const RobotState& other); - - /** \brief Copy operator */ - RobotState& operator=(const RobotState& other); - - /** \brief Get the robot model this state is constructed for. */ - const RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - /** \brief Get the number of variables that make up this state. */ - std::size_t getVariableCount() const - { - return robot_model_->getVariableCount(); - } - - /** \brief Get the names of the variables that make up this state, in the order they are stored in memory. */ - const std::vector& getVariableNames() const - { - return robot_model_->getVariableNames(); - } - - /** \brief Get the model of a particular link */ - const LinkModel* getLinkModel(const std::string& link) const - { - return robot_model_->getLinkModel(link); - } - - /** \brief Get the model of a particular joint */ - const JointModel* getJointModel(const std::string& joint) const - { - return robot_model_->getJointModel(joint); - } - - /** \brief Get the model of a particular joint group */ - const JointModelGroup* getJointModelGroup(const std::string& group) const - { - return robot_model_->getJointModelGroup(group); - } - - /** \name Getting and setting variable position - * @{ - */ - - /** \brief Get a raw pointer to the positions of the variables - stored in this state. Use carefully. If you change these values - externally you need to make sure you trigger a forced update for - the state by calling update(true). */ - double* getVariablePositions() - { - return position_.data(); - } - - /** \brief Get a raw pointer to the positions of the variables - stored in this state. */ - const double* getVariablePositions() const - { - return position_.data(); - } - - /** \brief It is assumed \e positions is an array containing the new - positions for all variables in this state. Those values are - copied into the state. */ - void setVariablePositions(const double* position); - - /** \brief It is assumed \e positions is an array containing the new - positions for all variables in this state. Those values are - copied into the state. */ - void setVariablePositions(const std::vector& position) - { - assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode - setVariablePositions(&position[0]); - } - - /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. - */ - void setVariablePositions(const std::map& variable_map); - - /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. - Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariablePositions(const std::map& variable_map, - std::vector& missing_variables); - - /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. - Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariablePositions(const std::vector& variable_names, - const std::vector& variable_position); - - /** \brief Set the position of a single variable. An exception is thrown if the variable name is not known */ - void setVariablePosition(const std::string& variable, double value) - { - setVariablePosition(robot_model_->getVariableIndex(variable), value); - } - - /** \brief Set the position of a single variable. The variable is specified by its index (a value associated by the - * RobotModel to each variable) */ - void setVariablePosition(int index, double value) - { - position_[index] = value; - const JointModel* jm = robot_model_->getJointOfVariable(index); - if (jm) - { - markDirtyJointTransforms(jm); - updateMimicJoint(jm); - } - } - - /** \brief Get the position of a particular variable. An exception is thrown if the variable is not known. */ - double getVariablePosition(const std::string& variable) const - { - return position_[robot_model_->getVariableIndex(variable)]; - } - - /** \brief Get the position of a particular variable. The variable is - specified by its index. No checks are performed for the validity - of the index passed */ - double getVariablePosition(int index) const - { - return position_[index]; - } - - /** @} */ - - /** \name Getting and setting variable velocity - * @{ - */ - - /** \brief By default, if velocities are never set or initialized, - the state remembers that there are no velocities set. This is - useful to know when serializing or copying the state.*/ - bool hasVelocities() const - { - return has_velocity_; - } - - /** \brief Get raw access to the velocities of the variables that make up this state. The values are in the same order - * as reported by getVariableNames() */ - double* getVariableVelocities() - { - markVelocity(); - return velocity_.data(); - } - - /** \brief Get const access to the velocities of the variables that make up this state. The values are in the same - * order as reported by getVariableNames() */ - const double* getVariableVelocities() const - { - return velocity_.data(); - } - - /** \brief Set all velocities to 0.0 */ - void zeroVelocities(); - - /** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */ - void setVariableVelocities(const double* velocity) - { - has_velocity_ = true; - // assume everything is in order in terms of array lengths (for efficiency reasons) - memcpy(velocity_.data(), velocity, robot_model_->getVariableCount() * sizeof(double)); - } - - /** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */ - void setVariableVelocities(const std::vector& velocity) - { - assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode - setVariableVelocities(&velocity[0]); - } - - /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. - */ - void setVariableVelocities(const std::map& variable_map); - - /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. - Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariableVelocities(const std::map& variable_map, - std::vector& missing_variables); - - /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. - */ - void setVariableVelocities(const std::vector& variable_names, - const std::vector& variable_velocity); - - /** \brief Set the velocity of a variable. If an unknown variable name is specified, an exception is thrown. */ - void setVariableVelocity(const std::string& variable, double value) - { - setVariableVelocity(robot_model_->getVariableIndex(variable), value); - } - - /** \brief Set the velocity of a single variable. The variable is specified by its index (a value associated by the - * RobotModel to each variable) */ - void setVariableVelocity(int index, double value) - { - markVelocity(); - velocity_[index] = value; - } - - /** \brief Get the velocity of a particular variable. An exception is thrown if the variable is not known. */ - double getVariableVelocity(const std::string& variable) const - { - return velocity_[robot_model_->getVariableIndex(variable)]; - } - - /** \brief Get the velocity of a particular variable. The variable is - specified by its index. No checks are performed for the validity - of the index passed */ - double getVariableVelocity(int index) const - { - return velocity_[index]; - } - - /** \brief Remove velocities from this state (this differs from setting them to zero) */ - void dropVelocities(); - - /** @} */ - - /** \name Getting and setting variable acceleration - * @{ - */ - - /** \brief By default, if accelerations are never set or initialized, the state remembers that there are no - accelerations set. This is - useful to know when serializing or copying the state. If hasAccelerations() reports true, hasEffort() will - certainly report false. */ - bool hasAccelerations() const - { - return has_acceleration_; - } - - /** \brief Get raw access to the accelerations of the variables that make up this state. The values are in the same - * order as reported by getVariableNames(). The area of memory overlaps with effort (effort and acceleration should - * not be set at the same time) */ - double* getVariableAccelerations() - { - markAcceleration(); - return effort_or_acceleration_.data(); - } - - /** \brief Get const raw access to the accelerations of the variables that make up this state. The values are in the - * same order as reported by getVariableNames() */ - const double* getVariableAccelerations() const - { - return effort_or_acceleration_.data(); - } - - /** \brief Set all accelerations to 0.0 */ - void zeroAccelerations(); - - /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this - * state */ - void setVariableAccelerations(const double* acceleration) - { - has_acceleration_ = true; - has_effort_ = false; - - // assume everything is in order in terms of array lengths (for efficiency reasons) - memcpy(effort_or_acceleration_.data(), acceleration, robot_model_->getVariableCount() * sizeof(double)); - } - - /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this - * state */ - void setVariableAccelerations(const std::vector& acceleration) - { - assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode - setVariableAccelerations(&acceleration[0]); - } - - /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is - * thrown. */ - void setVariableAccelerations(const std::map& variable_map); - - /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is - thrown. - Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariableAccelerations(const std::map& variable_map, - std::vector& missing_variables); - - /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is - * thrown. */ - void setVariableAccelerations(const std::vector& variable_names, - const std::vector& variable_acceleration); - - /** \brief Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown. */ - void setVariableAcceleration(const std::string& variable, double value) - { - setVariableAcceleration(robot_model_->getVariableIndex(variable), value); - } - - /** \brief Set the acceleration of a single variable. The variable is specified by its index (a value associated by - * the RobotModel to each variable) */ - void setVariableAcceleration(int index, double value) - { - markAcceleration(); - effort_or_acceleration_[index] = value; - } - - /** \brief Get the acceleration of a particular variable. An exception is thrown if the variable is not known. */ - double getVariableAcceleration(const std::string& variable) const - { - return effort_or_acceleration_[robot_model_->getVariableIndex(variable)]; - } - - /** \brief Get the acceleration of a particular variable. The variable is - specified by its index. No checks are performed for the validity - of the index passed */ - double getVariableAcceleration(int index) const - { - return effort_or_acceleration_[index]; - } - - /** \brief Remove accelerations from this state (this differs from setting them to zero) */ - void dropAccelerations(); - - /** @} */ - - /** \name Getting and setting variable effort - * @{ - */ - - /** \brief By default, if effort is never set or initialized, the state remembers that there is no effort set. This is - useful to know when serializing or copying the state. If hasEffort() reports true, hasAccelerations() will - certainly report false. */ - bool hasEffort() const - { - return has_effort_; - } - - /** \brief Get raw access to the effort of the variables that make up this state. The values are in the same order as - * reported by getVariableNames(). The area of memory overlaps with accelerations (effort and acceleration should not - * be set at the same time) */ - double* getVariableEffort() - { - markEffort(); - return effort_or_acceleration_.data(); - } - - /** \brief Get const raw access to the effort of the variables that make up this state. The values are in the same - * order as reported by getVariableNames(). */ - const double* getVariableEffort() const - { - return effort_or_acceleration_.data(); - } - - /** \brief Set all effort values to 0.0 */ - void zeroEffort(); - - /** \brief Given an array with effort values for all variables, set those values as the effort in this state */ - void setVariableEffort(const double* effort) - { - has_effort_ = true; - has_acceleration_ = false; - // assume everything is in order in terms of array lengths (for efficiency reasons) - memcpy(effort_or_acceleration_.data(), effort, robot_model_->getVariableCount() * sizeof(double)); - } - - /** \brief Given an array with effort values for all variables, set those values as the effort in this state */ - void setVariableEffort(const std::vector& effort) - { - assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode - setVariableEffort(&effort[0]); - } - - /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. */ - void setVariableEffort(const std::map& variable_map); - - /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. - Additionally, \e missing_variables is filled with the names of the variables that are not set. */ - void setVariableEffort(const std::map& variable_map, std::vector& missing_variables); - - /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. */ - void setVariableEffort(const std::vector& variable_names, - const std::vector& variable_acceleration); - - /** \brief Set the effort of a variable. If an unknown variable name is specified, an exception is thrown. */ - void setVariableEffort(const std::string& variable, double value) - { - setVariableEffort(robot_model_->getVariableIndex(variable), value); - } - - /** \brief Set the effort of a single variable. The variable is specified by its index (a value associated by the - * RobotModel to each variable) */ - void setVariableEffort(int index, double value) - { - markEffort(); - effort_or_acceleration_[index] = value; - } - - /** \brief Get the effort of a particular variable. An exception is thrown if the variable is not known. */ - double getVariableEffort(const std::string& variable) const - { - return effort_or_acceleration_[robot_model_->getVariableIndex(variable)]; - } - - /** \brief Get the effort of a particular variable. The variable is - specified by its index. No checks are performed for the validity - of the index passed */ - double getVariableEffort(int index) const - { - return effort_or_acceleration_[index]; - } - - /** \brief Remove effort values from this state (this differs from setting them to zero) */ - void dropEffort(); - - /** \brief Reduce RobotState to kinematic information (remove velocity, acceleration and effort, if present) */ - void dropDynamics(); - - /** \brief Invert velocity if present. */ - void invertVelocity(); - - /** @} */ - - /** \name Getting and setting joint positions, velocities, accelerations and effort for a single joint - * The joint might be multi-DOF, i.e. require more than one variable to set. - * See setVariablePositions(), setVariableVelocities(), setVariableEffort() to handle multiple joints. - * @{ - */ - void setJointPositions(const std::string& joint_name, const double* position) - { - setJointPositions(robot_model_->getJointModel(joint_name), position); - } - - void setJointPositions(const std::string& joint_name, const std::vector& position) - { - setJointPositions(robot_model_->getJointModel(joint_name), &position[0]); - } - - void setJointPositions(const JointModel* joint, const std::vector& position) - { - setJointPositions(joint, &position[0]); - } - - void setJointPositions(const JointModel* joint, const double* position); - - void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform) - { - setJointPositions(robot_model_->getJointModel(joint_name), transform); - } - - void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform); - - void setJointVelocities(const JointModel* joint, const double* velocity); - - void setJointEfforts(const JointModel* joint, const double* effort); - - const double* getJointPositions(const std::string& joint_name) const - { - return getJointPositions(robot_model_->getJointModel(joint_name)); - } - - // Returns nullptr if `joint` doesn't have any active variables. - const double* getJointPositions(const JointModel* joint) const; - - const double* getJointVelocities(const std::string& joint_name) const - { - return getJointVelocities(robot_model_->getJointModel(joint_name)); - } - - // Returns nullptr if `joint` doesn't have any active variables. - const double* getJointVelocities(const JointModel* joint) const; - - const double* getJointAccelerations(const std::string& joint_name) const - { - return getJointAccelerations(robot_model_->getJointModel(joint_name)); - } - - // Returns nullptr if `joint` doesn't have any active variables. - const double* getJointAccelerations(const JointModel* joint) const; - - const double* getJointEffort(const std::string& joint_name) const - { - return getJointEffort(robot_model_->getJointModel(joint_name)); - } - - // Returns nullptr if `joint` doesn't have any active variables. - const double* getJointEffort(const JointModel* joint) const; - - /** @} */ - - /** \name Getting and setting group positions - * @{ - */ - - /** \brief Given positions for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupPositions(const std::string& joint_group_name, const double* gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupPositions(jmg, gstate); - } - - /** \brief Given positions for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupPositions(const std::string& joint_group_name, const std::vector& gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - assert(gstate.size() == jmg->getVariableCount()); - setJointGroupPositions(jmg, &gstate[0]); - } - } - - /** \brief Given positions for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) - { - assert(gstate.size() == group->getVariableCount()); - setJointGroupPositions(group, &gstate[0]); - } - - /** \brief Given positions for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupPositions(const JointModelGroup* group, const double* gstate); - - /** \brief Given positions for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - assert(values.size() == jmg->getVariableCount()); - setJointGroupPositions(jmg, values); - } - } - - /** \brief Given positions for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values); - - /** \brief Given positions for the variables of active joints that make up a group, - * in the order found in the group (excluding values of mimic joints), set those - * as the new values that correspond to the group */ - void setJointGroupActivePositions(const JointModelGroup* group, const std::vector& gstate); - - /** \brief Given positions for the variables of active joints that make up a group, - * in the order found in the group (excluding values of mimic joints), set those - * as the new values that correspond to the group */ - void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector& gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - assert(gstate.size() == jmg->getActiveVariableCount()); - setJointGroupActivePositions(jmg, gstate); - } - } - - /** \brief Given positions for the variables of active joints that make up a group, - * in the order found in the group (excluding values of mimic joints), set those - * as the new values that correspond to the group */ - void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values); - - /** \brief Given positions for the variables of active joints that make up a group, - * in the order found in the group (excluding values of mimic joints), set those - * as the new values that correspond to the group */ - void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - assert(values.size() == jmg->getActiveVariableCount()); - setJointGroupActivePositions(jmg, values); - } - } - - /** \brief For a given group, copy the position values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupPositions(const std::string& joint_group_name, std::vector& gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - gstate.resize(jmg->getVariableCount()); - copyJointGroupPositions(jmg, &gstate[0]); - } - } - - /** \brief For a given group, copy the position values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupPositions(jmg, gstate); - } - - /** \brief For a given group, copy the position values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupPositions(const JointModelGroup* group, std::vector& gstate) const - { - gstate.resize(group->getVariableCount()); - copyJointGroupPositions(group, &gstate[0]); - } - - /** \brief For a given group, copy the position values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const; - - /** \brief For a given group, copy the position values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupPositions(jmg, values); - } - - /** \brief For a given group, copy the position values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const; - - /** @} */ - - /** \name Getting and setting group velocities - * @{ - */ - - /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupVelocities(jmg, gstate); - } - - /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupVelocities(const std::string& joint_group_name, const std::vector& gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupVelocities(jmg, &gstate[0]); - } - - /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupVelocities(const JointModelGroup* group, const std::vector& gstate) - { - setJointGroupVelocities(group, &gstate[0]); - } - - /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupVelocities(const JointModelGroup* group, const double* gstate); - - /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupVelocities(jmg, values); - } - - /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values - * of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values); - - /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupVelocities(const std::string& joint_group_name, std::vector& gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - gstate.resize(jmg->getVariableCount()); - copyJointGroupVelocities(jmg, &gstate[0]); - } - } - - /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupVelocities(jmg, gstate); - } - - /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupVelocities(const JointModelGroup* group, std::vector& gstate) const - { - gstate.resize(group->getVariableCount()); - copyJointGroupVelocities(group, &gstate[0]); - } - - /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const; - - /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupVelocities(jmg, values); - } - - /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, - * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the - * RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const; - - /** @} */ - - /** \name Getting and setting group accelerations - * @{ - */ - - /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including - * values of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupAccelerations(jmg, gstate); - } - - /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including - * values of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector& gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupAccelerations(jmg, &gstate[0]); - } - - /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including - * values of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupAccelerations(const JointModelGroup* group, const std::vector& gstate) - { - setJointGroupAccelerations(group, &gstate[0]); - } - - /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including - * values of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate); - - /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including - * values of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupAccelerations(jmg, values); - } - - /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including - * values of mimic joints), set those as the new values that correspond to the group */ - void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values); - - /** \brief For a given group, copy the acceleration values of the variables that make up the group into another - * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of - * memory in the RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector& gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - { - gstate.resize(jmg->getVariableCount()); - copyJointGroupAccelerations(jmg, &gstate[0]); - } - } - - /** \brief For a given group, copy the acceleration values of the variables that make up the group into another - * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of - * memory in the RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupAccelerations(jmg, gstate); - } - - /** \brief For a given group, copy the acceleration values of the variables that make up the group into another - * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of - * memory in the RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupAccelerations(const JointModelGroup* group, std::vector& gstate) const - { - gstate.resize(group->getVariableCount()); - copyJointGroupAccelerations(group, &gstate[0]); - } - - /** \brief For a given group, copy the acceleration values of the variables that make up the group into another - * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of - * memory in the RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const; - - /** \brief For a given group, copy the acceleration values of the variables that make up the group into another - * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of - * memory in the RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupAccelerations(jmg, values); - } - - /** \brief For a given group, copy the acceleration values of the variables that make up the group into another - * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of - * memory in the RobotState itself, so we copy instead of returning a pointer.*/ - void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const; - - /** @} */ - - /** \name Setting from Inverse Kinematics - * @{ - */ - - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be - set by computing inverse kinematics. - The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. - @param pose The pose the last link in the chain needs to achieve - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be - set by computing inverse kinematics. - The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. - @param pose The pose the \e tip link in the chain needs to achieve - @param tip The name of the link the pose is specified for - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be - set by computing inverse kinematics. - The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. - @param pose The pose the last link in the chain needs to achieve - @param tip The name of the link the pose is specified for - @param timeout The timeout passed to the kinematics solver on each attempt */ - bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be - set by computing inverse kinematics. - The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. - @param pose The pose the last link in the chain needs to achieve - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be - set by computing inverse kinematics. - The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. - @param pose The pose the last link in the chain needs to achieve - @param tip The name of the frame for which IK is attempted. - @param consistency_limits This specifies the desired distance between the solution and the seed state - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, - const std::vector& consistency_limits, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief Warning: This function inefficiently copies all transforms around. - If the group consists of a set of sub-groups that are each a chain and a solver - is available for each sub-group, then the joint values can be set by computing inverse kinematics. - The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed - to be in the same order as the order of the sub-groups in this group. Returns true on success. - @param poses The poses the last link in each chain needs to achieve - @param tips The names of the frames for which IK is attempted. - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** \brief Warning: This function inefficiently copies all transforms around. - If the group consists of a set of sub-groups that are each a chain and a solver - is available for each sub-group, then the joint values can be set by computing inverse kinematics. - The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed - to be in the same order as the order of the sub-groups in this group. Returns true on success. - @param poses The poses the last link in each chain needs to achieve - @param tips The names of the frames for which IK is attempted. - @param consistency_limits This specifies the desired distance between the solution and the seed state - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, const std::vector>& consistency_limits, - double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); - - /** - \brief setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for - non-chain kinematics. In this case, we divide the group into subgroups and do IK solving individually - @param poses The poses the last link in each chain needs to achieve - @param tips The names of the frames for which IK is attempted. - @param consistency_limits This specifies the desired distance between the solution and the seed state - @param timeout The timeout passed to the kinematics solver on each attempt - @param constraint A state validity constraint to be required for IK solutions */ - bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, - const std::vector& tips, - const std::vector>& consistency_limits, double timeout = 0.0, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - - /** \brief Set the joint values from a Cartesian velocity applied during a time dt - * @param group the group of joints this function operates on - * @param twist a Cartesian velocity on the 'tip' frame - * @param tip the frame for which the twist is given - * @param dt a time interval (seconds) - * @param st a secondary task computation function - */ - bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); - - /** \brief Set the joint values from a Cartesian velocity applied during a time dt - * @param group the group of joints this function operates on - * @param twist a Cartesian velocity on the 'tip' frame - * @param tip the frame for which the twist is given - * @param dt a time interval (seconds) - * @param st a secondary task computation function - */ - bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip, - double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); - - /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. - * \param group The group to compute the Jacobian for - * \param link The link model to compute the Jacobian for - * \param reference_point_position The reference point position (with respect to the link specified in link) - * \param jacobian The resultant jacobian, with the origin at the group root link. - * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation - * (default is false) - * \return True if jacobian was successfully computed, false otherwise - */ - bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position, - Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const; - - /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. - * \param group The group to compute the Jacobian for - * \param link The link model to compute the Jacobian for - * \param reference_point_position The reference point position (with respect to the link specified in link) - * \param jacobian The resultant jacobian, with the origin at the group root link. - * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation - * (default is false) - * \return True if jacobian was successfully computed, false otherwise - */ - bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position, - Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) - { - updateLinkTransforms(); - return static_cast(this)->getJacobian(group, link, reference_point_position, jacobian, - use_quaternion_representation); - } - - /** \brief Compute the Jacobian with reference to the last link of a specified group, and origin at the group root - * link. If the group is not a chain, an exception is thrown. - * \param group The group to compute the Jacobian for - * \param reference_point_position The reference point position (with respect to the link specified in link_name) - * \return The computed Jacobian. - */ - Eigen::MatrixXd getJacobian(const JointModelGroup* group, - const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const; - - /** \brief Compute the Jacobian with reference to the last link of a specified group, and origin at the group root - * link. If the group is not a chain, an exception is thrown. - * \param group The group to compute the Jacobian for - * \param reference_point_position The reference point position (with respect to the link specified in link_name) - * \return The computed Jacobian. - */ - Eigen::MatrixXd getJacobian(const JointModelGroup* group, - const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) - { - updateLinkTransforms(); - return static_cast(this)->getJacobian(group, reference_point_position); - } - - /** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and - * store it in \e qdot */ - void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, - const LinkModel* tip) const; - - /** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and - * store it in \e qdot */ - void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, - const LinkModel* tip) - { - updateLinkTransforms(); - static_cast(this)->computeVariableVelocity(jmg, qdot, twist, tip); - } - - /** \brief Given the velocities for the variables in this group (\e qdot) and an amount of time (\e dt), - update the current state using the Euler forward method. If the constraint specified is satisfied, return true, - otherwise return false. */ - bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt, - const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); - - /** @} */ - - /** \name Getting and setting whole states - * @{ - */ - - void setVariableValues(const sensor_msgs::msg::JointState& msg) - { - if (!msg.position.empty()) - setVariablePositions(msg.name, msg.position); - if (!msg.velocity.empty()) - setVariableVelocities(msg.name, msg.velocity); - } - - /** \brief Set all joints to their default positions. - The default position is 0, or if that is not within bounds then half way - between min and max bound. */ - void setToDefaultValues(); - - /** \brief Set the joints in \e group to the position \e name defined in the SRDF */ - bool setToDefaultValues(const JointModelGroup* group, const std::string& name); - - bool setToDefaultValues(const std::string& group_name, const std::string& state_name) - { - const JointModelGroup* jmg = getJointModelGroup(group_name); - if (jmg) - { - return setToDefaultValues(jmg, state_name); - } - else - { - return false; - } - } - - /** \brief Set all joints to random values. Values will be within default bounds. */ - void setToRandomPositions(); - - /** \brief Set all joints in \e group to random values. Values will be within default bounds. */ - void setToRandomPositions(const JointModelGroup* group); - - /** \brief Set all joints in \e group to random values using a specified random number generator. - Values will be within default bounds. */ - void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng); - - /** \brief Set all joints in \e group to random values near the value in \e seed. - * \e distance is the maximum amount each joint value will vary from the - * corresponding value in \e seed. \distance represents meters for - * prismatic/positional joints and radians for revolute/orientation joints. - * Resulting values are clamped within default bounds. */ - void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance); - - /** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number - * generator. \e distance is the maximum amount each joint value will vary from the corresponding value in \e seed. - * \distance represents meters for prismatic/positional joints and radians for revolute/orientation joints. Resulting - * values are clamped within default bounds. */ - void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance, - random_numbers::RandomNumberGenerator& rng); - - /** \brief Set all joints in \e group to random values near the value in \e seed. - * \e distances \b MUST have the same size as \c - * group.getActiveJointModels(). Each value in \e distances is the maximum - * amount the corresponding active joint in \e group will vary from the - * corresponding value in \e seed. \distance represents meters for - * prismatic/positional joints and radians for revolute/orientation joints. - * Resulting values are clamped within default bounds. */ - void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, - const std::vector& distances); - - /** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number - * generator. \e distances \b MUST have the same size as \c group.getActiveJointModels(). Each value in \e distances - * is the maximum amount the corresponding active joint in \e group will vary from the corresponding value in \e seed. - * \distance represents meters for prismatic/positional joints and radians for revolute/orientation joints. Resulting - * values are clamped within default bounds. */ - void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, - const std::vector& distances, random_numbers::RandomNumberGenerator& rng); - - /** @} */ - - /** \name Updating and getting transforms - * @{ - */ - - /** \brief Update the transforms for the collision bodies. This call is needed before calling collision checking. - If updating link transforms or joint transforms is needed, the corresponding updates are also triggered. */ - void updateCollisionBodyTransforms(); - - /** \brief Update the reference frame transforms for links. This call is needed before using the transforms of links - * for coordinate transforms. */ - void updateLinkTransforms(); - - /** \brief Update all transforms. */ - void update(bool force = false); - - /** \brief Update the state after setting a particular link to the input global transform pose. - - This "warps" the given link to the given pose, neglecting the joint values of its parent joint. - The link transforms of link and all its descendants are updated, but not marked as dirty, - although they do not match the joint values anymore! - Collision body transforms are not yet updated, but marked dirty only. - Use update(false) or updateCollisionBodyTransforms() to update them as well. - */ - void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false) - { - updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward); - } - - /** \brief Update the state after setting a particular link to the input global transform pose.*/ - void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false); - - /** \brief Get the latest link upwards the kinematic tree which is only connected via fixed joints. - * - * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel, - * but can additionally resolve parents for attached objects / subframes. - */ - const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const; - - /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. - * This is typically the root link of the URDF unless a virtual joint is present. - * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. - * A related, more comprehensive function is |getFrameTransform|, which additionally to link frames - * also searches for attached object frames and their subframes. - * - * This will throw an exception if the passed link is not found - * - * The returned transformation is always a valid isometry. - */ - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) - { - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); - } - - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) - { - if (!link) - { - throw Exception("Invalid link"); - } - updateLinkTransforms(); - return global_link_transforms_[link->getLinkIndex()]; - } - - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const - { - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); - } - - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const - { - if (!link) - { - throw Exception("Invalid link"); - } - assert(checkLinkTransforms()); - return global_link_transforms_[link->getLinkIndex()]; - } - - /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. - * This is typically the root link of the URDF unless a virtual joint is present. - * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. - * - * As opposed to the visual links in |getGlobalLinkTransform|, this function returns - * the collision link transform used for collision checking. - * - * @param link_name: name of link to lookup - * @param index: specify which collision body to lookup, if more than one exists - */ - const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) - { - return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) - { - updateCollisionBodyTransforms(); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const - { - return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); - } - - const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const - { - assert(checkCollisionTransforms()); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; - } - - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) - { - return getJointTransform(robot_model_->getJointModel(joint_name)); - } - - const Eigen::Isometry3d& getJointTransform(const JointModel* joint); - - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const - { - return getJointTransform(robot_model_->getJointModel(joint_name)); - } - - const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const - { - assert(checkJointTransforms(joint)); - return variable_joint_transforms_[joint->getJointIndex()]; - } - - bool dirtyJointTransform(const JointModel* joint) const - { - return dirty_joint_transforms_[joint->getJointIndex()]; - } - - bool dirtyLinkTransforms() const - { - return dirty_link_transforms_; - } - - bool dirtyCollisionBodyTransforms() const - { - return dirty_link_transforms_ || dirty_collision_body_transforms_; - } - - /** \brief Returns true if anything in this state is dirty */ - bool dirty() const - { - return dirtyCollisionBodyTransforms(); - } - - /** @} */ - - /** \name Computing distances - * @{ - */ - - /** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */ - double distance(const RobotState& other) const - { - return robot_model_->distance(position_.data(), other.getVariablePositions()); - } - - /** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */ - double distance(const RobotState& other, const JointModelGroup* joint_group) const; - - /** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */ - double distance(const RobotState& other, const JointModel* joint) const; - - /** - * Interpolate towards "to" state. Mimic joints are correctly updated and flags are set so that FK is recomputed - * when needed. - * - * @param to interpolate to this state - * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. - * @param state holds the result - */ - void interpolate(const RobotState& to, double t, RobotState& state) const; - - /** - * Interpolate towards "to" state, but only for the joints in the specified group. Mimic joints are correctly updated - * and flags are set so that FK is recomputed when needed. - * - * @param to interpolate to this state - * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. - * @param state holds the result - * @param joint_group interpolate only for the joints in this group - */ - void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const; - - /** - * Interpolate towards "to" state, but only for a single joint. Mimic joints are correctly updated - * and flags are set so that FK is recomputed when needed. - * - * @param to interpolate to this state - * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. - * @param state holds the result - * @param joint interpolate only for this joint - */ - void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const; - - void enforceBounds(); - void enforceBounds(const JointModelGroup* joint_group); - void enforceBounds(const JointModel* joint) - { - enforcePositionBounds(joint); - if (has_velocity_) - enforceVelocityBounds(joint); - } - void enforcePositionBounds(const JointModel* joint); - - /// Call harmonizePosition() for all joints / all joints in group / given joint - void harmonizePositions(); - void harmonizePositions(const JointModelGroup* joint_group); - void harmonizePosition(const JointModel* joint); - - void enforceVelocityBounds(const JointModel* joint); - - bool satisfiesBounds(double margin = 0.0) const; - bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const; - bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const - { - return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin)); - } - bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const - { - return joint->satisfiesPositionBounds(getJointPositions(joint), margin); - } - bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const - { - return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin); - } - - /** \brief Get the minimm distance from this state to the bounds. - The minimum distance and the joint for which this minimum is achieved are returned. */ - std::pair getMinDistanceToPositionBounds() const; - - /** \brief Get the minimm distance from a group in this state to the bounds. - The minimum distance and the joint for which this minimum is achieved are returned. */ - std::pair getMinDistanceToPositionBounds(const JointModelGroup* group) const; - - /** \brief Get the minimm distance from a set of joints in the state to the bounds. - The minimum distance and the joint for which this minimum is achieved are returned. */ - std::pair - getMinDistanceToPositionBounds(const std::vector& joints) const; - - /** - * \brief Check that the time to move between two waypoints is sufficient given velocity limits and time step - * \param other - robot state to compare joint positions against - * \param group - planning group to compare joint positions against - * \param dt - time step between the two points - */ - bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const; - - /** @} */ - - /** \name Managing attached bodies - * @{ - */ - - /** \brief Add an attached body to this state. - * - * This only adds the given body to this RobotState - * instance. It does not change anything about other - * representations of the object elsewhere in the system. So if the - * body represents an object in a collision_detection::World (like - * from a planning_scene::PlanningScene), you will likely need to remove the - * corresponding object from that world to avoid having collisions - * detected against it. - **/ - void attachBody(std::unique_ptr attached_body); - - /** @brief Add an attached body to a link - * @param id The string id associated with the attached body - * @param pose The pose associated with the attached body - * @param shapes The shapes that make up the attached body - * @param shape_poses The transforms between the object pose and the attached body's shapes - * @param touch_links The set of links that the attached body is allowed to touch - * @param link_name The link to attach to - * @param detach_posture The posture of the gripper when placing the object - * @param subframe_poses Transforms to points of interest on the object (can be used as end effector link) - * - * This only adds the given body to this RobotState - * instance. It does not change anything about other - * representations of the object elsewhere in the system. So if the - * body represents an object in a collision_detection::World (like - * from a planning_scene::PlanningScene), you will likely need to remove the - * corresponding object from that world to avoid having collisions - * detected against it. */ - void attachBody(const std::string& id, const Eigen::Isometry3d& pose, - const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, - const std::set& touch_links, const std::string& link_name, - const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(), - const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); - - /** @brief Add an attached body to a link - * @param id The string id associated with the attached body - * @param pose The pose associated with the attached body - * @param shapes The shapes that make up the attached body - * @param shape_poses The transforms between the object pose and the attached body's shapes - * @param touch_links The set of links that the attached body is allowed to touch - * @param link_name The link to attach to - * @param detach_posture The posture of the gripper when placing the object - * @param subframe_poses Transforms to points of interest on the object (can be used as end effector link) - * - * This only adds the given body to this RobotState - * instance. It does not change anything about other - * representations of the object elsewhere in the system. So if the - * body represents an object in a collision_detection::World (like - * from a planning_scene::PlanningScene), you will likely need to remove the - * corresponding object from that world to avoid having collisions - * detected against it. */ - void attachBody(const std::string& id, const Eigen::Isometry3d& pose, - const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, - const std::vector& touch_links, const std::string& link_name, - const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(), - const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()) - { - std::set touch_links_set(touch_links.begin(), touch_links.end()); - attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses); - } - - /** \brief Get all bodies attached to the model corresponding to this state */ - void getAttachedBodies(std::vector& attached_bodies) const; - - /** \brief Get all bodies attached to a particular group the model corresponding to this state */ - void getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const; - - /** \brief Get all bodies attached to a particular link in the model corresponding to this state */ - void getAttachedBodies(std::vector& attached_bodies, const LinkModel* link_model) const; - - /** \brief Remove the attached body named \e id. Return false if the object was not found (and thus not removed). - * Return true on success. */ - bool clearAttachedBody(const std::string& id); - - /** \brief Clear the bodies attached to a specific link */ - void clearAttachedBodies(const LinkModel* link); - - /** \brief Clear the bodies attached to a specific group */ - void clearAttachedBodies(const JointModelGroup* group); - - /** \brief Clear all attached bodies. This calls delete on the AttachedBody instances, if needed. */ - void clearAttachedBodies(); - - /** \brief Get the attached body named \e name. Return nullptr if not found. */ - const AttachedBody* getAttachedBody(const std::string& name) const; - - /** \brief Check if an attached body named \e id exists in this state */ - bool hasAttachedBody(const std::string& id) const; - - void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback); - /** @} */ - - /** \brief Compute an axis-aligned bounding box that contains the current state. - The format for \e aabb is (minx, maxx, miny, maxy, minz, maxz) */ - void computeAABB(std::vector& aabb) const; - - /** \brief Compute an axis-aligned bounding box that contains the current state. - The format for \e aabb is (minx, maxx, miny, maxy, minz, maxz) */ - void computeAABB(std::vector& aabb) - { - updateLinkTransforms(); - static_cast(this)->computeAABB(aabb); - } - - /** \brief Return the instance of a random number generator */ - random_numbers::RandomNumberGenerator& getRandomNumberGenerator() - { - if (!rng_) - rng_ = std::make_unique(); - return *rng_; - } - - /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id - * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. - * - * The returned transformation is always a valid isometry. */ - const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr); - - /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id - * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. - * - * The returned transformation is always a valid isometry. */ - const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; - - /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id - * - * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. - * - * The returned transformation is always a valid isometry. */ - const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, - bool& frame_found) const; - - /** \brief Check if a transformation matrix from the model frame (root of model) to frame \e frame_id is known */ - bool knowsFrameTransform(const std::string& frame_id) const; - - /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. - * @param arr The returned marker array - * @param link_names The list of link names for which the markers should be created. - * @param color The color for the marker - * @param ns The namespace for the markers - * @param dur The rclcpp::Duration for which the markers should stay visible - */ - void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, - const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, - bool include_attached = false) const; - - /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first. - * @param arr The returned marker array - * @param link_names The list of link names for which the markers should be created. - * @param color The color for the marker - * @param ns The namespace for the markers - * @param dur The rclcpp::Duration for which the markers should stay visible - */ - void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, - const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, - bool include_attached = false) - { - updateCollisionBodyTransforms(); - static_cast(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached); - } - - /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. - * @param arr The returned marker array - * @param link_names The list of link names for which the markers should be created. - */ - void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, - bool include_attached = false) const; - - /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first. - * @param arr The returned marker array - * @param link_names The list of link names for which the markers should be created. - */ - void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, - bool include_attached = false) - { - updateCollisionBodyTransforms(); - static_cast(this)->getRobotMarkers(arr, link_names, include_attached); - } - - void printStatePositions(std::ostream& out = std::cout) const; - - /** \brief Output to console the current state of the robot's joint limits */ - void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const; - - void printStateInfo(std::ostream& out = std::cout) const; - - void printTransforms(std::ostream& out = std::cout) const; - - void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const; - - void printDirtyInfo(std::ostream& out = std::cout) const; - - std::string getStateTreeString() const; - - /** - * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver - * @param pose - the input to change - * @param solver - a kin solver whose base frame is important to us - * @return true if no error - */ - bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver); - - /** - * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver - * @param pose - the input to change - * @param ik_frame - the name of frame of reference of base of ik solver - * @return true if no error - */ - bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame); - -private: - void allocMemory(); - void init(); - void copyFrom(const RobotState& other); - - void markDirtyJointTransforms(const JointModel* joint) - { - dirty_joint_transforms_[joint->getJointIndex()] = 1; - dirty_link_transforms_ = - dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); - } - - void markDirtyJointTransforms(const JointModelGroup* group) - { - for (const JointModel* jm : group->getActiveJointModels()) - dirty_joint_transforms_[jm->getJointIndex()] = 1; - dirty_link_transforms_ = dirty_link_transforms_ == nullptr ? - group->getCommonRoot() : - robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot()); - } - - void markVelocity(); - void markAcceleration(); - void markEffort(); - - void updateMimicJoint(const JointModel* joint); - - /** \brief Update all mimic joints within group */ - void updateMimicJoints(const JointModelGroup* group); - - void updateLinkTransformsInternal(const JointModel* start); - - void getMissingKeys(const std::map& variable_map, - std::vector& missing_variables) const; - void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const; - - /** \brief This function is only called in debug mode */ - bool checkJointTransforms(const JointModel* joint) const; - - /** \brief This function is only called in debug mode */ - bool checkLinkTransforms() const; - - /** \brief This function is only called in debug mode */ - bool checkCollisionTransforms() const; - - RobotModelConstPtr robot_model_; - - std::vector position_; - std::vector velocity_; - std::vector effort_or_acceleration_; - bool has_velocity_ = false; - bool has_acceleration_ = false; - bool has_effort_ = false; - - const JointModel* dirty_link_transforms_ = nullptr; - const JointModel* dirty_collision_body_transforms_ = nullptr; - - // All the following transform variables point into aligned memory. - // They are updated lazily, based on the flags in dirty_joint_transforms_ - // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_ - std::vector variable_joint_transforms_; ///< Local transforms of all joints - std::vector global_link_transforms_; ///< Transforms from model frame to link frame for each link - std::vector global_collision_body_transforms_; ///< Transforms from model frame to collision - ///< bodies - std::vector dirty_joint_transforms_; - - /** \brief All attached bodies that are part of this state, indexed by their name */ - std::map> attached_body_map_; - - /** \brief This event is called when there is a change in the attached bodies for this state; - The event specifies the body that changed and whether it was just attached or about to be detached. */ - AttachedBodyCallback attached_body_update_callback_; - - /** \brief For certain operations a state needs a random number generator. However, it may be slightly expensive - to allocate the random number generator if many state instances are generated. For this reason, the generator - is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but - call getRandomNumberGenerator() instead. */ - std::unique_ptr rng_; -}; - -/** \brief Operator overload for printing variable bounds to a stream */ -std::ostream& operator<<(std::ostream& out, const RobotState& s); -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp new file mode 100644 index 0000000000..7f2e3637f8 --- /dev/null +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp @@ -0,0 +1,1753 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, 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, Inc. 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/* Terminology + * Model Frame: RobotModel's root frame == PlanningScene's planning frame + If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint. + Otherwise, it is the root link of the URDF model. + * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics +*/ + +namespace moveit +{ +namespace core +{ +MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc + +/** \brief Signature for functions that can verify that if the group \e joint_group in \e robot_state is set to \e + joint_group_variable_values + the state is valid or not. Returns true if the state is valid. This call is allowed to modify \e robot_state (e.g., + set \e joint_group_variable_values) */ +typedef std::function + GroupStateValidityCallbackFn; + +/** \brief Representation of a robot's state. This includes position, + velocity, acceleration and effort. + + At the lowest level, a state is a collection of variables. Each + variable has a name and can have position, velocity, acceleration + and effort associated to it. Effort and acceleration share the + memory area for efficiency reasons (one should not set both + acceleration and effort in the same state and expect things to + work). Often variables correspond to joint names as well (joints + with one degree of freedom have one variable), but joints with + multiple degrees of freedom have more variables. Operations are + allowed at variable level, joint level (see JointModel) and joint + group level (see JointModelGroup). + + For efficiency reasons a state computes forward kinematics in a + lazy fashion. This can sometimes lead to problems if the update() + function was not called on the state. */ +class RobotState +{ +public: + /** \brief A state can be constructed from a specified robot model. No values are initialized. + Call setToDefaultValues() if a state needs to provide valid information. */ + RobotState(const RobotModelConstPtr& robot_model); + ~RobotState(); + + /** \brief Copy constructor. */ + RobotState(const RobotState& other); + + /** \brief Copy operator */ + RobotState& operator=(const RobotState& other); + + /** \brief Get the robot model this state is constructed for. */ + const RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + /** \brief Get the number of variables that make up this state. */ + std::size_t getVariableCount() const + { + return robot_model_->getVariableCount(); + } + + /** \brief Get the names of the variables that make up this state, in the order they are stored in memory. */ + const std::vector& getVariableNames() const + { + return robot_model_->getVariableNames(); + } + + /** \brief Get the model of a particular link */ + const LinkModel* getLinkModel(const std::string& link) const + { + return robot_model_->getLinkModel(link); + } + + /** \brief Get the model of a particular joint */ + const JointModel* getJointModel(const std::string& joint) const + { + return robot_model_->getJointModel(joint); + } + + /** \brief Get the model of a particular joint group */ + const JointModelGroup* getJointModelGroup(const std::string& group) const + { + return robot_model_->getJointModelGroup(group); + } + + /** \name Getting and setting variable position + * @{ + */ + + /** \brief Get a raw pointer to the positions of the variables + stored in this state. Use carefully. If you change these values + externally you need to make sure you trigger a forced update for + the state by calling update(true). */ + double* getVariablePositions() + { + return position_.data(); + } + + /** \brief Get a raw pointer to the positions of the variables + stored in this state. */ + const double* getVariablePositions() const + { + return position_.data(); + } + + /** \brief It is assumed \e positions is an array containing the new + positions for all variables in this state. Those values are + copied into the state. */ + void setVariablePositions(const double* position); + + /** \brief It is assumed \e positions is an array containing the new + positions for all variables in this state. Those values are + copied into the state. */ + void setVariablePositions(const std::vector& position) + { + assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode + setVariablePositions(&position[0]); + } + + /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. + */ + void setVariablePositions(const std::map& variable_map); + + /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. + Additionally, \e missing_variables is filled with the names of the variables that are not set. */ + void setVariablePositions(const std::map& variable_map, + std::vector& missing_variables); + + /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. + Additionally, \e missing_variables is filled with the names of the variables that are not set. */ + void setVariablePositions(const std::vector& variable_names, + const std::vector& variable_position); + + /** \brief Set the position of a single variable. An exception is thrown if the variable name is not known */ + void setVariablePosition(const std::string& variable, double value) + { + setVariablePosition(robot_model_->getVariableIndex(variable), value); + } + + /** \brief Set the position of a single variable. The variable is specified by its index (a value associated by the + * RobotModel to each variable) */ + void setVariablePosition(int index, double value) + { + position_[index] = value; + const JointModel* jm = robot_model_->getJointOfVariable(index); + if (jm) + { + markDirtyJointTransforms(jm); + updateMimicJoint(jm); + } + } + + /** \brief Get the position of a particular variable. An exception is thrown if the variable is not known. */ + double getVariablePosition(const std::string& variable) const + { + return position_[robot_model_->getVariableIndex(variable)]; + } + + /** \brief Get the position of a particular variable. The variable is + specified by its index. No checks are performed for the validity + of the index passed */ + double getVariablePosition(int index) const + { + return position_[index]; + } + + /** @} */ + + /** \name Getting and setting variable velocity + * @{ + */ + + /** \brief By default, if velocities are never set or initialized, + the state remembers that there are no velocities set. This is + useful to know when serializing or copying the state.*/ + bool hasVelocities() const + { + return has_velocity_; + } + + /** \brief Get raw access to the velocities of the variables that make up this state. The values are in the same order + * as reported by getVariableNames() */ + double* getVariableVelocities() + { + markVelocity(); + return velocity_.data(); + } + + /** \brief Get const access to the velocities of the variables that make up this state. The values are in the same + * order as reported by getVariableNames() */ + const double* getVariableVelocities() const + { + return velocity_.data(); + } + + /** \brief Set all velocities to 0.0 */ + void zeroVelocities(); + + /** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */ + void setVariableVelocities(const double* velocity) + { + has_velocity_ = true; + // assume everything is in order in terms of array lengths (for efficiency reasons) + memcpy(velocity_.data(), velocity, robot_model_->getVariableCount() * sizeof(double)); + } + + /** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */ + void setVariableVelocities(const std::vector& velocity) + { + assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode + setVariableVelocities(&velocity[0]); + } + + /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. + */ + void setVariableVelocities(const std::map& variable_map); + + /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. + Additionally, \e missing_variables is filled with the names of the variables that are not set. */ + void setVariableVelocities(const std::map& variable_map, + std::vector& missing_variables); + + /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. + */ + void setVariableVelocities(const std::vector& variable_names, + const std::vector& variable_velocity); + + /** \brief Set the velocity of a variable. If an unknown variable name is specified, an exception is thrown. */ + void setVariableVelocity(const std::string& variable, double value) + { + setVariableVelocity(robot_model_->getVariableIndex(variable), value); + } + + /** \brief Set the velocity of a single variable. The variable is specified by its index (a value associated by the + * RobotModel to each variable) */ + void setVariableVelocity(int index, double value) + { + markVelocity(); + velocity_[index] = value; + } + + /** \brief Get the velocity of a particular variable. An exception is thrown if the variable is not known. */ + double getVariableVelocity(const std::string& variable) const + { + return velocity_[robot_model_->getVariableIndex(variable)]; + } + + /** \brief Get the velocity of a particular variable. The variable is + specified by its index. No checks are performed for the validity + of the index passed */ + double getVariableVelocity(int index) const + { + return velocity_[index]; + } + + /** \brief Remove velocities from this state (this differs from setting them to zero) */ + void dropVelocities(); + + /** @} */ + + /** \name Getting and setting variable acceleration + * @{ + */ + + /** \brief By default, if accelerations are never set or initialized, the state remembers that there are no + accelerations set. This is + useful to know when serializing or copying the state. If hasAccelerations() reports true, hasEffort() will + certainly report false. */ + bool hasAccelerations() const + { + return has_acceleration_; + } + + /** \brief Get raw access to the accelerations of the variables that make up this state. The values are in the same + * order as reported by getVariableNames(). The area of memory overlaps with effort (effort and acceleration should + * not be set at the same time) */ + double* getVariableAccelerations() + { + markAcceleration(); + return effort_or_acceleration_.data(); + } + + /** \brief Get const raw access to the accelerations of the variables that make up this state. The values are in the + * same order as reported by getVariableNames() */ + const double* getVariableAccelerations() const + { + return effort_or_acceleration_.data(); + } + + /** \brief Set all accelerations to 0.0 */ + void zeroAccelerations(); + + /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this + * state */ + void setVariableAccelerations(const double* acceleration) + { + has_acceleration_ = true; + has_effort_ = false; + + // assume everything is in order in terms of array lengths (for efficiency reasons) + memcpy(effort_or_acceleration_.data(), acceleration, robot_model_->getVariableCount() * sizeof(double)); + } + + /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this + * state */ + void setVariableAccelerations(const std::vector& acceleration) + { + assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode + setVariableAccelerations(&acceleration[0]); + } + + /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is + * thrown. */ + void setVariableAccelerations(const std::map& variable_map); + + /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is + thrown. + Additionally, \e missing_variables is filled with the names of the variables that are not set. */ + void setVariableAccelerations(const std::map& variable_map, + std::vector& missing_variables); + + /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is + * thrown. */ + void setVariableAccelerations(const std::vector& variable_names, + const std::vector& variable_acceleration); + + /** \brief Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown. */ + void setVariableAcceleration(const std::string& variable, double value) + { + setVariableAcceleration(robot_model_->getVariableIndex(variable), value); + } + + /** \brief Set the acceleration of a single variable. The variable is specified by its index (a value associated by + * the RobotModel to each variable) */ + void setVariableAcceleration(int index, double value) + { + markAcceleration(); + effort_or_acceleration_[index] = value; + } + + /** \brief Get the acceleration of a particular variable. An exception is thrown if the variable is not known. */ + double getVariableAcceleration(const std::string& variable) const + { + return effort_or_acceleration_[robot_model_->getVariableIndex(variable)]; + } + + /** \brief Get the acceleration of a particular variable. The variable is + specified by its index. No checks are performed for the validity + of the index passed */ + double getVariableAcceleration(int index) const + { + return effort_or_acceleration_[index]; + } + + /** \brief Remove accelerations from this state (this differs from setting them to zero) */ + void dropAccelerations(); + + /** @} */ + + /** \name Getting and setting variable effort + * @{ + */ + + /** \brief By default, if effort is never set or initialized, the state remembers that there is no effort set. This is + useful to know when serializing or copying the state. If hasEffort() reports true, hasAccelerations() will + certainly report false. */ + bool hasEffort() const + { + return has_effort_; + } + + /** \brief Get raw access to the effort of the variables that make up this state. The values are in the same order as + * reported by getVariableNames(). The area of memory overlaps with accelerations (effort and acceleration should not + * be set at the same time) */ + double* getVariableEffort() + { + markEffort(); + return effort_or_acceleration_.data(); + } + + /** \brief Get const raw access to the effort of the variables that make up this state. The values are in the same + * order as reported by getVariableNames(). */ + const double* getVariableEffort() const + { + return effort_or_acceleration_.data(); + } + + /** \brief Set all effort values to 0.0 */ + void zeroEffort(); + + /** \brief Given an array with effort values for all variables, set those values as the effort in this state */ + void setVariableEffort(const double* effort) + { + has_effort_ = true; + has_acceleration_ = false; + // assume everything is in order in terms of array lengths (for efficiency reasons) + memcpy(effort_or_acceleration_.data(), effort, robot_model_->getVariableCount() * sizeof(double)); + } + + /** \brief Given an array with effort values for all variables, set those values as the effort in this state */ + void setVariableEffort(const std::vector& effort) + { + assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode + setVariableEffort(&effort[0]); + } + + /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. */ + void setVariableEffort(const std::map& variable_map); + + /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. + Additionally, \e missing_variables is filled with the names of the variables that are not set. */ + void setVariableEffort(const std::map& variable_map, std::vector& missing_variables); + + /** \brief Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. */ + void setVariableEffort(const std::vector& variable_names, + const std::vector& variable_acceleration); + + /** \brief Set the effort of a variable. If an unknown variable name is specified, an exception is thrown. */ + void setVariableEffort(const std::string& variable, double value) + { + setVariableEffort(robot_model_->getVariableIndex(variable), value); + } + + /** \brief Set the effort of a single variable. The variable is specified by its index (a value associated by the + * RobotModel to each variable) */ + void setVariableEffort(int index, double value) + { + markEffort(); + effort_or_acceleration_[index] = value; + } + + /** \brief Get the effort of a particular variable. An exception is thrown if the variable is not known. */ + double getVariableEffort(const std::string& variable) const + { + return effort_or_acceleration_[robot_model_->getVariableIndex(variable)]; + } + + /** \brief Get the effort of a particular variable. The variable is + specified by its index. No checks are performed for the validity + of the index passed */ + double getVariableEffort(int index) const + { + return effort_or_acceleration_[index]; + } + + /** \brief Remove effort values from this state (this differs from setting them to zero) */ + void dropEffort(); + + /** \brief Reduce RobotState to kinematic information (remove velocity, acceleration and effort, if present) */ + void dropDynamics(); + + /** \brief Invert velocity if present. */ + void invertVelocity(); + + /** @} */ + + /** \name Getting and setting joint positions, velocities, accelerations and effort for a single joint + * The joint might be multi-DOF, i.e. require more than one variable to set. + * See setVariablePositions(), setVariableVelocities(), setVariableEffort() to handle multiple joints. + * @{ + */ + void setJointPositions(const std::string& joint_name, const double* position) + { + setJointPositions(robot_model_->getJointModel(joint_name), position); + } + + void setJointPositions(const std::string& joint_name, const std::vector& position) + { + setJointPositions(robot_model_->getJointModel(joint_name), &position[0]); + } + + void setJointPositions(const JointModel* joint, const std::vector& position) + { + setJointPositions(joint, &position[0]); + } + + void setJointPositions(const JointModel* joint, const double* position); + + void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform) + { + setJointPositions(robot_model_->getJointModel(joint_name), transform); + } + + void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform); + + void setJointVelocities(const JointModel* joint, const double* velocity); + + void setJointEfforts(const JointModel* joint, const double* effort); + + const double* getJointPositions(const std::string& joint_name) const + { + return getJointPositions(robot_model_->getJointModel(joint_name)); + } + + // Returns nullptr if `joint` doesn't have any active variables. + const double* getJointPositions(const JointModel* joint) const; + + const double* getJointVelocities(const std::string& joint_name) const + { + return getJointVelocities(robot_model_->getJointModel(joint_name)); + } + + // Returns nullptr if `joint` doesn't have any active variables. + const double* getJointVelocities(const JointModel* joint) const; + + const double* getJointAccelerations(const std::string& joint_name) const + { + return getJointAccelerations(robot_model_->getJointModel(joint_name)); + } + + // Returns nullptr if `joint` doesn't have any active variables. + const double* getJointAccelerations(const JointModel* joint) const; + + const double* getJointEffort(const std::string& joint_name) const + { + return getJointEffort(robot_model_->getJointModel(joint_name)); + } + + // Returns nullptr if `joint` doesn't have any active variables. + const double* getJointEffort(const JointModel* joint) const; + + /** @} */ + + /** \name Getting and setting group positions + * @{ + */ + + /** \brief Given positions for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupPositions(const std::string& joint_group_name, const double* gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupPositions(jmg, gstate); + } + + /** \brief Given positions for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupPositions(const std::string& joint_group_name, const std::vector& gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(gstate.size() == jmg->getVariableCount()); + setJointGroupPositions(jmg, &gstate[0]); + } + } + + /** \brief Given positions for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) + { + assert(gstate.size() == group->getVariableCount()); + setJointGroupPositions(group, &gstate[0]); + } + + /** \brief Given positions for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupPositions(const JointModelGroup* group, const double* gstate); + + /** \brief Given positions for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(values.size() == jmg->getVariableCount()); + setJointGroupPositions(jmg, values); + } + } + + /** \brief Given positions for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const JointModelGroup* group, const std::vector& gstate); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector& gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(gstate.size() == jmg->getActiveVariableCount()); + setJointGroupActivePositions(jmg, gstate); + } + } + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values); + + /** \brief Given positions for the variables of active joints that make up a group, + * in the order found in the group (excluding values of mimic joints), set those + * as the new values that correspond to the group */ + void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + assert(values.size() == jmg->getActiveVariableCount()); + setJointGroupActivePositions(jmg, values); + } + } + + /** \brief For a given group, copy the position values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupPositions(const std::string& joint_group_name, std::vector& gstate) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + gstate.resize(jmg->getVariableCount()); + copyJointGroupPositions(jmg, &gstate[0]); + } + } + + /** \brief For a given group, copy the position values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupPositions(jmg, gstate); + } + + /** \brief For a given group, copy the position values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupPositions(const JointModelGroup* group, std::vector& gstate) const + { + gstate.resize(group->getVariableCount()); + copyJointGroupPositions(group, &gstate[0]); + } + + /** \brief For a given group, copy the position values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const; + + /** \brief For a given group, copy the position values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupPositions(jmg, values); + } + + /** \brief For a given group, copy the position values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const; + + /** @} */ + + /** \name Getting and setting group velocities + * @{ + */ + + /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupVelocities(jmg, gstate); + } + + /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupVelocities(const std::string& joint_group_name, const std::vector& gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupVelocities(jmg, &gstate[0]); + } + + /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupVelocities(const JointModelGroup* group, const std::vector& gstate) + { + setJointGroupVelocities(group, &gstate[0]); + } + + /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupVelocities(const JointModelGroup* group, const double* gstate); + + /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupVelocities(jmg, values); + } + + /** \brief Given velocities for the variables that make up a group, in the order found in the group (including values + * of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values); + + /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupVelocities(const std::string& joint_group_name, std::vector& gstate) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + gstate.resize(jmg->getVariableCount()); + copyJointGroupVelocities(jmg, &gstate[0]); + } + } + + /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupVelocities(jmg, gstate); + } + + /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupVelocities(const JointModelGroup* group, std::vector& gstate) const + { + gstate.resize(group->getVariableCount()); + copyJointGroupVelocities(group, &gstate[0]); + } + + /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const; + + /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupVelocities(jmg, values); + } + + /** \brief For a given group, copy the velocity values of the variables that make up the group into another location, + * in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the + * RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const; + + /** @} */ + + /** \name Getting and setting group accelerations + * @{ + */ + + /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including + * values of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupAccelerations(jmg, gstate); + } + + /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including + * values of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector& gstate) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupAccelerations(jmg, &gstate[0]); + } + + /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including + * values of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupAccelerations(const JointModelGroup* group, const std::vector& gstate) + { + setJointGroupAccelerations(group, &gstate[0]); + } + + /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including + * values of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate); + + /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including + * values of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values) + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupAccelerations(jmg, values); + } + + /** \brief Given accelerations for the variables that make up a group, in the order found in the group (including + * values of mimic joints), set those as the new values that correspond to the group */ + void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values); + + /** \brief For a given group, copy the acceleration values of the variables that make up the group into another + * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of + * memory in the RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector& gstate) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + { + gstate.resize(jmg->getVariableCount()); + copyJointGroupAccelerations(jmg, &gstate[0]); + } + } + + /** \brief For a given group, copy the acceleration values of the variables that make up the group into another + * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of + * memory in the RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupAccelerations(jmg, gstate); + } + + /** \brief For a given group, copy the acceleration values of the variables that make up the group into another + * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of + * memory in the RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupAccelerations(const JointModelGroup* group, std::vector& gstate) const + { + gstate.resize(group->getVariableCount()); + copyJointGroupAccelerations(group, &gstate[0]); + } + + /** \brief For a given group, copy the acceleration values of the variables that make up the group into another + * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of + * memory in the RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const; + + /** \brief For a given group, copy the acceleration values of the variables that make up the group into another + * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of + * memory in the RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const + { + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupAccelerations(jmg, values); + } + + /** \brief For a given group, copy the acceleration values of the variables that make up the group into another + * location, in the order that the variables are found in the group. This is not necessarily a contiguous block of + * memory in the RobotState itself, so we copy instead of returning a pointer.*/ + void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const; + + /** @} */ + + /** \name Setting from Inverse Kinematics + * @{ + */ + + /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be + set by computing inverse kinematics. + The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. + @param pose The pose the last link in the chain needs to achieve + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be + set by computing inverse kinematics. + The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. + @param pose The pose the \e tip link in the chain needs to achieve + @param tip The name of the link the pose is specified for + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be + set by computing inverse kinematics. + The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. + @param pose The pose the last link in the chain needs to achieve + @param tip The name of the link the pose is specified for + @param timeout The timeout passed to the kinematics solver on each attempt */ + bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be + set by computing inverse kinematics. + The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. + @param pose The pose the last link in the chain needs to achieve + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be + set by computing inverse kinematics. + The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. + @param pose The pose the last link in the chain needs to achieve + @param tip The name of the frame for which IK is attempted. + @param consistency_limits This specifies the desired distance between the solution and the seed state + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, + const std::vector& consistency_limits, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief Warning: This function inefficiently copies all transforms around. + If the group consists of a set of sub-groups that are each a chain and a solver + is available for each sub-group, then the joint values can be set by computing inverse kinematics. + The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed + to be in the same order as the order of the sub-groups in this group. Returns true on success. + @param poses The poses the last link in each chain needs to achieve + @param tips The names of the frames for which IK is attempted. + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, + const std::vector& tips, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief Warning: This function inefficiently copies all transforms around. + If the group consists of a set of sub-groups that are each a chain and a solver + is available for each sub-group, then the joint values can be set by computing inverse kinematics. + The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed + to be in the same order as the order of the sub-groups in this group. Returns true on success. + @param poses The poses the last link in each chain needs to achieve + @param tips The names of the frames for which IK is attempted. + @param consistency_limits This specifies the desired distance between the solution and the seed state + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, + const std::vector& tips, const std::vector>& consistency_limits, + double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** + \brief setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for + non-chain kinematics. In this case, we divide the group into subgroups and do IK solving individually + @param poses The poses the last link in each chain needs to achieve + @param tips The names of the frames for which IK is attempted. + @param consistency_limits This specifies the desired distance between the solution and the seed state + @param timeout The timeout passed to the kinematics solver on each attempt + @param constraint A state validity constraint to be required for IK solutions */ + bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses, + const std::vector& tips, + const std::vector>& consistency_limits, double timeout = 0.0, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); + + /** \brief Set the joint values from a Cartesian velocity applied during a time dt + * @param group the group of joints this function operates on + * @param twist a Cartesian velocity on the 'tip' frame + * @param tip the frame for which the twist is given + * @param dt a time interval (seconds) + * @param st a secondary task computation function + */ + bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); + + /** \brief Set the joint values from a Cartesian velocity applied during a time dt + * @param group the group of joints this function operates on + * @param twist a Cartesian velocity on the 'tip' frame + * @param tip the frame for which the twist is given + * @param dt a time interval (seconds) + * @param st a secondary task computation function + */ + bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip, + double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); + + /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. + * \param group The group to compute the Jacobian for + * \param link The link model to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link) + * \param jacobian The resultant jacobian, with the origin at the group root link. + * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation + * (default is false) + * \return True if jacobian was successfully computed, false otherwise + */ + bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position, + Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const; + + /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. + * \param group The group to compute the Jacobian for + * \param link The link model to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link) + * \param jacobian The resultant jacobian, with the origin at the group root link. + * \param use_quaternion_representation Flag indicating if the Jacobian should use a quaternion representation + * (default is false) + * \return True if jacobian was successfully computed, false otherwise + */ + bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position, + Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) + { + updateLinkTransforms(); + return static_cast(this)->getJacobian(group, link, reference_point_position, jacobian, + use_quaternion_representation); + } + + /** \brief Compute the Jacobian with reference to the last link of a specified group, and origin at the group root + * link. If the group is not a chain, an exception is thrown. + * \param group The group to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link_name) + * \return The computed Jacobian. + */ + Eigen::MatrixXd getJacobian(const JointModelGroup* group, + const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const; + + /** \brief Compute the Jacobian with reference to the last link of a specified group, and origin at the group root + * link. If the group is not a chain, an exception is thrown. + * \param group The group to compute the Jacobian for + * \param reference_point_position The reference point position (with respect to the link specified in link_name) + * \return The computed Jacobian. + */ + Eigen::MatrixXd getJacobian(const JointModelGroup* group, + const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) + { + updateLinkTransforms(); + return static_cast(this)->getJacobian(group, reference_point_position); + } + + /** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and + * store it in \e qdot */ + void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, + const LinkModel* tip) const; + + /** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and + * store it in \e qdot */ + void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, + const LinkModel* tip) + { + updateLinkTransforms(); + static_cast(this)->computeVariableVelocity(jmg, qdot, twist, tip); + } + + /** \brief Given the velocities for the variables in this group (\e qdot) and an amount of time (\e dt), + update the current state using the Euler forward method. If the constraint specified is satisfied, return true, + otherwise return false. */ + bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt, + const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); + + /** @} */ + + /** \name Getting and setting whole states + * @{ + */ + + void setVariableValues(const sensor_msgs::msg::JointState& msg) + { + if (!msg.position.empty()) + setVariablePositions(msg.name, msg.position); + if (!msg.velocity.empty()) + setVariableVelocities(msg.name, msg.velocity); + } + + /** \brief Set all joints to their default positions. + The default position is 0, or if that is not within bounds then half way + between min and max bound. */ + void setToDefaultValues(); + + /** \brief Set the joints in \e group to the position \e name defined in the SRDF */ + bool setToDefaultValues(const JointModelGroup* group, const std::string& name); + + bool setToDefaultValues(const std::string& group_name, const std::string& state_name) + { + const JointModelGroup* jmg = getJointModelGroup(group_name); + if (jmg) + { + return setToDefaultValues(jmg, state_name); + } + else + { + return false; + } + } + + /** \brief Set all joints to random values. Values will be within default bounds. */ + void setToRandomPositions(); + + /** \brief Set all joints in \e group to random values. Values will be within default bounds. */ + void setToRandomPositions(const JointModelGroup* group); + + /** \brief Set all joints in \e group to random values using a specified random number generator. + Values will be within default bounds. */ + void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng); + + /** \brief Set all joints in \e group to random values near the value in \e seed. + * \e distance is the maximum amount each joint value will vary from the + * corresponding value in \e seed. \distance represents meters for + * prismatic/positional joints and radians for revolute/orientation joints. + * Resulting values are clamped within default bounds. */ + void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance); + + /** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number + * generator. \e distance is the maximum amount each joint value will vary from the corresponding value in \e seed. + * \distance represents meters for prismatic/positional joints and radians for revolute/orientation joints. Resulting + * values are clamped within default bounds. */ + void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance, + random_numbers::RandomNumberGenerator& rng); + + /** \brief Set all joints in \e group to random values near the value in \e seed. + * \e distances \b MUST have the same size as \c + * group.getActiveJointModels(). Each value in \e distances is the maximum + * amount the corresponding active joint in \e group will vary from the + * corresponding value in \e seed. \distance represents meters for + * prismatic/positional joints and radians for revolute/orientation joints. + * Resulting values are clamped within default bounds. */ + void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, + const std::vector& distances); + + /** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number + * generator. \e distances \b MUST have the same size as \c group.getActiveJointModels(). Each value in \e distances + * is the maximum amount the corresponding active joint in \e group will vary from the corresponding value in \e seed. + * \distance represents meters for prismatic/positional joints and radians for revolute/orientation joints. Resulting + * values are clamped within default bounds. */ + void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, + const std::vector& distances, random_numbers::RandomNumberGenerator& rng); + + /** @} */ + + /** \name Updating and getting transforms + * @{ + */ + + /** \brief Update the transforms for the collision bodies. This call is needed before calling collision checking. + If updating link transforms or joint transforms is needed, the corresponding updates are also triggered. */ + void updateCollisionBodyTransforms(); + + /** \brief Update the reference frame transforms for links. This call is needed before using the transforms of links + * for coordinate transforms. */ + void updateLinkTransforms(); + + /** \brief Update all transforms. */ + void update(bool force = false); + + /** \brief Update the state after setting a particular link to the input global transform pose. + + This "warps" the given link to the given pose, neglecting the joint values of its parent joint. + The link transforms of link and all its descendants are updated, but not marked as dirty, + although they do not match the joint values anymore! + Collision body transforms are not yet updated, but marked dirty only. + Use update(false) or updateCollisionBodyTransforms() to update them as well. + */ + void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false) + { + updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward); + } + + /** \brief Update the state after setting a particular link to the input global transform pose.*/ + void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false); + + /** \brief Get the latest link upwards the kinematic tree which is only connected via fixed joints. + * + * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel, + * but can additionally resolve parents for attached objects / subframes. + */ + const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const; + + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. + * This is typically the root link of the URDF unless a virtual joint is present. + * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. + * A related, more comprehensive function is |getFrameTransform|, which additionally to link frames + * also searches for attached object frames and their subframes. + * + * This will throw an exception if the passed link is not found + * + * The returned transformation is always a valid isometry. + */ + const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) + { + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); + } + + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) + { + if (!link) + { + throw Exception("Invalid link"); + } + updateLinkTransforms(); + return global_link_transforms_[link->getLinkIndex()]; + } + + const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const + { + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); + } + + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const + { + if (!link) + { + throw Exception("Invalid link"); + } + assert(checkLinkTransforms()); + return global_link_transforms_[link->getLinkIndex()]; + } + + /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel. + * This is typically the root link of the URDF unless a virtual joint is present. + * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. + * + * As opposed to the visual links in |getGlobalLinkTransform|, this function returns + * the collision link transform used for collision checking. + * + * @param link_name: name of link to lookup + * @param index: specify which collision body to lookup, if more than one exists + */ + const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) + { + return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); + } + + const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) + { + updateCollisionBodyTransforms(); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; + } + + const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const + { + return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); + } + + const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const + { + assert(checkCollisionTransforms()); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; + } + + const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) + { + return getJointTransform(robot_model_->getJointModel(joint_name)); + } + + const Eigen::Isometry3d& getJointTransform(const JointModel* joint); + + const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const + { + return getJointTransform(robot_model_->getJointModel(joint_name)); + } + + const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const + { + assert(checkJointTransforms(joint)); + return variable_joint_transforms_[joint->getJointIndex()]; + } + + bool dirtyJointTransform(const JointModel* joint) const + { + return dirty_joint_transforms_[joint->getJointIndex()]; + } + + bool dirtyLinkTransforms() const + { + return dirty_link_transforms_; + } + + bool dirtyCollisionBodyTransforms() const + { + return dirty_link_transforms_ || dirty_collision_body_transforms_; + } + + /** \brief Returns true if anything in this state is dirty */ + bool dirty() const + { + return dirtyCollisionBodyTransforms(); + } + + /** @} */ + + /** \name Computing distances + * @{ + */ + + /** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */ + double distance(const RobotState& other) const + { + return robot_model_->distance(position_.data(), other.getVariablePositions()); + } + + /** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */ + double distance(const RobotState& other, const JointModelGroup* joint_group) const; + + /** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */ + double distance(const RobotState& other, const JointModel* joint) const; + + /** + * Interpolate towards "to" state. Mimic joints are correctly updated and flags are set so that FK is recomputed + * when needed. + * + * @param to interpolate to this state + * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. + * @param state holds the result + */ + void interpolate(const RobotState& to, double t, RobotState& state) const; + + /** + * Interpolate towards "to" state, but only for the joints in the specified group. Mimic joints are correctly updated + * and flags are set so that FK is recomputed when needed. + * + * @param to interpolate to this state + * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. + * @param state holds the result + * @param joint_group interpolate only for the joints in this group + */ + void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const; + + /** + * Interpolate towards "to" state, but only for a single joint. Mimic joints are correctly updated + * and flags are set so that FK is recomputed when needed. + * + * @param to interpolate to this state + * @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly. + * @param state holds the result + * @param joint interpolate only for this joint + */ + void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const; + + void enforceBounds(); + void enforceBounds(const JointModelGroup* joint_group); + void enforceBounds(const JointModel* joint) + { + enforcePositionBounds(joint); + if (has_velocity_) + enforceVelocityBounds(joint); + } + void enforcePositionBounds(const JointModel* joint); + + /// Call harmonizePosition() for all joints / all joints in group / given joint + void harmonizePositions(); + void harmonizePositions(const JointModelGroup* joint_group); + void harmonizePosition(const JointModel* joint); + + void enforceVelocityBounds(const JointModel* joint); + + bool satisfiesBounds(double margin = 0.0) const; + bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const; + bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const + { + return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin)); + } + bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const + { + return joint->satisfiesPositionBounds(getJointPositions(joint), margin); + } + bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const + { + return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin); + } + + /** \brief Get the minimm distance from this state to the bounds. + The minimum distance and the joint for which this minimum is achieved are returned. */ + std::pair getMinDistanceToPositionBounds() const; + + /** \brief Get the minimm distance from a group in this state to the bounds. + The minimum distance and the joint for which this minimum is achieved are returned. */ + std::pair getMinDistanceToPositionBounds(const JointModelGroup* group) const; + + /** \brief Get the minimm distance from a set of joints in the state to the bounds. + The minimum distance and the joint for which this minimum is achieved are returned. */ + std::pair + getMinDistanceToPositionBounds(const std::vector& joints) const; + + /** + * \brief Check that the time to move between two waypoints is sufficient given velocity limits and time step + * \param other - robot state to compare joint positions against + * \param group - planning group to compare joint positions against + * \param dt - time step between the two points + */ + bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const; + + /** @} */ + + /** \name Managing attached bodies + * @{ + */ + + /** \brief Add an attached body to this state. + * + * This only adds the given body to this RobotState + * instance. It does not change anything about other + * representations of the object elsewhere in the system. So if the + * body represents an object in a collision_detection::World (like + * from a planning_scene::PlanningScene), you will likely need to remove the + * corresponding object from that world to avoid having collisions + * detected against it. + **/ + void attachBody(std::unique_ptr attached_body); + + /** @brief Add an attached body to a link + * @param id The string id associated with the attached body + * @param pose The pose associated with the attached body + * @param shapes The shapes that make up the attached body + * @param shape_poses The transforms between the object pose and the attached body's shapes + * @param touch_links The set of links that the attached body is allowed to touch + * @param link_name The link to attach to + * @param detach_posture The posture of the gripper when placing the object + * @param subframe_poses Transforms to points of interest on the object (can be used as end effector link) + * + * This only adds the given body to this RobotState + * instance. It does not change anything about other + * representations of the object elsewhere in the system. So if the + * body represents an object in a collision_detection::World (like + * from a planning_scene::PlanningScene), you will likely need to remove the + * corresponding object from that world to avoid having collisions + * detected against it. */ + void attachBody(const std::string& id, const Eigen::Isometry3d& pose, + const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, + const std::set& touch_links, const std::string& link_name, + const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(), + const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); + + /** @brief Add an attached body to a link + * @param id The string id associated with the attached body + * @param pose The pose associated with the attached body + * @param shapes The shapes that make up the attached body + * @param shape_poses The transforms between the object pose and the attached body's shapes + * @param touch_links The set of links that the attached body is allowed to touch + * @param link_name The link to attach to + * @param detach_posture The posture of the gripper when placing the object + * @param subframe_poses Transforms to points of interest on the object (can be used as end effector link) + * + * This only adds the given body to this RobotState + * instance. It does not change anything about other + * representations of the object elsewhere in the system. So if the + * body represents an object in a collision_detection::World (like + * from a planning_scene::PlanningScene), you will likely need to remove the + * corresponding object from that world to avoid having collisions + * detected against it. */ + void attachBody(const std::string& id, const Eigen::Isometry3d& pose, + const std::vector& shapes, const EigenSTL::vector_Isometry3d& shape_poses, + const std::vector& touch_links, const std::string& link_name, + const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(), + const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()) + { + std::set touch_links_set(touch_links.begin(), touch_links.end()); + attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses); + } + + /** \brief Get all bodies attached to the model corresponding to this state */ + void getAttachedBodies(std::vector& attached_bodies) const; + + /** \brief Get all bodies attached to a particular group the model corresponding to this state */ + void getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const; + + /** \brief Get all bodies attached to a particular link in the model corresponding to this state */ + void getAttachedBodies(std::vector& attached_bodies, const LinkModel* link_model) const; + + /** \brief Remove the attached body named \e id. Return false if the object was not found (and thus not removed). + * Return true on success. */ + bool clearAttachedBody(const std::string& id); + + /** \brief Clear the bodies attached to a specific link */ + void clearAttachedBodies(const LinkModel* link); + + /** \brief Clear the bodies attached to a specific group */ + void clearAttachedBodies(const JointModelGroup* group); + + /** \brief Clear all attached bodies. This calls delete on the AttachedBody instances, if needed. */ + void clearAttachedBodies(); + + /** \brief Get the attached body named \e name. Return nullptr if not found. */ + const AttachedBody* getAttachedBody(const std::string& name) const; + + /** \brief Check if an attached body named \e id exists in this state */ + bool hasAttachedBody(const std::string& id) const; + + void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback); + /** @} */ + + /** \brief Compute an axis-aligned bounding box that contains the current state. + The format for \e aabb is (minx, maxx, miny, maxy, minz, maxz) */ + void computeAABB(std::vector& aabb) const; + + /** \brief Compute an axis-aligned bounding box that contains the current state. + The format for \e aabb is (minx, maxx, miny, maxy, minz, maxz) */ + void computeAABB(std::vector& aabb) + { + updateLinkTransforms(); + static_cast(this)->computeAABB(aabb); + } + + /** \brief Return the instance of a random number generator */ + random_numbers::RandomNumberGenerator& getRandomNumberGenerator() + { + if (!rng_) + rng_ = std::make_unique(); + return *rng_; + } + + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id + * + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ + const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr); + + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id + * + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ + const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; + + /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id + * + * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ + const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, + bool& frame_found) const; + + /** \brief Check if a transformation matrix from the model frame (root of model) to frame \e frame_id is known */ + bool knowsFrameTransform(const std::string& frame_id) const; + + /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. + * @param arr The returned marker array + * @param link_names The list of link names for which the markers should be created. + * @param color The color for the marker + * @param ns The namespace for the markers + * @param dur The rclcpp::Duration for which the markers should stay visible + */ + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, + bool include_attached = false) const; + + /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first. + * @param arr The returned marker array + * @param link_names The list of link names for which the markers should be created. + * @param color The color for the marker + * @param ns The namespace for the markers + * @param dur The rclcpp::Duration for which the markers should stay visible + */ + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur, + bool include_attached = false) + { + updateCollisionBodyTransforms(); + static_cast(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached); + } + + /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. + * @param arr The returned marker array + * @param link_names The list of link names for which the markers should be created. + */ + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + bool include_attached = false) const; + + /** @brief Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first. + * @param arr The returned marker array + * @param link_names The list of link names for which the markers should be created. + */ + void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector& link_names, + bool include_attached = false) + { + updateCollisionBodyTransforms(); + static_cast(this)->getRobotMarkers(arr, link_names, include_attached); + } + + void printStatePositions(std::ostream& out = std::cout) const; + + /** \brief Output to console the current state of the robot's joint limits */ + void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const; + + void printStateInfo(std::ostream& out = std::cout) const; + + void printTransforms(std::ostream& out = std::cout) const; + + void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const; + + void printDirtyInfo(std::ostream& out = std::cout) const; + + std::string getStateTreeString() const; + + /** + * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver + * @param pose - the input to change + * @param solver - a kin solver whose base frame is important to us + * @return true if no error + */ + bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver); + + /** + * \brief Transform pose from the robot model's base frame to the reference frame of the IK solver + * @param pose - the input to change + * @param ik_frame - the name of frame of reference of base of ik solver + * @return true if no error + */ + bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame); + +private: + void allocMemory(); + void init(); + void copyFrom(const RobotState& other); + + void markDirtyJointTransforms(const JointModel* joint) + { + dirty_joint_transforms_[joint->getJointIndex()] = 1; + dirty_link_transforms_ = + dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); + } + + void markDirtyJointTransforms(const JointModelGroup* group) + { + for (const JointModel* jm : group->getActiveJointModels()) + dirty_joint_transforms_[jm->getJointIndex()] = 1; + dirty_link_transforms_ = dirty_link_transforms_ == nullptr ? + group->getCommonRoot() : + robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot()); + } + + void markVelocity(); + void markAcceleration(); + void markEffort(); + + void updateMimicJoint(const JointModel* joint); + + /** \brief Update all mimic joints within group */ + void updateMimicJoints(const JointModelGroup* group); + + void updateLinkTransformsInternal(const JointModel* start); + + void getMissingKeys(const std::map& variable_map, + std::vector& missing_variables) const; + void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const; + + /** \brief This function is only called in debug mode */ + bool checkJointTransforms(const JointModel* joint) const; + + /** \brief This function is only called in debug mode */ + bool checkLinkTransforms() const; + + /** \brief This function is only called in debug mode */ + bool checkCollisionTransforms() const; + + RobotModelConstPtr robot_model_; + + std::vector position_; + std::vector velocity_; + std::vector effort_or_acceleration_; + bool has_velocity_ = false; + bool has_acceleration_ = false; + bool has_effort_ = false; + + const JointModel* dirty_link_transforms_ = nullptr; + const JointModel* dirty_collision_body_transforms_ = nullptr; + + // All the following transform variables point into aligned memory. + // They are updated lazily, based on the flags in dirty_joint_transforms_ + // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_ + std::vector variable_joint_transforms_; ///< Local transforms of all joints + std::vector global_link_transforms_; ///< Transforms from model frame to link frame for each link + std::vector global_collision_body_transforms_; ///< Transforms from model frame to collision + ///< bodies + std::vector dirty_joint_transforms_; + + /** \brief All attached bodies that are part of this state, indexed by their name */ + std::map> attached_body_map_; + + /** \brief This event is called when there is a change in the attached bodies for this state; + The event specifies the body that changed and whether it was just attached or about to be detached. */ + AttachedBodyCallback attached_body_update_callback_; + + /** \brief For certain operations a state needs a random number generator. However, it may be slightly expensive + to allocate the random number generator if many state instances are generated. For this reason, the generator + is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but + call getRandomNumberGenerator() instead. */ + std::unique_ptr rng_; +}; + +/** \brief Operator overload for printing variable bounds to a stream */ +std::ostream& operator<<(std::ostream& out, const RobotState& s); +} // namespace core +} // namespace moveit diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 3da139aac1..3a606dfe5d 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 71b927daaf..0c85e910e9 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -37,7 +37,7 @@ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman, Robert Haschke */ #include -#include +#include #include #include #include diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 89538525b5..67082811aa 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -36,7 +36,7 @@ /* Author: Ioan Sucan, Dave Coleman */ #include -#include +#include #include #include #include diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 1f1593ca78..ff847bcb54 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -37,9 +37,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -47,8 +47,8 @@ #include #include #include -#include -#include +#include +#include #include namespace moveit diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 06c40df745..161e280877 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -41,9 +41,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include // Robot and planning group for benchmarks. constexpr char PANDA_TEST_ROBOT[] = "panda"; diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index e1d710805c..5af16e3788 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -33,9 +33,9 @@ *********************************************************************/ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 79936d45dc..cae7b5f730 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -34,16 +34,16 @@ /* Author: Martin Pecka */ -#include -#include -#include +#include +#include +#include #include #include #include #include #include #include -#include +#include // To visualize bbox of the PR2, set this to 1. #ifndef VISUALIZE_PR2_RVIZ diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index c15f172b76..d5af24e429 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -34,11 +34,11 @@ /* Author: Michael Lautman */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp index 973c5560a5..b4aa27dba5 100644 --- a/moveit_core/robot_state/test/test_kinematic_complex.cpp +++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp @@ -34,15 +34,15 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include #include -#include +#include constexpr double EPSILON = 1e-2; constexpr double M_TAU = 2 * M_PI; diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index ed2887e2a2..d1763c1a7c 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,399 +47,5 @@ /* Author: Ioan Sucan, Adam Leeper */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace robot_trajectory -{ -MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc - -/** \brief Maintain a sequence of waypoints and the time durations - between these waypoints */ -class RobotTrajectory -{ -public: - /** @brief construct a trajectory for the whole robot */ - explicit RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model); - - /** @brief construct a trajectory for the named JointModelGroup - * If group is an empty string, this is equivalent to the first constructor, - * otherwise it is equivalent to `RobotTrajectory(robot_model, robot_model->getJointModelGroup(group))`. - */ - RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group); - - /** @brief construct a trajectory for the JointModelGroup - * Only joints from the specified group will be considered in this trajectory, - * even though all waypoints still consist of full RobotStates (representing all joints). - * - * If group is nullptr this is equivalent to the first constructor. - */ - RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group); - - /** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */ - RobotTrajectory& operator=(const RobotTrajectory&) = default; - - /** @brief Copy constructor allowing a shallow or deep copy of waypoints - * @param other - RobotTrajectory to copy from - * @param deepcopy - copy waypoints by value (true) or by pointer (false)? - */ - RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false); - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - const moveit::core::JointModelGroup* getGroup() const - { - return group_; - } - - const std::string& getGroupName() const; - - RobotTrajectory& setGroupName(const std::string& group_name) - { - group_ = robot_model_->getJointModelGroup(group_name); - return *this; - } - - std::size_t getWayPointCount() const - { - return waypoints_.size(); - } - - std::size_t size() const - { - assert(waypoints_.size() == duration_from_previous_.size()); - return waypoints_.size(); - } - - const moveit::core::RobotState& getWayPoint(std::size_t index) const - { - return *waypoints_[index]; - } - - const moveit::core::RobotState& getLastWayPoint() const - { - return *waypoints_.back(); - } - - const moveit::core::RobotState& getFirstWayPoint() const - { - return *waypoints_.front(); - } - - moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index) - { - return waypoints_[index]; - } - - moveit::core::RobotStatePtr& getLastWayPointPtr() - { - return waypoints_.back(); - } - - moveit::core::RobotStatePtr& getFirstWayPointPtr() - { - return waypoints_.front(); - } - - const std::deque& getWayPointDurations() const - { - return duration_from_previous_; - } - - /** @brief Returns the duration after start that a waypoint will be reached. - * @param The waypoint index. - * @return The duration from start; returns overall duration if index is out of range. - */ - double getWayPointDurationFromStart(std::size_t index) const; - - double getWayPointDurationFromPrevious(std::size_t index) const - { - if (duration_from_previous_.size() > index) - { - return duration_from_previous_[index]; - } - else - { - return 0.0; - } - } - - RobotTrajectory& setWayPointDurationFromPrevious(std::size_t index, double value) - { - if (duration_from_previous_.size() <= index) - duration_from_previous_.resize(index + 1, 0.0); - duration_from_previous_[index] = value; - return *this; - } - - bool empty() const - { - return waypoints_.empty(); - } - - /** - * \brief Add a point to the trajectory - * \param state - current robot state - * \param dt - duration from previous - */ - RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotState& state, double dt) - { - return addSuffixWayPoint(std::make_shared(state), dt); - } - - /** - * \brief Add a point to the trajectory - * \param state - current robot state - * \param dt - duration from previous - */ - RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt) - { - state->update(); - waypoints_.push_back(state); - duration_from_previous_.push_back(dt); - return *this; - } - - RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotState& state, double dt) - { - return addPrefixWayPoint(std::make_shared(state), dt); - } - - RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt) - { - state->update(); - waypoints_.push_front(state); - duration_from_previous_.push_front(dt); - return *this; - } - - RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt) - { - return insertWayPoint(index, std::make_shared(state), dt); - } - - RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt) - { - state->update(); - waypoints_.insert(waypoints_.begin() + index, state); - duration_from_previous_.insert(duration_from_previous_.begin() + index, dt); - return *this; - } - - /** - * \brief Add a specified part of a trajectory to the end of the current trajectory. The default (when \p start_index - * and \p end_index are omitted) is to add the whole trajectory. - * \param source - the trajectory containing the part to append to the end of current trajectory - * \param dt - time step between last traj point in current traj and first traj point of append traj - * \param start_index - index of first traj point of the part to append from the source traj, the default is to add - * from the start of the source traj - * \param end_index - index of last traj point of the part to append from the source traj, the default is to add until - * the end of the source traj - */ - RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0, - size_t end_index = std::numeric_limits::max()); - - void swap(robot_trajectory::RobotTrajectory& other) noexcept; - - /** - * \brief Remove a point from the trajectory - * \param index - the index to remove - */ - RobotTrajectory& removeWayPoint(std::size_t index) - { - waypoints_.erase(waypoints_.begin() + index); - duration_from_previous_.erase(duration_from_previous_.begin() + index); - return *this; - } - - RobotTrajectory& clear() - { - waypoints_.clear(); - duration_from_previous_.clear(); - return *this; - } - - double getDuration() const; - - double getAverageSegmentDuration() const; - - void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory, - const std::vector& joint_filter = std::vector()) const; - - /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required - to contain the values - for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Each - point in the trajectory - to be constructed internally is obtained by copying the reference state and overwriting the content from a - trajectory point in \e trajectory. */ - RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, - const trajectory_msgs::msg::JointTrajectory& trajectory); - - /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required - to contain the values - for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Each - point in the trajectory - to be constructed internally is obtained by copying the reference state and overwriting the content from a - trajectory point in \e trajectory. */ - RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, - const moveit_msgs::msg::RobotTrajectory& trajectory); - - /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required - to contain the values - for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Before - use, the reference state is updated - using \e state. Each point in the trajectory to be constructed internally is obtained by copying the reference - state and overwriting the content - from a trajectory point in \e trajectory. */ - RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, - const moveit_msgs::msg::RobotState& state, - const moveit_msgs::msg::RobotTrajectory& trajectory); - - RobotTrajectory& reverse(); - - RobotTrajectory& unwind(); - - /** @brief Unwind, starting from an initial state **/ - RobotTrajectory& unwind(const moveit::core::RobotState& state); - - /** @brief Finds the waypoint indices before and after a duration from start. - * @param The duration from start. - * @param The waypoint index before the supplied duration. - * @param The waypoint index after (or equal to) the supplied duration. - * @param The progress (0 to 1) between the two waypoints, based on time (not based on joint distances). - */ - void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const; - - // TODO support visitor function for interpolation, or at least different types. - /** @brief Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time - * interpolation. - * @param The duration from start. - * @param The resulting robot state. - * @return True if state is valid, false otherwise (trajectory is empty). - */ - bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const; - - class Iterator - { - std::deque::iterator waypoint_iterator_; - std::deque::iterator duration_iterator_; - - public: - explicit Iterator(const std::deque::iterator& waypoint_iterator, - const std::deque::iterator& duration_iterator) - : waypoint_iterator_(waypoint_iterator), duration_iterator_(duration_iterator) - { - } - Iterator& operator++() - { - waypoint_iterator_++; - duration_iterator_++; - return *this; - } - Iterator operator++(int) - { - Iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const Iterator& other) const - { - return ((waypoint_iterator_ == other.waypoint_iterator_) && (duration_iterator_ == other.duration_iterator_)); - } - bool operator!=(const Iterator& other) const - { - return !(*this == other); - } - std::pair operator*() const - { - return std::pair{ *waypoint_iterator_, *duration_iterator_ }; - } - - // iterator traits - using difference_type = long; - using value_type = std::pair; - using pointer = const std::pair*; - using reference = std::pair; - using iterator_category = std::input_iterator_tag; - }; - - RobotTrajectory::Iterator begin() - { - assert(waypoints_.size() == duration_from_previous_.size()); - return Iterator(waypoints_.begin(), duration_from_previous_.begin()); - } - RobotTrajectory::Iterator end() - { - assert(waypoints_.size() == duration_from_previous_.size()); - return Iterator(waypoints_.end(), duration_from_previous_.end()); - } - /** @brief Print information about the trajectory - * @param out Stream to print to - * @param variable_indexes The indexes of the variables to print. - * If empty/not specified and the group is defined, then uses the indexes for the group - * If empty and the group is not defined, uses ALL variables in robot_model - * - * e.g. - * Trajectory has 13 points over 2.965 seconds - * waypoint 0 time 0.000 pos 0.000 vel 0.000 acc 0.000 - * waypoint 1 time 0.067 pos 0.001 vel 0.033 acc 1.000 - * waypoint 2 time 0.665 pos 0.200 vel 0.632 acc 1.000 - * ... - */ - void print(std::ostream& out, std::vector variable_indexes = std::vector()) const; - -private: - moveit::core::RobotModelConstPtr robot_model_; - const moveit::core::JointModelGroup* group_; - std::deque waypoints_; - std::deque duration_from_previous_; -}; - -/** @brief Operator overload for printing trajectory to a stream */ -std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); - -/// \brief Calculate the path length of a given trajectory based on the -/// accumulated robot state distances. -/// The distance between two robot states is calculated based on the sum of -/// active joint distances between the two states (L1 norm). -/// \param[in] trajectory Given robot trajectory -/// \return Length of the robot trajectory [rad] -[[nodiscard]] double pathLength(const RobotTrajectory& trajectory); - -/// \brief Calculate the smoothness of a given trajectory -/// \param[in] trajectory Given robot trajectory -/// \return Smoothness of the given trajectory -/// or nullopt if it is not possible to calculate the smoothness -[[nodiscard]] std::optional smoothness(const RobotTrajectory& trajectory); - -/// \brief Calculate the waypoint density of a trajectory -/// \param[in] trajectory Given robot trajectory -/// \return Waypoint density of the given trajectory -/// or nullopt if it is not possible to calculate the density -[[nodiscard]] std::optional waypointDensity(const RobotTrajectory& trajectory); - -/// \brief Converts a RobotTrajectory to a JointTrajectory message -// \param[in] trajectory Given robot trajectory -// \param[in] include_mdof_joints Treat Multi-DOF variables as joints, e.g. position/x position/y position/theta -// \param[in] joint_filter Exclude joints with the provided names -// \return JointTrajectory message including all waypoints -// or nullopt if the provided RobotTrajectory or RobotModel is empty -[[nodiscard]] std::optional -toJointTrajectory(const RobotTrajectory& trajectory, bool include_mdof_joints = false, - const std::vector& joint_filter = {}); -} // namespace robot_trajectory +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.hpp b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.hpp new file mode 100644 index 0000000000..576a65f318 --- /dev/null +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.hpp @@ -0,0 +1,433 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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, Adam Leeper */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace robot_trajectory +{ +MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc + +/** \brief Maintain a sequence of waypoints and the time durations + between these waypoints */ +class RobotTrajectory +{ +public: + /** @brief construct a trajectory for the whole robot */ + explicit RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model); + + /** @brief construct a trajectory for the named JointModelGroup + * If group is an empty string, this is equivalent to the first constructor, + * otherwise it is equivalent to `RobotTrajectory(robot_model, robot_model->getJointModelGroup(group))`. + */ + RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group); + + /** @brief construct a trajectory for the JointModelGroup + * Only joints from the specified group will be considered in this trajectory, + * even though all waypoints still consist of full RobotStates (representing all joints). + * + * If group is nullptr this is equivalent to the first constructor. + */ + RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group); + + /** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */ + RobotTrajectory& operator=(const RobotTrajectory&) = default; + + /** @brief Copy constructor allowing a shallow or deep copy of waypoints + * @param other - RobotTrajectory to copy from + * @param deepcopy - copy waypoints by value (true) or by pointer (false)? + */ + RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false); + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + const moveit::core::JointModelGroup* getGroup() const + { + return group_; + } + + const std::string& getGroupName() const; + + RobotTrajectory& setGroupName(const std::string& group_name) + { + group_ = robot_model_->getJointModelGroup(group_name); + return *this; + } + + std::size_t getWayPointCount() const + { + return waypoints_.size(); + } + + std::size_t size() const + { + assert(waypoints_.size() == duration_from_previous_.size()); + return waypoints_.size(); + } + + const moveit::core::RobotState& getWayPoint(std::size_t index) const + { + return *waypoints_[index]; + } + + const moveit::core::RobotState& getLastWayPoint() const + { + return *waypoints_.back(); + } + + const moveit::core::RobotState& getFirstWayPoint() const + { + return *waypoints_.front(); + } + + moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index) + { + return waypoints_[index]; + } + + moveit::core::RobotStatePtr& getLastWayPointPtr() + { + return waypoints_.back(); + } + + moveit::core::RobotStatePtr& getFirstWayPointPtr() + { + return waypoints_.front(); + } + + const std::deque& getWayPointDurations() const + { + return duration_from_previous_; + } + + /** @brief Returns the duration after start that a waypoint will be reached. + * @param The waypoint index. + * @return The duration from start; returns overall duration if index is out of range. + */ + double getWayPointDurationFromStart(std::size_t index) const; + + double getWayPointDurationFromPrevious(std::size_t index) const + { + if (duration_from_previous_.size() > index) + { + return duration_from_previous_[index]; + } + else + { + return 0.0; + } + } + + RobotTrajectory& setWayPointDurationFromPrevious(std::size_t index, double value) + { + if (duration_from_previous_.size() <= index) + duration_from_previous_.resize(index + 1, 0.0); + duration_from_previous_[index] = value; + return *this; + } + + bool empty() const + { + return waypoints_.empty(); + } + + /** + * \brief Add a point to the trajectory + * \param state - current robot state + * \param dt - duration from previous + */ + RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotState& state, double dt) + { + return addSuffixWayPoint(std::make_shared(state), dt); + } + + /** + * \brief Add a point to the trajectory + * \param state - current robot state + * \param dt - duration from previous + */ + RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt) + { + state->update(); + waypoints_.push_back(state); + duration_from_previous_.push_back(dt); + return *this; + } + + RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotState& state, double dt) + { + return addPrefixWayPoint(std::make_shared(state), dt); + } + + RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt) + { + state->update(); + waypoints_.push_front(state); + duration_from_previous_.push_front(dt); + return *this; + } + + RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt) + { + return insertWayPoint(index, std::make_shared(state), dt); + } + + RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt) + { + state->update(); + waypoints_.insert(waypoints_.begin() + index, state); + duration_from_previous_.insert(duration_from_previous_.begin() + index, dt); + return *this; + } + + /** + * \brief Add a specified part of a trajectory to the end of the current trajectory. The default (when \p start_index + * and \p end_index are omitted) is to add the whole trajectory. + * \param source - the trajectory containing the part to append to the end of current trajectory + * \param dt - time step between last traj point in current traj and first traj point of append traj + * \param start_index - index of first traj point of the part to append from the source traj, the default is to add + * from the start of the source traj + * \param end_index - index of last traj point of the part to append from the source traj, the default is to add until + * the end of the source traj + */ + RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0, + size_t end_index = std::numeric_limits::max()); + + void swap(robot_trajectory::RobotTrajectory& other) noexcept; + + /** + * \brief Remove a point from the trajectory + * \param index - the index to remove + */ + RobotTrajectory& removeWayPoint(std::size_t index) + { + waypoints_.erase(waypoints_.begin() + index); + duration_from_previous_.erase(duration_from_previous_.begin() + index); + return *this; + } + + RobotTrajectory& clear() + { + waypoints_.clear(); + duration_from_previous_.clear(); + return *this; + } + + double getDuration() const; + + double getAverageSegmentDuration() const; + + void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& joint_filter = std::vector()) const; + + /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required + to contain the values + for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Each + point in the trajectory + to be constructed internally is obtained by copying the reference state and overwriting the content from a + trajectory point in \e trajectory. */ + RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, + const trajectory_msgs::msg::JointTrajectory& trajectory); + + /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required + to contain the values + for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Each + point in the trajectory + to be constructed internally is obtained by copying the reference state and overwriting the content from a + trajectory point in \e trajectory. */ + RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, + const moveit_msgs::msg::RobotTrajectory& trajectory); + + /** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required + to contain the values + for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Before + use, the reference state is updated + using \e state. Each point in the trajectory to be constructed internally is obtained by copying the reference + state and overwriting the content + from a trajectory point in \e trajectory. */ + RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, + const moveit_msgs::msg::RobotState& state, + const moveit_msgs::msg::RobotTrajectory& trajectory); + + RobotTrajectory& reverse(); + + RobotTrajectory& unwind(); + + /** @brief Unwind, starting from an initial state **/ + RobotTrajectory& unwind(const moveit::core::RobotState& state); + + /** @brief Finds the waypoint indices before and after a duration from start. + * @param The duration from start. + * @param The waypoint index before the supplied duration. + * @param The waypoint index after (or equal to) the supplied duration. + * @param The progress (0 to 1) between the two waypoints, based on time (not based on joint distances). + */ + void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const; + + // TODO support visitor function for interpolation, or at least different types. + /** @brief Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time + * interpolation. + * @param The duration from start. + * @param The resulting robot state. + * @return True if state is valid, false otherwise (trajectory is empty). + */ + bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const; + + class Iterator + { + std::deque::iterator waypoint_iterator_; + std::deque::iterator duration_iterator_; + + public: + explicit Iterator(const std::deque::iterator& waypoint_iterator, + const std::deque::iterator& duration_iterator) + : waypoint_iterator_(waypoint_iterator), duration_iterator_(duration_iterator) + { + } + Iterator& operator++() + { + waypoint_iterator_++; + duration_iterator_++; + return *this; + } + Iterator operator++(int) + { + Iterator retval = *this; + ++(*this); + return retval; + } + bool operator==(const Iterator& other) const + { + return ((waypoint_iterator_ == other.waypoint_iterator_) && (duration_iterator_ == other.duration_iterator_)); + } + bool operator!=(const Iterator& other) const + { + return !(*this == other); + } + std::pair operator*() const + { + return std::pair{ *waypoint_iterator_, *duration_iterator_ }; + } + + // iterator traits + using difference_type = long; + using value_type = std::pair; + using pointer = const std::pair*; + using reference = std::pair; + using iterator_category = std::input_iterator_tag; + }; + + RobotTrajectory::Iterator begin() + { + assert(waypoints_.size() == duration_from_previous_.size()); + return Iterator(waypoints_.begin(), duration_from_previous_.begin()); + } + RobotTrajectory::Iterator end() + { + assert(waypoints_.size() == duration_from_previous_.size()); + return Iterator(waypoints_.end(), duration_from_previous_.end()); + } + /** @brief Print information about the trajectory + * @param out Stream to print to + * @param variable_indexes The indexes of the variables to print. + * If empty/not specified and the group is defined, then uses the indexes for the group + * If empty and the group is not defined, uses ALL variables in robot_model + * + * e.g. + * Trajectory has 13 points over 2.965 seconds + * waypoint 0 time 0.000 pos 0.000 vel 0.000 acc 0.000 + * waypoint 1 time 0.067 pos 0.001 vel 0.033 acc 1.000 + * waypoint 2 time 0.665 pos 0.200 vel 0.632 acc 1.000 + * ... + */ + void print(std::ostream& out, std::vector variable_indexes = std::vector()) const; + +private: + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* group_; + std::deque waypoints_; + std::deque duration_from_previous_; +}; + +/** @brief Operator overload for printing trajectory to a stream */ +std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); + +/// \brief Calculate the path length of a given trajectory based on the +/// accumulated robot state distances. +/// The distance between two robot states is calculated based on the sum of +/// active joint distances between the two states (L1 norm). +/// \param[in] trajectory Given robot trajectory +/// \return Length of the robot trajectory [rad] +[[nodiscard]] double pathLength(const RobotTrajectory& trajectory); + +/// \brief Calculate the smoothness of a given trajectory +/// \param[in] trajectory Given robot trajectory +/// \return Smoothness of the given trajectory +/// or nullopt if it is not possible to calculate the smoothness +[[nodiscard]] std::optional smoothness(const RobotTrajectory& trajectory); + +/// \brief Calculate the waypoint density of a trajectory +/// \param[in] trajectory Given robot trajectory +/// \return Waypoint density of the given trajectory +/// or nullopt if it is not possible to calculate the density +[[nodiscard]] std::optional waypointDensity(const RobotTrajectory& trajectory); + +/// \brief Converts a RobotTrajectory to a JointTrajectory message +// \param[in] trajectory Given robot trajectory +// \param[in] include_mdof_joints Treat Multi-DOF variables as joints, e.g. position/x position/y position/theta +// \param[in] joint_filter Exclude joints with the provided names +// \return JointTrajectory message including all waypoints +// or nullopt if the provided RobotTrajectory or RobotModel is empty +[[nodiscard]] std::optional +toJointTrajectory(const RobotTrajectory& trajectory, bool include_mdof_joints = false, + const std::vector& joint_filter = {}); +} // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 8e0eed38be..ee1fe5b351 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -35,8 +35,8 @@ /* Author: Ioan Sucan, Adam Leeper */ #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 8ffdcb1a8a..16793dd43b 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index 26038d1bbc..b383610ac4 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /******************************************************************************* * BSD 3-Clause License * @@ -34,133 +46,5 @@ /* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */ #pragma once - -#include -#include -#include -#include - -namespace trajectory_processing -{ -class RuckigSmoothing -{ -public: - /** - * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. - * \param[in,out] trajectory A path which needs smoothing. - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. - * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) - * \return true if successful. - */ - static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0, const bool mitigate_overshoot = false, - const double overshoot_threshold = 0.01); - - /** - * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. - * \param[in,out] trajectory A path which needs smoothing. - * \param velocity_limits Joint names and velocity limits in rad/s - * \param acceleration_limits Joint names and acceleration limits in rad/s^2 - * \param jerk_limits Joint names and jerk limits in rad/s^3 - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. - * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) - * \return true if successful. - */ - static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, - const std::unordered_map& velocity_limits, - const std::unordered_map& acceleration_limits, - const std::unordered_map& jerk_limits, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0, const bool mitigate_overshoot = false, - const double overshoot_threshold = 0.01); - - /** - * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. - * \param[in,out] trajectory A path which needs smoothing. - * \param joint_limits Joint names and corresponding velocity limits in rad/s, acceleration limits in rad/s^2, - * and jerk limits in rad/s^3 - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, - const std::vector& joint_limits, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0); - -private: - /** - * \brief A utility function to check if the group is defined. - * \param trajectory Trajectory to smooth. - */ - [[nodiscard]] static bool validateGroup(const robot_trajectory::RobotTrajectory& trajectory); - - /** - * \brief A utility function to get bounds from a JointModelGroup and save them for Ruckig. - * \param max_velocity_scaling_factor Scale all joint velocity limits by this factor. Usually 1.0. - * \param max_acceleration_scaling_factor Scale all joint acceleration limits by this factor. Usually 1.0. - * \param group The RobotModel and the limits are retrieved from this group. - * \param[out] ruckig_input The limits are stored in this ruckig::InputParameter, for use in Ruckig. - */ - [[nodiscard]] static bool getRobotModelBounds(const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor, - const moveit::core::JointModelGroup* const group, - ruckig::InputParameter& ruckig_input); - - /** - * \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint. - * \param current_waypoint The nominal current state - * \param next_waypoint The nominal, desired state at the next waypoint - * \param joint_group The MoveIt JointModelGroup of interest - * \param[out] ruckig_input The Rucking parameters for the next iteration - */ - static void getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint, - const moveit::core::RobotStateConstPtr& next_waypoint, - const moveit::core::JointModelGroup* joint_group, - ruckig::InputParameter& ruckig_input); - - /** - * \brief Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values - * \param first_waypoint The Ruckig input/output parameters are initialized to the values at this waypoint - * \param joint_group The MoveIt JointModelGroup of interest - * \param[out] rucking_input Input parameters to Ruckig. Initialized here. - */ - static void initializeRuckigState(const moveit::core::RobotState& first_waypoint, - const moveit::core::JointModelGroup* joint_group, - ruckig::InputParameter& ruckig_input); - - /** - * \brief A utility function to instantiate and run Ruckig for a series of waypoints. - * \param[in, out] trajectory Trajectory to smooth. - * \param[in, out] ruckig_input Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) - * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. - * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) - */ - [[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input, - const bool mitigate_overshoot = false, const double overshoot_threshold = 0.01); - - /** - * \brief Extend the duration of every trajectory segment - * \param[in] duration_extension_factor A number greater than 1. Extend every timestep by this much. - * \param[in] num_waypoints Number of waypoints in the trajectory. - * \param[in] num_dof Degrees of freedom in the manipulator. - * \param[in] move_group_idx For accessing the joints of interest out of the full RobotState. - * \param[in] original_trajectory Durations are extended based on the data in this original trajectory. - * \param[in, out] trajectory This trajectory will be returned with modified waypoint durations. - */ - static void extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints, - const size_t num_dof, const std::vector& move_group_idx, - const robot_trajectory::RobotTrajectory& original_trajectory, - robot_trajectory::RobotTrajectory& trajectory); - - /** \brief Check if a trajectory out of Ruckig overshoots the target state */ - static bool checkOvershoot(ruckig::Trajectory& ruckig_trajectory, - const size_t num_dof, ruckig::InputParameter& ruckig_input, - const double overshoot_threshold); -}; -} // namespace trajectory_processing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.hpp new file mode 100644 index 0000000000..b22ea520cc --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.hpp @@ -0,0 +1,166 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2021, PickNik Robotics + * 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 HOLDER 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: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */ + +#pragma once + +#include +#include +#include +#include + +namespace trajectory_processing +{ +class RuckigSmoothing +{ +public: + /** + * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. + * \param[in,out] trajectory A path which needs smoothing. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. + * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) + * \return true if successful. + */ + static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0, const bool mitigate_overshoot = false, + const double overshoot_threshold = 0.01); + + /** + * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. + * \param[in,out] trajectory A path which needs smoothing. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + * \param jerk_limits Joint names and jerk limits in rad/s^3 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. + * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) + * \return true if successful. + */ + static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits, + const std::unordered_map& jerk_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0, const bool mitigate_overshoot = false, + const double overshoot_threshold = 0.01); + + /** + * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. + * \param[in,out] trajectory A path which needs smoothing. + * \param joint_limits Joint names and corresponding velocity limits in rad/s, acceleration limits in rad/s^2, + * and jerk limits in rad/s^3 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0); + +private: + /** + * \brief A utility function to check if the group is defined. + * \param trajectory Trajectory to smooth. + */ + [[nodiscard]] static bool validateGroup(const robot_trajectory::RobotTrajectory& trajectory); + + /** + * \brief A utility function to get bounds from a JointModelGroup and save them for Ruckig. + * \param max_velocity_scaling_factor Scale all joint velocity limits by this factor. Usually 1.0. + * \param max_acceleration_scaling_factor Scale all joint acceleration limits by this factor. Usually 1.0. + * \param group The RobotModel and the limits are retrieved from this group. + * \param[out] ruckig_input The limits are stored in this ruckig::InputParameter, for use in Ruckig. + */ + [[nodiscard]] static bool getRobotModelBounds(const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor, + const moveit::core::JointModelGroup* const group, + ruckig::InputParameter& ruckig_input); + + /** + * \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint. + * \param current_waypoint The nominal current state + * \param next_waypoint The nominal, desired state at the next waypoint + * \param joint_group The MoveIt JointModelGroup of interest + * \param[out] ruckig_input The Rucking parameters for the next iteration + */ + static void getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint, + const moveit::core::RobotStateConstPtr& next_waypoint, + const moveit::core::JointModelGroup* joint_group, + ruckig::InputParameter& ruckig_input); + + /** + * \brief Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values + * \param first_waypoint The Ruckig input/output parameters are initialized to the values at this waypoint + * \param joint_group The MoveIt JointModelGroup of interest + * \param[out] rucking_input Input parameters to Ruckig. Initialized here. + */ + static void initializeRuckigState(const moveit::core::RobotState& first_waypoint, + const moveit::core::JointModelGroup* joint_group, + ruckig::InputParameter& ruckig_input); + + /** + * \brief A utility function to instantiate and run Ruckig for a series of waypoints. + * \param[in, out] trajectory Trajectory to smooth. + * \param[in, out] ruckig_input Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) + * \param mitigate_overshoot If true, overshoot is mitigated by extending trajectory duration. + * \param overshoot_threshold If an overshoot is greater than this, duration is extended (radians, for a single joint) + */ + [[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory, + ruckig::InputParameter& ruckig_input, + const bool mitigate_overshoot = false, const double overshoot_threshold = 0.01); + + /** + * \brief Extend the duration of every trajectory segment + * \param[in] duration_extension_factor A number greater than 1. Extend every timestep by this much. + * \param[in] num_waypoints Number of waypoints in the trajectory. + * \param[in] num_dof Degrees of freedom in the manipulator. + * \param[in] move_group_idx For accessing the joints of interest out of the full RobotState. + * \param[in] original_trajectory Durations are extended based on the data in this original trajectory. + * \param[in, out] trajectory This trajectory will be returned with modified waypoint durations. + */ + static void extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints, + const size_t num_dof, const std::vector& move_group_idx, + const robot_trajectory::RobotTrajectory& original_trajectory, + robot_trajectory::RobotTrajectory& trajectory); + + /** \brief Check if a trajectory out of Ruckig overshoots the target state */ + static bool checkOvershoot(ruckig::Trajectory& ruckig_trajectory, + const size_t num_dof, ruckig::InputParameter& ruckig_input, + const double overshoot_threshold); +}; +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 8f7b90ec03..2481703ba0 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /* * Copyright (c) 2011-2012, Georgia Tech Research Corporation * All rights reserved. @@ -37,266 +49,5 @@ */ #pragma once - -#include -#include -#include -#include - -namespace trajectory_processing -{ - -// The intermediate waypoints of the input path need to be blended so that the entire path is diffentiable. -// This constant defines the maximum deviation allowed at those intermediate waypoints, in radians for revolute joints, -// or meters for prismatic joints. -constexpr double DEFAULT_PATH_TOLERANCE = 0.1; - -enum LimitType -{ - VELOCITY, - ACCELERATION -}; - -const std::unordered_map LIMIT_TYPES = { { VELOCITY, "velocity" }, - { ACCELERATION, "acceleration" } }; -class PathSegment -{ -public: - PathSegment(double length = 0.0) : length_(length) - { - } - virtual ~PathSegment() // is required for destructing derived classes - { - } - double getLength() const - { - return length_; - } - virtual Eigen::VectorXd getConfig(double s) const = 0; - virtual Eigen::VectorXd getTangent(double s) const = 0; - virtual Eigen::VectorXd getCurvature(double s) const = 0; - virtual std::list getSwitchingPoints() const = 0; - virtual PathSegment* clone() const = 0; - - double position_; - -protected: - double length_; -}; - -class Path -{ -public: - // Create a Path from a vector of waypoints and a maximum deviation to tolerate at the intermediate waypoints. - // The algorithm needs max_deviation to be greater than zero so that the path is differentiable. - static std::optional create(const std::vector& waypoint, - double max_deviation = DEFAULT_PATH_TOLERANCE); - - // Copy constructor. - Path(const Path& path); - - double getLength() const; - Eigen::VectorXd getConfig(double s) const; - Eigen::VectorXd getTangent(double s) const; - Eigen::VectorXd getCurvature(double s) const; - - /** @brief Get the next switching point. - * @param[in] s Arc length traveled so far - * @param[out] discontinuity True if this switching point is a discontinuity - * @return arc length to the switching point - **/ - double getNextSwitchingPoint(double s, bool& discontinuity) const; - - /// @brief Return a list of all switching points as a pair (arc length to switching point, discontinuity) - std::list> getSwitchingPoints() const; - -private: - // Default constructor private to prevent misuse. Use `create` instead to create a Path object. - Path() = default; - - PathSegment* getPathSegment(double& s) const; - - double length_ = 0.0; - std::list> switching_points_; - std::list> path_segments_; -}; - -class Trajectory -{ -public: - /// @brief Generates a time-optimal trajectory. - /// @returns std::nullopt if the trajectory couldn't be parameterized. - static std::optional create(const Path& path, const Eigen::VectorXd& max_velocity, - const Eigen::VectorXd& max_acceleration, double time_step = 0.001); - - /// @brief Returns the optimal duration of the trajectory - double getDuration() const; - - /** @brief Return the position/configuration vector for a given point in time - */ - Eigen::VectorXd getPosition(double time) const; - /** @brief Return the velocity vector for a given point in time */ - Eigen::VectorXd getVelocity(double time) const; - /** @brief Return the acceleration vector for a given point in time */ - Eigen::VectorXd getAcceleration(double time) const; - -private: - Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, - double time_step); - - struct TrajectoryStep - { - TrajectoryStep() - { - } - TrajectoryStep(double path_pos, double path_vel) : path_pos_(path_pos), path_vel_(path_vel) - { - } - double path_pos_; - double path_vel_; - double time_; - }; - - bool getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration, - double& after_acceleration); - bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, - double& before_acceleration, double& after_acceleration); - bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration, - double& after_acceleration); - bool integrateForward(std::list& trajectory, double acceleration); - void integrateBackward(std::list& start_trajectory, double path_pos, double path_vel, - double acceleration); - double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max); - double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max); - double getAccelerationMaxPathVelocity(double path_pos) const; - double getVelocityMaxPathVelocity(double path_pos) const; - double getAccelerationMaxPathVelocityDeriv(double path_pos); - double getVelocityMaxPathVelocityDeriv(double path_pos); - - std::list::const_iterator getTrajectorySegment(double time) const; - - Path path_; - Eigen::VectorXd max_velocity_; - Eigen::VectorXd max_acceleration_; - unsigned int joint_num_ = 0.0; - bool valid_ = true; - std::list trajectory_; - std::list end_trajectory_; // non-empty only if the trajectory generation failed. - - double time_step_ = 0.0; - - mutable double cached_time_ = std::numeric_limits::max(); - mutable std::list::const_iterator cached_trajectory_segment_; -}; - -MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration); -class TimeOptimalTrajectoryGeneration : public TimeParameterization -{ -public: - TimeOptimalTrajectoryGeneration(const double path_tolerance = DEFAULT_PATH_TOLERANCE, const double resample_dt = 0.1, - const double min_angle_change = 0.001); - - // clang-format off -/** - * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). - * Resampling the trajectory doesn't change the start and goal point, - * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). - * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. - * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, - * meters for prismatic joints. - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - // clang-format on - bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const override; - - // clang-format off -/** - * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). - * Resampling the trajectory doesn't change the start and goal point, - * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). - * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. - * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, - * meters for prismatic joints. - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param velocity_limits Joint names and velocity limits in rad/s - * \param acceleration_limits Joint names and acceleration limits in rad/s^2 - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - // clang-format on - bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const std::unordered_map& velocity_limits, - const std::unordered_map& acceleration_limits, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const override; - - // clang-format off -/** - * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). - * Resampling the trajectory doesn't change the start and goal point, - * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). - * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. - * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, - * meters for prismatic joints. - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2 - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - // clang-format on - bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const std::vector& joint_limits, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const override; - -private: - bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, - const Eigen::VectorXd& max_velocity, - const Eigen::VectorXd& max_acceleration) const; - - /** - * @brief Check if a combination of revolute and prismatic joints is used. path_tolerance_ is not valid, if so. - * \param group The JointModelGroup to check. - * \return true if there are mixed joints. - */ - bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const; - - /** - * @brief Check if the requested scaling factor is valid and if not, return 1.0. - * \param requested_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits - * \param limit_type Whether the velocity or acceleration scaling factor is being verified - * \return The user requested scaling factor, if it is valid. Otherwise, return 1.0. - */ - double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const; - - const double path_tolerance_; - const double resample_dt_; - const double min_angle_change_; -}; - -// clang-format off -/** - * \brief Compute a trajectory with the desired number of waypoints. - * Resampling the trajectory doesn't change the start and goal point, - * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). - * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. - * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, - * meters for prismatic joints. - * This is a free function because it needs to modify the const resample_dt_ member of TimeOptimalTrajectoryGeneration class. - * \param num_waypoints The desired number of waypoints (plus or minus one due to numerical rounding). - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ -// clang-format on -bool totgComputeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTrajectory& trajectory, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0); -} // namespace trajectory_processing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp new file mode 100644 index 0000000000..cba0df41ef --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp @@ -0,0 +1,302 @@ +/* + * Copyright (c) 2011-2012, Georgia Tech Research Corporation + * All rights reserved. + * + * Author: Tobias Kunz + * Date: 05/2012 + * + * Humanoid Robotics Lab Georgia Institute of Technology + * Director: Mike Stilman http://www.golems.org + * + * Algorithm details and publications: + * http://www.golems.org/node/1570 + * + * This file is provided under the following "BSD-style" License: + * 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. + * 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 HOLDER 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. + */ + +#pragma once + +#include +#include +#include +#include + +namespace trajectory_processing +{ + +// The intermediate waypoints of the input path need to be blended so that the entire path is diffentiable. +// This constant defines the maximum deviation allowed at those intermediate waypoints, in radians for revolute joints, +// or meters for prismatic joints. +constexpr double DEFAULT_PATH_TOLERANCE = 0.1; + +enum LimitType +{ + VELOCITY, + ACCELERATION +}; + +const std::unordered_map LIMIT_TYPES = { { VELOCITY, "velocity" }, + { ACCELERATION, "acceleration" } }; +class PathSegment +{ +public: + PathSegment(double length = 0.0) : length_(length) + { + } + virtual ~PathSegment() // is required for destructing derived classes + { + } + double getLength() const + { + return length_; + } + virtual Eigen::VectorXd getConfig(double s) const = 0; + virtual Eigen::VectorXd getTangent(double s) const = 0; + virtual Eigen::VectorXd getCurvature(double s) const = 0; + virtual std::list getSwitchingPoints() const = 0; + virtual PathSegment* clone() const = 0; + + double position_; + +protected: + double length_; +}; + +class Path +{ +public: + // Create a Path from a vector of waypoints and a maximum deviation to tolerate at the intermediate waypoints. + // The algorithm needs max_deviation to be greater than zero so that the path is differentiable. + static std::optional create(const std::vector& waypoint, + double max_deviation = DEFAULT_PATH_TOLERANCE); + + // Copy constructor. + Path(const Path& path); + + double getLength() const; + Eigen::VectorXd getConfig(double s) const; + Eigen::VectorXd getTangent(double s) const; + Eigen::VectorXd getCurvature(double s) const; + + /** @brief Get the next switching point. + * @param[in] s Arc length traveled so far + * @param[out] discontinuity True if this switching point is a discontinuity + * @return arc length to the switching point + **/ + double getNextSwitchingPoint(double s, bool& discontinuity) const; + + /// @brief Return a list of all switching points as a pair (arc length to switching point, discontinuity) + std::list> getSwitchingPoints() const; + +private: + // Default constructor private to prevent misuse. Use `create` instead to create a Path object. + Path() = default; + + PathSegment* getPathSegment(double& s) const; + + double length_ = 0.0; + std::list> switching_points_; + std::list> path_segments_; +}; + +class Trajectory +{ +public: + /// @brief Generates a time-optimal trajectory. + /// @returns std::nullopt if the trajectory couldn't be parameterized. + static std::optional create(const Path& path, const Eigen::VectorXd& max_velocity, + const Eigen::VectorXd& max_acceleration, double time_step = 0.001); + + /// @brief Returns the optimal duration of the trajectory + double getDuration() const; + + /** @brief Return the position/configuration vector for a given point in time + */ + Eigen::VectorXd getPosition(double time) const; + /** @brief Return the velocity vector for a given point in time */ + Eigen::VectorXd getVelocity(double time) const; + /** @brief Return the acceleration vector for a given point in time */ + Eigen::VectorXd getAcceleration(double time) const; + +private: + Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, + double time_step); + + struct TrajectoryStep + { + TrajectoryStep() + { + } + TrajectoryStep(double path_pos, double path_vel) : path_pos_(path_pos), path_vel_(path_vel) + { + } + double path_pos_; + double path_vel_; + double time_; + }; + + bool getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration, + double& after_acceleration); + bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, + double& before_acceleration, double& after_acceleration); + bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration, + double& after_acceleration); + bool integrateForward(std::list& trajectory, double acceleration); + void integrateBackward(std::list& start_trajectory, double path_pos, double path_vel, + double acceleration); + double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max); + double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max); + double getAccelerationMaxPathVelocity(double path_pos) const; + double getVelocityMaxPathVelocity(double path_pos) const; + double getAccelerationMaxPathVelocityDeriv(double path_pos); + double getVelocityMaxPathVelocityDeriv(double path_pos); + + std::list::const_iterator getTrajectorySegment(double time) const; + + Path path_; + Eigen::VectorXd max_velocity_; + Eigen::VectorXd max_acceleration_; + unsigned int joint_num_ = 0.0; + bool valid_ = true; + std::list trajectory_; + std::list end_trajectory_; // non-empty only if the trajectory generation failed. + + double time_step_ = 0.0; + + mutable double cached_time_ = std::numeric_limits::max(); + mutable std::list::const_iterator cached_trajectory_segment_; +}; + +MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration); +class TimeOptimalTrajectoryGeneration : public TimeParameterization +{ +public: + TimeOptimalTrajectoryGeneration(const double path_tolerance = DEFAULT_PATH_TOLERANCE, const double resample_dt = 0.1, + const double min_angle_change = 0.001); + + // clang-format off +/** + * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, + * meters for prismatic joints. + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + // clang-format on + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const override; + + // clang-format off +/** + * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, + * meters for prismatic joints. + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + // clang-format on + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const override; + + // clang-format off +/** + * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, + * meters for prismatic joints. + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + // clang-format on + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const override; + +private: + bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, + const Eigen::VectorXd& max_velocity, + const Eigen::VectorXd& max_acceleration) const; + + /** + * @brief Check if a combination of revolute and prismatic joints is used. path_tolerance_ is not valid, if so. + * \param group The JointModelGroup to check. + * \return true if there are mixed joints. + */ + bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const; + + /** + * @brief Check if the requested scaling factor is valid and if not, return 1.0. + * \param requested_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits + * \param limit_type Whether the velocity or acceleration scaling factor is being verified + * \return The user requested scaling factor, if it is valid. Otherwise, return 1.0. + */ + double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const; + + const double path_tolerance_; + const double resample_dt_; + const double min_angle_change_; +}; + +// clang-format off +/** + * \brief Compute a trajectory with the desired number of waypoints. + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, + * meters for prismatic joints. + * This is a free function because it needs to modify the const resample_dt_ member of TimeOptimalTrajectoryGeneration class. + * \param num_waypoints The desired number of waypoints (plus or minus one due to numerical rounding). + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ +// clang-format on +bool totgComputeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTrajectory& trajectory, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0); +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h index ece7e9ea1e..9d73c3f9a7 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -1,60 +1,15 @@ -#pragma once - -#include - -namespace trajectory_processing -{ -/** - * @brief Base class for trajectory parameterization algorithms - */ -MOVEIT_CLASS_FORWARD(TimeParameterization); -class TimeParameterization -{ -public: - TimeParameterization() = default; - TimeParameterization(const TimeParameterization&) = default; - TimeParameterization(TimeParameterization&&) = default; - TimeParameterization& operator=(const TimeParameterization&) = default; - TimeParameterization& operator=(TimeParameterization&&) = default; - virtual ~TimeParameterization() = default; - /** - * \brief Compute a trajectory with waypoints spaced equally in time - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const = 0; - - /** - * \brief Compute a trajectory with waypoints spaced equally in time - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param velocity_limits Joint names and velocity limits in rad/s - * \param acceleration_limits Joint names and acceleration limits in rad/s^2 - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const std::unordered_map& velocity_limits, - const std::unordered_map& acceleration_limits, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const = 0; - - /** - * \brief Compute a trajectory with waypoints spaced equally in time - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2 - * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. - */ - virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const std::vector& joint_limits, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const = 0; -}; -} // namespace trajectory_processing +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.hpp new file mode 100644 index 0000000000..c9854bd43c --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include + +namespace trajectory_processing +{ +/** + * @brief Base class for trajectory parameterization algorithms + */ +MOVEIT_CLASS_FORWARD(TimeParameterization); +class TimeParameterization +{ +public: + TimeParameterization() = default; + TimeParameterization(const TimeParameterization&) = default; + TimeParameterization(TimeParameterization&&) = default; + TimeParameterization& operator=(const TimeParameterization&) = default; + TimeParameterization& operator=(TimeParameterization&&) = default; + virtual ~TimeParameterization() = default; + + /** + * \brief Compute a trajectory with waypoints spaced equally in time + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; + + /** + * \brief Compute a trajectory with waypoints spaced equally in time + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; + + /** + * \brief Compute a trajectory with waypoints spaced equally in time + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; +}; +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 876f3af06c..82944432e8 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,56 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace trajectory_processing -{ -/** - * \brief Checks if a robot trajectory is empty. - * \param [in] trajectory The robot trajectory to check. - * \return True if the trajectory is empty, false otherwise. - */ -bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory); -/** - * \brief Returns the number of waypoints in a robot trajectory. - * \param [in] trajectory The robot trajectory to count waypoints in. - * \return The number of waypoints in the trajectory. - */ -std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& trajectory); -/** - * \brief Applies time parameterization to a robot trajectory using the Time-Optimal Trajectory Generation (TOTG) - * algorithm. - * \param [in,out] trajectory The robot trajectory to be time parameterized. - * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory. - * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory. - * \param [in] path_tolerance The path tolerance to use for time parameterization (default: 0.1). - * \param [in] resample_dt The time step to use for time parameterization (default: 0.1). - * \param [in] min_angle_change The minimum angle change to use for time parameterization (default: 0.001). - * \return True if time parameterization was successful, false otherwise. - */ -bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, - double acceleration_scaling_factor, double path_tolerance = 0.1, - double resample_dt = 0.1, double min_angle_change = 0.001); -/** - * \brief Applies Ruckig smoothing to a robot trajectory. - * \param [in,out] trajectory The robot trajectory to be smoothed. - * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory. - * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory. - * \param [in] mitigate_overshoot Whether to mitigate overshoot during smoothing (default: false). - * \param [in] overshoot_threshold The maximum allowed overshoot during smoothing (default: 0.01). - * \return True if smoothing was successful, false otherwise. - */ -bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, - double acceleration_scaling_factor, bool mitigate_overshoot = false, - double overshoot_threshold = 0.01); - -/** - * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. - */ -[[nodiscard]] trajectory_msgs::msg::JointTrajectory -createTrajectoryMessage(const std::vector& joint_names, - const trajectory_processing::Trajectory& trajectory, const int sampling_rate); -} // namespace trajectory_processing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp new file mode 100644 index 0000000000..daa1f1f00b --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +namespace trajectory_processing +{ +/** + * \brief Checks if a robot trajectory is empty. + * \param [in] trajectory The robot trajectory to check. + * \return True if the trajectory is empty, false otherwise. + */ +bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory); +/** + * \brief Returns the number of waypoints in a robot trajectory. + * \param [in] trajectory The robot trajectory to count waypoints in. + * \return The number of waypoints in the trajectory. + */ +std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& trajectory); +/** + * \brief Applies time parameterization to a robot trajectory using the Time-Optimal Trajectory Generation (TOTG) + * algorithm. + * \param [in,out] trajectory The robot trajectory to be time parameterized. + * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory. + * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory. + * \param [in] path_tolerance The path tolerance to use for time parameterization (default: 0.1). + * \param [in] resample_dt The time step to use for time parameterization (default: 0.1). + * \param [in] min_angle_change The minimum angle change to use for time parameterization (default: 0.001). + * \return True if time parameterization was successful, false otherwise. + */ +bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, double path_tolerance = 0.1, + double resample_dt = 0.1, double min_angle_change = 0.001); +/** + * \brief Applies Ruckig smoothing to a robot trajectory. + * \param [in,out] trajectory The robot trajectory to be smoothed. + * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory. + * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory. + * \param [in] mitigate_overshoot Whether to mitigate overshoot during smoothing (default: false). + * \param [in] overshoot_threshold The maximum allowed overshoot during smoothing (default: 0.01). + * \return True if smoothing was successful, false otherwise. + */ +bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, bool mitigate_overshoot = false, + double overshoot_threshold = 0.01); + +/** + * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. + */ +[[nodiscard]] trajectory_msgs::msg::JointTrajectory +createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, const int sampling_rate); +} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 84c27c8c22..db1e4c1285 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index ad9225b34a..091ba1ab42 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index cca3586187..f89dae256f 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp b/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp index c0eb07be11..413d37cdef 100644 --- a/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp +++ b/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp @@ -37,11 +37,11 @@ // To run this benchmark, 'cd' to the build/moveit_core/trajectory_processing directory and directly run the binary. #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include // Robot and planning group to use in the benchmarks. constexpr char TEST_ROBOT[] = "panda"; diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp index e7d2ef6737..6d1ae7c8c0 100644 --- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp @@ -34,9 +34,9 @@ /* Author: Andy Zelenak */ #include -#include -#include -#include +#include +#include +#include namespace { diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index ba3f9681cb..9c0ae63ddd 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -37,8 +37,8 @@ */ #include -#include -#include +#include +#include using trajectory_processing::Path; using trajectory_processing::TimeOptimalTrajectoryGeneration; diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 7b31fbb4f4..df62410bf2 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,180 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -namespace moveit -{ -namespace core -{ -MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr... etc - -/// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning -/// frame -using FixedTransformsMap = std::map, - Eigen::aligned_allocator > >; - -/** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for - transforming different quantities. Transforms are maintained as a list of transforms to a particular frame. - All stored transforms are considered fixed. */ -class Transforms -{ -public: - /** - * @brief Transforms cannot be copy-constructed - */ - Transforms(const Transforms&) = delete; - - /** - * @brief Transforms cannot be copy-assigned - */ - Transforms& operator=(const Transforms&) = delete; - - /** - * @brief Construct a transform list - */ - Transforms(const std::string& target_frame); - - /** - * @brief Destructor - */ - virtual ~Transforms(); - - /** \brief Check if two frames end up being the same once the missing / are added as prefix (if they are missing) */ - static bool sameFrame(const std::string& frame1, const std::string& frame2); - - /** - * @brief Get the planning frame corresponding to this set of transforms - * @return The planning frame - */ - const std::string& getTargetFrame() const; - - /** - * \name Setting and retrieving transforms maintained in this class - */ - /**@{*/ - - /** - * @brief Return all the transforms - * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame). The - * transforms are guaranteed to be valid isometries. - */ - const FixedTransformsMap& getAllTransforms() const; - - /** - * @brief Get a vector of all the transforms as ROS messages - * @param transforms The output transforms. They are guaranteed to be valid isometries. - */ - void copyTransforms(std::vector& transforms) const; - - /** - * @brief Set a transform in the transform tree (adding it if necessary) - * @param t The input transform (w.r.t the target frame) - * @param from_frame The frame for which the input transform is specified - */ - void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame); - - /** - * @brief Set a transform in the transform tree (adding it if necessary) - * @param transform The input transform (the frame_id must match the target frame) - */ - void setTransform(const geometry_msgs::msg::TransformStamped& transform); - - /** - * @brief Set a transform in the transform tree (adding it if necessary) - * @param transform The input transforms (the frame_id must match the target frame) - */ - void setTransforms(const std::vector& transforms); - - /** - * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the - * planning frame) - */ - void setAllTransforms(const FixedTransformsMap& transforms); - - /**@}*/ - - /** - * \name Applying transforms - */ - /**@{*/ - - /** - * @brief Transform a vector in from_frame to the target_frame - * Note: assumes that v_in and v_out are "free" vectors, not points - * @param from_frame The frame from which the transform is computed - * @param v_in The input vector (in from_frame) - * @param v_out The resultant (transformed) vector - */ - void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const - { - // getTransform() returns a valid isometry by contract - v_out = getTransform(from_frame).linear() * v_in; - } - - /** - * @brief Transform a quaternion in from_frame to the target_frame - * @param from_frame The frame in which the input quaternion is specified - * @param v_in The input quaternion (in from_frame). Make sure the quaternion is normalized. - * @param v_out The resultant (transformed) quaternion. It is guaranteed to be a valid and normalized quaternion. - */ - void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, - Eigen::Quaterniond& q_out) const - { - // getTransform() returns a valid isometry by contract - q_out = getTransform(from_frame).linear() * q_in; - } - - /** - * @brief Transform a rotation matrix in from_frame to the target_frame - * @param from_frame The frame in which the input rotation matrix is specified - * @param m_in The input rotation matrix (in from_frame). Make sure the matrix is a valid rotation matrix. - * @param m_out The resultant (transformed) rotation matrix. It is guaranteed to be a valid rotation matrix. - */ - void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const - { - // getTransform() returns a valid isometry by contract - m_out = getTransform(from_frame).linear() * m_in; - } - - /** - * @brief Transform a pose in from_frame to the target_frame - * @param from_frame The frame in which the input pose is specified - * @param t_in The input pose (in from_frame). Make sure the pose is a valid isometry. - * @param t_out The resultant (transformed) pose. It is guaranteed to be a valid isometry. - */ - void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const - { - // getTransform() returns a valid isometry by contract - t_out = getTransform(from_frame) * t_in; - } - /**@}*/ - - /** - * @brief Check whether data can be transformed from a particular frame - */ - virtual bool canTransform(const std::string& from_frame) const; - - /** - * @brief Check whether a frame stays constant as the state of the robot model changes. - * This is true for any transform mainatined by this object. - */ - virtual bool isFixedFrame(const std::string& frame) const; - - /** - * @brief Get transform for from_frame (w.r.t target frame) - * @param from_frame The string id of the frame for which the transform is being computed - * @return The required transform. It is guaranteed to be a valid isometry. - */ - virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; - -protected: - std::string target_frame_; - FixedTransformsMap transforms_map_; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.hpp b/moveit_core/transforms/include/moveit/transforms/transforms.hpp new file mode 100644 index 0000000000..589c12ea2e --- /dev/null +++ b/moveit_core/transforms/include/moveit/transforms/transforms.hpp @@ -0,0 +1,214 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr... etc + +/// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning +/// frame +using FixedTransformsMap = std::map, + Eigen::aligned_allocator > >; + +/** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for + transforming different quantities. Transforms are maintained as a list of transforms to a particular frame. + All stored transforms are considered fixed. */ +class Transforms +{ +public: + /** + * @brief Transforms cannot be copy-constructed + */ + Transforms(const Transforms&) = delete; + + /** + * @brief Transforms cannot be copy-assigned + */ + Transforms& operator=(const Transforms&) = delete; + + /** + * @brief Construct a transform list + */ + Transforms(const std::string& target_frame); + + /** + * @brief Destructor + */ + virtual ~Transforms(); + + /** \brief Check if two frames end up being the same once the missing / are added as prefix (if they are missing) */ + static bool sameFrame(const std::string& frame1, const std::string& frame2); + + /** + * @brief Get the planning frame corresponding to this set of transforms + * @return The planning frame + */ + const std::string& getTargetFrame() const; + + /** + * \name Setting and retrieving transforms maintained in this class + */ + /**@{*/ + + /** + * @brief Return all the transforms + * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame). The + * transforms are guaranteed to be valid isometries. + */ + const FixedTransformsMap& getAllTransforms() const; + + /** + * @brief Get a vector of all the transforms as ROS messages + * @param transforms The output transforms. They are guaranteed to be valid isometries. + */ + void copyTransforms(std::vector& transforms) const; + + /** + * @brief Set a transform in the transform tree (adding it if necessary) + * @param t The input transform (w.r.t the target frame) + * @param from_frame The frame for which the input transform is specified + */ + void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame); + + /** + * @brief Set a transform in the transform tree (adding it if necessary) + * @param transform The input transform (the frame_id must match the target frame) + */ + void setTransform(const geometry_msgs::msg::TransformStamped& transform); + + /** + * @brief Set a transform in the transform tree (adding it if necessary) + * @param transform The input transforms (the frame_id must match the target frame) + */ + void setTransforms(const std::vector& transforms); + + /** + * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the + * planning frame) + */ + void setAllTransforms(const FixedTransformsMap& transforms); + + /**@}*/ + + /** + * \name Applying transforms + */ + /**@{*/ + + /** + * @brief Transform a vector in from_frame to the target_frame + * Note: assumes that v_in and v_out are "free" vectors, not points + * @param from_frame The frame from which the transform is computed + * @param v_in The input vector (in from_frame) + * @param v_out The resultant (transformed) vector + */ + void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const + { + // getTransform() returns a valid isometry by contract + v_out = getTransform(from_frame).linear() * v_in; + } + + /** + * @brief Transform a quaternion in from_frame to the target_frame + * @param from_frame The frame in which the input quaternion is specified + * @param v_in The input quaternion (in from_frame). Make sure the quaternion is normalized. + * @param v_out The resultant (transformed) quaternion. It is guaranteed to be a valid and normalized quaternion. + */ + void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, + Eigen::Quaterniond& q_out) const + { + // getTransform() returns a valid isometry by contract + q_out = getTransform(from_frame).linear() * q_in; + } + + /** + * @brief Transform a rotation matrix in from_frame to the target_frame + * @param from_frame The frame in which the input rotation matrix is specified + * @param m_in The input rotation matrix (in from_frame). Make sure the matrix is a valid rotation matrix. + * @param m_out The resultant (transformed) rotation matrix. It is guaranteed to be a valid rotation matrix. + */ + void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const + { + // getTransform() returns a valid isometry by contract + m_out = getTransform(from_frame).linear() * m_in; + } + + /** + * @brief Transform a pose in from_frame to the target_frame + * @param from_frame The frame in which the input pose is specified + * @param t_in The input pose (in from_frame). Make sure the pose is a valid isometry. + * @param t_out The resultant (transformed) pose. It is guaranteed to be a valid isometry. + */ + void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const + { + // getTransform() returns a valid isometry by contract + t_out = getTransform(from_frame) * t_in; + } + /**@}*/ + + /** + * @brief Check whether data can be transformed from a particular frame + */ + virtual bool canTransform(const std::string& from_frame) const; + + /** + * @brief Check whether a frame stays constant as the state of the robot model changes. + * This is true for any transform mainatined by this object. + */ + virtual bool isFixedFrame(const std::string& frame) const; + + /** + * @brief Get transform for from_frame (w.r.t target frame) + * @param from_frame The string id of the frame for which the transform is being computed + * @return The required transform. It is guaranteed to be a valid isometry. + */ + virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; + +protected: + std::string target_frame_; + FixedTransformsMap transforms_map_; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 14a57b2c6e..808f18e5d6 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/transforms/test/test_transforms.cpp b/moveit_core/transforms/test/test_transforms.cpp index 59725dba3e..e00a1a8816 100644 --- a/moveit_core/transforms/test/test_transforms.cpp +++ b/moveit_core/transforms/test/test_transforms.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_core/utils/include/moveit/utils/eigen_test_utils.h b/moveit_core/utils/include/moveit/utils/eigen_test_utils.h index 749ed17d2d..235d4096d9 100644 --- a/moveit_core/utils/include/moveit/utils/eigen_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/eigen_test_utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -34,44 +46,6 @@ /* Author: Robert Haschke */ -#include -#include -#include - -/** Provide operator<< for Eigen::Transform */ -template -std::ostream& operator<<(std::ostream& s, const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t) -{ - return s << "p=[" << t.translation().transpose() << "] q=[" << Eigen::Quaterniond(t.linear()).coeffs().transpose() - << "]"; -} - -/** Predicate to compare two Eigen entities */ -template -struct IsApprox -{ - double prec_; - IsApprox(double prec_) : prec_(prec_) - { - } - - ::testing::AssertionResult operator()(const char* expr1, const char* expr2, T1 val1, T2 val2) - { - if (val1.isApprox(val2, prec_)) - return ::testing::AssertionSuccess(); - - std::stringstream msg; - msg << "Expected equality of these values (up to precision " << prec_ << "):" << std::fixed - << std::setprecision(1 - std::log10(prec_)) // limit to precision - << "\n " << expr1 << "\n Which is: " << val1 // first - << "\n " << expr2 << "\n Which is: " << val2; // second - return ::testing::AssertionFailure() << msg.str(); - } -}; - -#define EXPECT_EIGEN_EQ(val1, val2) \ - EXPECT_PRED_FORMAT2((IsApprox( \ - Eigen::NumTraits::type::Scalar>::dummy_precision())), \ - val1, val2) -#define EXPECT_EIGEN_NEAR(val1, val2, prec_) \ - EXPECT_PRED_FORMAT2((IsApprox(prec_)), val1, val2) +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/eigen_test_utils.hpp b/moveit_core/utils/include/moveit/utils/eigen_test_utils.hpp new file mode 100644 index 0000000000..f773db1226 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/eigen_test_utils.hpp @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Bielefeld University + * 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 Bielefeld University 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: Robert Haschke */ + +#pragma once + +#include +#include +#include + +/** Provide operator<< for Eigen::Transform */ +template +std::ostream& operator<<(std::ostream& s, const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t) +{ + return s << "p=[" << t.translation().transpose() << "] q=[" << Eigen::Quaterniond(t.linear()).coeffs().transpose() + << "]"; +} + +/** Predicate to compare two Eigen entities */ +template +struct IsApprox +{ + double prec_; + IsApprox(double prec_) : prec_(prec_) + { + } + + ::testing::AssertionResult operator()(const char* expr1, const char* expr2, T1 val1, T2 val2) + { + if (val1.isApprox(val2, prec_)) + return ::testing::AssertionSuccess(); + + std::stringstream msg; + msg << "Expected equality of these values (up to precision " << prec_ << "):" << std::fixed + << std::setprecision(1 - std::log10(prec_)) // limit to precision + << "\n " << expr1 << "\n Which is: " << val1 // first + << "\n " << expr2 << "\n Which is: " << val2; // second + return ::testing::AssertionFailure() << msg.str(); + } +}; + +#define EXPECT_EIGEN_EQ(val1, val2) \ + EXPECT_PRED_FORMAT2((IsApprox( \ + Eigen::NumTraits::type::Scalar>::dummy_precision())), \ + val1, val2) +#define EXPECT_EIGEN_NEAR(val1, val2, prec_) \ + EXPECT_PRED_FORMAT2((IsApprox(prec_)), val1, val2) diff --git a/moveit_core/utils/include/moveit/utils/lexical_casts.h b/moveit_core/utils/include/moveit/utils/lexical_casts.h index 92e53a677e..f698d2eca8 100644 --- a/moveit_core/utils/include/moveit/utils/lexical_casts.h +++ b/moveit_core/utils/include/moveit/utils/lexical_casts.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,35 +47,5 @@ /* Author: Simon Schmeisser */ #pragma once - -/** \file lexical_casts.h - * \brief locale-agnostic conversion functions from floating point numbers to strings - * - * Depending on the system locale, a different decimal separator might be used - * for floating point numbers. This is often not wanted for internal (ie non-user - * facing) purposes. This module provides conversion functions that use std::locale::classic() - * (i.e. the default if no locale is set on the system). - */ - -#include -namespace moveit -{ -namespace core -{ -/** \brief Convert a double to std::string using the classic C locale */ -std::string toString(double d); - -/** \brief Convert a float to std::string using the classic C locale */ -std::string toString(float f); - -/** \brief Converts a std::string to double using the classic C locale - \throws std::runtime_exception if not a valid number -*/ -double toDouble(const std::string& s); - -/** \brief Converts a std::string to float using the classic C locale - \throws std::runtime_exception if not a valid number -*/ -float toFloat(const std::string& s); -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/lexical_casts.hpp b/moveit_core/utils/include/moveit/utils/lexical_casts.hpp new file mode 100644 index 0000000000..92e53a677e --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/lexical_casts.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, isys vision, GmbH. + * 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: Simon Schmeisser */ + +#pragma once + +/** \file lexical_casts.h + * \brief locale-agnostic conversion functions from floating point numbers to strings + * + * Depending on the system locale, a different decimal separator might be used + * for floating point numbers. This is often not wanted for internal (ie non-user + * facing) purposes. This module provides conversion functions that use std::locale::classic() + * (i.e. the default if no locale is set on the system). + */ + +#include +namespace moveit +{ +namespace core +{ +/** \brief Convert a double to std::string using the classic C locale */ +std::string toString(double d); + +/** \brief Convert a float to std::string using the classic C locale */ +std::string toString(float f); + +/** \brief Converts a std::string to double using the classic C locale + \throws std::runtime_exception if not a valid number +*/ +double toDouble(const std::string& s); + +/** \brief Converts a std::string to float using the classic C locale + \throws std::runtime_exception if not a valid number +*/ +float toFloat(const std::string& s); +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/logger.h b/moveit_core/utils/include/moveit/utils/logger.h new file mode 100644 index 0000000000..7f2c3490fa --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/logger.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Robotics 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 PickNik Robotics Inc. 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: Tyler Weaver */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/message_checks.h b/moveit_core/utils/include/moveit/utils/message_checks.h index a2a9313919..83b721c2dd 100644 --- a/moveit_core/utils/include/moveit/utils/message_checks.h +++ b/moveit_core/utils/include/moveit/utils/message_checks.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,39 +47,5 @@ /* Author: Michael 'v4hn' Goerner */ #pragma once - -/** \file empty_msgs.h - * \brief Checks for empty MoveIt-related messages - * - */ - -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace core -{ -/** \brief Check if a message includes any information about a planning scene, or whether it is empty. */ -bool isEmpty(const moveit_msgs::msg::PlanningScene& msg); - -/** \brief Check if a message includes any information about a planning scene world, or whether it is empty. */ -bool isEmpty(const moveit_msgs::msg::PlanningSceneWorld& msg); - -/** \brief Check if a message includes any information about a robot state, or whether it is empty. */ -bool isEmpty(const moveit_msgs::msg::RobotState& msg); - -/** \brief Check if a message specifies constraints */ -bool isEmpty(const moveit_msgs::msg::Constraints& msg); - -/** \brief Check if the message contains any non-zero entries */ -bool isEmpty(const geometry_msgs::msg::Quaternion& msg); - -/** \brief Check if the message contains any non-zero entries */ -bool isEmpty(const geometry_msgs::msg::Pose& msg); -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/message_checks.hpp b/moveit_core/utils/include/moveit/utils/message_checks.hpp new file mode 100644 index 0000000000..a2a9313919 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/message_checks.hpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Universitaet Hamburg. + * 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: Michael 'v4hn' Goerner */ + +#pragma once + +/** \file empty_msgs.h + * \brief Checks for empty MoveIt-related messages + * + */ + +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +/** \brief Check if a message includes any information about a planning scene, or whether it is empty. */ +bool isEmpty(const moveit_msgs::msg::PlanningScene& msg); + +/** \brief Check if a message includes any information about a planning scene world, or whether it is empty. */ +bool isEmpty(const moveit_msgs::msg::PlanningSceneWorld& msg); + +/** \brief Check if a message includes any information about a robot state, or whether it is empty. */ +bool isEmpty(const moveit_msgs::msg::RobotState& msg); + +/** \brief Check if a message specifies constraints */ +bool isEmpty(const moveit_msgs::msg::Constraints& msg); + +/** \brief Check if the message contains any non-zero entries */ +bool isEmpty(const geometry_msgs::msg::Quaternion& msg); + +/** \brief Check if the message contains any non-zero entries */ +bool isEmpty(const geometry_msgs::msg::Pose& msg); +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h index 9e4ff26f2b..c7646d1832 100644 --- a/moveit_core/utils/include/moveit/utils/moveit_error_code.h +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,121 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace moveit -{ -namespace core -{ -/** - * @brief a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function - */ -class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes -{ -public: - MoveItErrorCode(const int code = moveit_msgs::msg::MoveItErrorCodes::UNDEFINED, const std::string& error_message = "", - const std::string& error_source = "") - { - val = code; - message = error_message; - source = error_source; - } - MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) - { - val = code.val; - message = code.message; - source = code.source; - } - explicit operator bool() const - { - return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int c) const - { - return val == c; - } - bool operator!=(const int c) const - { - return val != c; - } -}; - -/** - * @brief Convenience function to translated error message into string - @param error_code Error code to be translated to a string - @return Error code string - */ -inline std::string errorCodeToString(const MoveItErrorCode& error_code) -{ - switch (error_code.val) - { - case moveit::core::MoveItErrorCode::SUCCESS: - return std::string("SUCCESS"); - case moveit::core::MoveItErrorCode::UNDEFINED: - return std::string("UNDEFINED"); - case moveit::core::MoveItErrorCode::FAILURE: - return std::string("FAILURE"); - case moveit::core::MoveItErrorCode::PLANNING_FAILED: - return std::string("PLANNING_FAILED"); - case moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN: - return std::string("INVALID_MOTION_PLAN"); - case moveit::core::MoveItErrorCode::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: - return std::string("MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE"); - case moveit::core::MoveItErrorCode::CONTROL_FAILED: - return std::string("CONTROL_FAILED"); - case moveit::core::MoveItErrorCode::UNABLE_TO_AQUIRE_SENSOR_DATA: - return std::string("UNABLE_TO_AQUIRE_SENSOR_DATA"); - case moveit::core::MoveItErrorCode::TIMED_OUT: - return std::string("TIMED_OUT"); - case moveit::core::MoveItErrorCode::PREEMPTED: - return std::string("PREEMPTED"); - case moveit::core::MoveItErrorCode::START_STATE_IN_COLLISION: - return std::string("START_STATE_IN_COLLISION"); - case moveit::core::MoveItErrorCode::START_STATE_VIOLATES_PATH_CONSTRAINTS: - return std::string("START_STATE_VIOLATES_PATH_CONSTRAINTS"); - case moveit::core::MoveItErrorCode::START_STATE_INVALID: - return std::string("START_STATE_INVALID"); - case moveit::core::MoveItErrorCode::GOAL_IN_COLLISION: - return std::string("GOAL_IN_COLLISION"); - case moveit::core::MoveItErrorCode::GOAL_VIOLATES_PATH_CONSTRAINTS: - return std::string("GOAL_VIOLATES_PATH_CONSTRAINTS"); - case moveit::core::MoveItErrorCode::GOAL_CONSTRAINTS_VIOLATED: - return std::string("GOAL_CONSTRAINTS_VIOLATED"); - case moveit::core::MoveItErrorCode::GOAL_STATE_INVALID: - return std::string("GOAL_STATE_INVALID"); - case moveit::core::MoveItErrorCode::UNRECOGNIZED_GOAL_TYPE: - return std::string("UNRECOGNIZED_GOAL_TYPE"); - case moveit::core::MoveItErrorCode::INVALID_GROUP_NAME: - return std::string("INVALID_GROUP_NAME"); - case moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS: - return std::string("INVALID_GOAL_CONSTRAINTS"); - case moveit::core::MoveItErrorCode::INVALID_ROBOT_STATE: - return std::string("INVALID_ROBOT_STATE"); - case moveit::core::MoveItErrorCode::INVALID_LINK_NAME: - return std::string("INVALID_LINK_NAME"); - case moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME: - return std::string("INVALID_OBJECT_NAME"); - case moveit::core::MoveItErrorCode::FRAME_TRANSFORM_FAILURE: - return std::string("FRAME_TRANSFORM_FAILURE"); - case moveit::core::MoveItErrorCode::COLLISION_CHECKING_UNAVAILABLE: - return std::string("COLLISION_CHECKING_UNAVAILABLE"); - case moveit::core::MoveItErrorCode::ROBOT_STATE_STALE: - return std::string("ROBOT_STATE_STALE"); - case moveit::core::MoveItErrorCode::SENSOR_INFO_STALE: - return std::string("SENSOR_INFO_STALE"); - case moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE: - return std::string("COMMUNICATION_FAILURE"); - case moveit::core::MoveItErrorCode::CRASH: - return std::string("CRASH"); - case moveit::core::MoveItErrorCode::ABORT: - return std::string("ABORT"); - case moveit::core::MoveItErrorCode::NO_IK_SOLUTION: - return std::string("NO_IK_SOLUTION"); - } - return std::string("Unrecognized MoveItErrorCode. This should never happen!"); -} - -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.hpp b/moveit_core/utils/include/moveit/utils/moveit_error_code.hpp new file mode 100644 index 0000000000..9e4ff26f2b --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.hpp @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace core +{ +/** + * @brief a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function + */ +class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes +{ +public: + MoveItErrorCode(const int code = moveit_msgs::msg::MoveItErrorCodes::UNDEFINED, const std::string& error_message = "", + const std::string& error_source = "") + { + val = code; + message = error_message; + source = error_source; + } + MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) + { + val = code.val; + message = code.message; + source = code.source; + } + explicit operator bool() const + { + return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } +}; + +/** + * @brief Convenience function to translated error message into string + @param error_code Error code to be translated to a string + @return Error code string + */ +inline std::string errorCodeToString(const MoveItErrorCode& error_code) +{ + switch (error_code.val) + { + case moveit::core::MoveItErrorCode::SUCCESS: + return std::string("SUCCESS"); + case moveit::core::MoveItErrorCode::UNDEFINED: + return std::string("UNDEFINED"); + case moveit::core::MoveItErrorCode::FAILURE: + return std::string("FAILURE"); + case moveit::core::MoveItErrorCode::PLANNING_FAILED: + return std::string("PLANNING_FAILED"); + case moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN: + return std::string("INVALID_MOTION_PLAN"); + case moveit::core::MoveItErrorCode::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: + return std::string("MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE"); + case moveit::core::MoveItErrorCode::CONTROL_FAILED: + return std::string("CONTROL_FAILED"); + case moveit::core::MoveItErrorCode::UNABLE_TO_AQUIRE_SENSOR_DATA: + return std::string("UNABLE_TO_AQUIRE_SENSOR_DATA"); + case moveit::core::MoveItErrorCode::TIMED_OUT: + return std::string("TIMED_OUT"); + case moveit::core::MoveItErrorCode::PREEMPTED: + return std::string("PREEMPTED"); + case moveit::core::MoveItErrorCode::START_STATE_IN_COLLISION: + return std::string("START_STATE_IN_COLLISION"); + case moveit::core::MoveItErrorCode::START_STATE_VIOLATES_PATH_CONSTRAINTS: + return std::string("START_STATE_VIOLATES_PATH_CONSTRAINTS"); + case moveit::core::MoveItErrorCode::START_STATE_INVALID: + return std::string("START_STATE_INVALID"); + case moveit::core::MoveItErrorCode::GOAL_IN_COLLISION: + return std::string("GOAL_IN_COLLISION"); + case moveit::core::MoveItErrorCode::GOAL_VIOLATES_PATH_CONSTRAINTS: + return std::string("GOAL_VIOLATES_PATH_CONSTRAINTS"); + case moveit::core::MoveItErrorCode::GOAL_CONSTRAINTS_VIOLATED: + return std::string("GOAL_CONSTRAINTS_VIOLATED"); + case moveit::core::MoveItErrorCode::GOAL_STATE_INVALID: + return std::string("GOAL_STATE_INVALID"); + case moveit::core::MoveItErrorCode::UNRECOGNIZED_GOAL_TYPE: + return std::string("UNRECOGNIZED_GOAL_TYPE"); + case moveit::core::MoveItErrorCode::INVALID_GROUP_NAME: + return std::string("INVALID_GROUP_NAME"); + case moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS: + return std::string("INVALID_GOAL_CONSTRAINTS"); + case moveit::core::MoveItErrorCode::INVALID_ROBOT_STATE: + return std::string("INVALID_ROBOT_STATE"); + case moveit::core::MoveItErrorCode::INVALID_LINK_NAME: + return std::string("INVALID_LINK_NAME"); + case moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME: + return std::string("INVALID_OBJECT_NAME"); + case moveit::core::MoveItErrorCode::FRAME_TRANSFORM_FAILURE: + return std::string("FRAME_TRANSFORM_FAILURE"); + case moveit::core::MoveItErrorCode::COLLISION_CHECKING_UNAVAILABLE: + return std::string("COLLISION_CHECKING_UNAVAILABLE"); + case moveit::core::MoveItErrorCode::ROBOT_STATE_STALE: + return std::string("ROBOT_STATE_STALE"); + case moveit::core::MoveItErrorCode::SENSOR_INFO_STALE: + return std::string("SENSOR_INFO_STALE"); + case moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE: + return std::string("COMMUNICATION_FAILURE"); + case moveit::core::MoveItErrorCode::CRASH: + return std::string("CRASH"); + case moveit::core::MoveItErrorCode::ABORT: + return std::string("ABORT"); + case moveit::core::MoveItErrorCode::NO_IK_SOLUTION: + return std::string("NO_IK_SOLUTION"); + } + return std::string("Unrecognized MoveItErrorCode. This should never happen!"); +} + +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/include/moveit/utils/rclcpp_utils.h b/moveit_core/utils/include/moveit/utils/rclcpp_utils.h index 7fc5fa9d72..0e8174d79a 100644 --- a/moveit_core/utils/include/moveit/utils/rclcpp_utils.h +++ b/moveit_core/utils/include/moveit/utils/rclcpp_utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /* * Copyright (C) 2009, Willow Garage, Inc. * @@ -26,16 +38,5 @@ */ #pragma once - -#include - -// TODO(JafarAbdi): This's taken from ros_comm/../names.cpp for MoveIt 2 beta release -- remove when it's ported -namespace rclcpp -{ -namespace names -{ -std::string clean(const std::string& name); - -std::string append(const std::string& left, const std::string& right); -} // namespace names -} // namespace rclcpp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/rclcpp_utils.hpp b/moveit_core/utils/include/moveit/utils/rclcpp_utils.hpp new file mode 100644 index 0000000000..7fc5fa9d72 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/rclcpp_utils.hpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * 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 names of Willow Garage, Inc. 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. + */ + +#pragma once + +#include + +// TODO(JafarAbdi): This's taken from ros_comm/../names.cpp for MoveIt 2 beta release -- remove when it's ported +namespace rclcpp +{ +namespace names +{ +std::string clean(const std::string& name); + +std::string append(const std::string& left, const std::string& right); +} // namespace names +} // namespace rclcpp diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index a419d4cc4e..337086d241 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,197 +48,5 @@ /** \brief convenience functions and classes used for making simple robot models for testing. */ #pragma once - -#include -#if __has_include() -#include -#else -#include -#endif -#include -#include - -#include - -namespace moveit -{ -namespace core -{ - -/** \brief Loads a robot model given a URDF and SRDF file in a package. - * \param[in] package_name The name of the package containing the URDF and SRDF files. - * \param[in] urdf_relative_path The relative path to the URDF file in the package installed directory. - * \param[in] srdf_relative_path The relative path to the SRDF file in the package installed directory. - * \returns a RobotModel constructed from the URDF and SRDF files, or nullptr if the files could not be loaded. - */ -moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& package_name, - const std::string& urdf_relative_path, - const std::string& srdf_relative_path); - -/** \brief Loads a robot from moveit_resources. - * \param[in] robot_name The name of the robot in moveit_resources to load. - * This should be the prefix to many of the robot packages. - * For example, "pr2", "panda", or "fanuc". - * \returns a RobotModel constructed from robot_name's URDF and SRDF. - */ -moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name); - -/** \brief Loads a URDF Model Interface from moveit_resources. - * \param[in] robot_name The name of the robot in moveit_resources to load. - * This should be the prefix to many of the robot packages. - * For example, "pr2", "panda", or "fanuc". - * \returns a ModelInterface constructed from robot_name's URDF. - */ -urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name); - -/** \brief Loads an SRDF Model from moveit_resources. - * \param[in] robot_name The name of the robot in moveit_resources to load. - * This should be the prefix to many of the robot packages. - * For example, "pr2", "panda", or "fanuc". - * \returns an SRDF Model constructed from robot_name's URDF and SRDF. - */ -srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name); - -/** \brief Load an IK solver plugin for the given joint model group - * \param[in] jmg joint model group to load the plugin for - * \param[in] base_link base link of chain - * \param[in] tip_link tip link of chain - * \param[in] plugin name of the plugin ("KDL", or full name) - * \param[in] timeout default solver timeout - */ -void loadIKPluginForGroup(const rclcpp::Node::SharedPtr& node, JointModelGroup* jmg, const std::string& base_link, - const std::string& tip_link, std::string plugin = "KDL", double timeout = 0.1); - -/** \brief Easily build different robot models for testing. - * Essentially a programmer-friendly light wrapper around URDF and SRDF. - * Best shown by an example: - * \code{.cpp} - * RobotModelBuilder builder("my_robot", "base_link"); - * builder.addChain("a->b->c", "continuous"); - * builder.addGroup({"a", "b"}, {}, "example_group"); - * ASSERT_TRUE(builder.isValid()); - * RobotModelPtr model = builder.build(); - * \endcode - */ -class RobotModelBuilder -{ -public: - /** \brief Constructor, takes the names of the robot and the base link. - * \param[in] name The name of the robot, i.e. the 'name' attribute of the robot tag in URDF - * \param[in] base_link_name The name of the root link of the robot. All other links should be descendants of this - */ - RobotModelBuilder(const std::string& name, const std::string& base_link_name); - - /** \name URDF Functions - \{ */ - - /** \brief Adds a chain of links and joints to the builder. - * The joint names are generated automatically as "--joint". - * \param[in] section A list of link names separated by "->". The first link should already be added to the build by - * the time this function is called - * \param[in] type The type of the joints connecting all of the given links, e.g. "revolute" or "continuous". All of - * the joints will be given this type. To add multiple types of joints, call this method multiple times - * \param[in] joint_origins The "parent to joint" origins for the joints connecting the links. If not used, all - * origins will default to the identity transform - * \param[in] joint_axis The joint axis specified in the joint frame defaults to (1,0,0) - */ - void addChain(const std::string& section, const std::string& type, - const std::vector& joint_origins = {}, - urdf::Vector3 joint_axis = urdf::Vector3(1.0, 0.0, 0.0)); - - /** \brief Adds a collision mesh to a specific link. - * \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder - * \param[in] filename The path to the mesh file, e.g. - * "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl" - * \param[in] origin The origin pose of this collision mesh relative to the link origin - */ - void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::msg::Pose origin); - /** \brief Adds a collision box to a specific link. - * \param[in] link_name The name of the link to which the box will be added. Must already be in the builder. - * \param[in] size The dimensions of the box - * \param[in] origin The origin pose of this collision box relative to the link origin - */ - void addCollisionBox(const std::string& link_name, const std::vector& dims, geometry_msgs::msg::Pose origin); - - /** \brief Adds a visual box to a specific link. - * \param[in] link_name The name of the link to which the box will be added. Must already be in the builder. - * \param[in] size The dimensions of the box - * \param[in] origin The origin pose of this visual box relative to the link origin - */ - void addVisualBox(const std::string& link_name, const std::vector& size, geometry_msgs::msg::Pose origin); - - /** - * Adds an inertial component to a link. - * \param[in] link_name The name of the link for this inertial information - * \param[in] mass The mass of the link - * \param[in] origin The origin center pose of the center of mass of this link - */ - void addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy, - double ixz, double iyy, double iyz, double izz); - - /** \} */ - - /** \name SRDF functions - \{ */ - - /** \brief Adds a virtual joint to the SRDF. - * \param[in] parent_frame The parent, e.g. "odom" - * \param[in] child_link The child link of this virtual joint, usually the base link - * \param[in] type The type of joint, can be "fixed", "floating", or "planar" - * \param[in] name The name of the virtual joint, if not given it's automatically made to be - * "--virtual-joint" - */ - void addVirtualJoint(const std::string& parent_frame, const std::string& child_link, const std::string& type, - const std::string& name = ""); - - /** \brief Adds a new group using a chain of links. The group is the parent joint of each link in the chain. - * \param[in] base_link The starting link of the chain - * \param[in] tip_link The ending link of the chain. - * \param[in] name The name of the group, if not given it's set as "--chain-group" - */ - void addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name = ""); - - /** \brief Adds a new group using a list of links and a list of joints. - * \param[in] links The links (really their parent joints) to include in the group - * \param[in] joints The joints to include in the group - * \param[in] name The name of the group, required - */ - void addGroup(const std::vector& links, const std::vector& joints, const std::string& name); - - void addEndEffector(const std::string& name, const std::string& parent_link, const std::string& parent_group = "", - const std::string& component_group = ""); - - /** \brief Adds a new joint property - * \param[in] joint_name The name of the joint the property is specified for - * \param[in] property_name The joint property name - * \param[in] value The value of the joint property - */ - void addJointProperty(const std::string& joint_name, const std::string& property_name, const std::string& value); - - /** \} */ - - /** \brief Returns true if the building process so far has been valid. */ - bool isValid(); - - /** \brief Builds and returns the robot model added to the builder. - */ - moveit::core::RobotModelPtr build(); - -private: - /** \brief Adds different collision geometries to a link. */ - void addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& coll, - geometry_msgs::msg::Pose origin); - - /** \brief Adds different visual geometries to a link. */ - void addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis, geometry_msgs::msg::Pose origin); - - /// The URDF model, holds all of the URDF components of the robot added so far. - urdf::ModelInterfaceSharedPtr urdf_model_; - /// The SRDF model, holds all of the SRDF components of the robot added so far. - srdf::SRDFWriterPtr srdf_writer_; - - /// Whether the current builder state is valid. If any 'add' method fails, this becomes false. - bool is_valid_ = true; -}; -} // namespace core -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.hpp b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.hpp new file mode 100644 index 0000000000..f8355d92f8 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.hpp @@ -0,0 +1,232 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Bryce Willey. + * 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 MoveIt 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: Bryce Willey */ +/** \brief convenience functions and classes used for making simple robot models for testing. */ + +#pragma once + +#include +#if __has_include() +#include +#else +#include +#endif +#include +#include + +#include + +namespace moveit +{ +namespace core +{ + +/** \brief Loads a robot model given a URDF and SRDF file in a package. + * \param[in] package_name The name of the package containing the URDF and SRDF files. + * \param[in] urdf_relative_path The relative path to the URDF file in the package installed directory. + * \param[in] srdf_relative_path The relative path to the SRDF file in the package installed directory. + * \returns a RobotModel constructed from the URDF and SRDF files, or nullptr if the files could not be loaded. + */ +moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& package_name, + const std::string& urdf_relative_path, + const std::string& srdf_relative_path); + +/** \brief Loads a robot from moveit_resources. + * \param[in] robot_name The name of the robot in moveit_resources to load. + * This should be the prefix to many of the robot packages. + * For example, "pr2", "panda", or "fanuc". + * \returns a RobotModel constructed from robot_name's URDF and SRDF. + */ +moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name); + +/** \brief Loads a URDF Model Interface from moveit_resources. + * \param[in] robot_name The name of the robot in moveit_resources to load. + * This should be the prefix to many of the robot packages. + * For example, "pr2", "panda", or "fanuc". + * \returns a ModelInterface constructed from robot_name's URDF. + */ +urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name); + +/** \brief Loads an SRDF Model from moveit_resources. + * \param[in] robot_name The name of the robot in moveit_resources to load. + * This should be the prefix to many of the robot packages. + * For example, "pr2", "panda", or "fanuc". + * \returns an SRDF Model constructed from robot_name's URDF and SRDF. + */ +srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name); + +/** \brief Load an IK solver plugin for the given joint model group + * \param[in] jmg joint model group to load the plugin for + * \param[in] base_link base link of chain + * \param[in] tip_link tip link of chain + * \param[in] plugin name of the plugin ("KDL", or full name) + * \param[in] timeout default solver timeout + */ +void loadIKPluginForGroup(const rclcpp::Node::SharedPtr& node, JointModelGroup* jmg, const std::string& base_link, + const std::string& tip_link, std::string plugin = "KDL", double timeout = 0.1); + +/** \brief Easily build different robot models for testing. + * Essentially a programmer-friendly light wrapper around URDF and SRDF. + * Best shown by an example: + * \code{.cpp} + * RobotModelBuilder builder("my_robot", "base_link"); + * builder.addChain("a->b->c", "continuous"); + * builder.addGroup({"a", "b"}, {}, "example_group"); + * ASSERT_TRUE(builder.isValid()); + * RobotModelPtr model = builder.build(); + * \endcode + */ +class RobotModelBuilder +{ +public: + /** \brief Constructor, takes the names of the robot and the base link. + * \param[in] name The name of the robot, i.e. the 'name' attribute of the robot tag in URDF + * \param[in] base_link_name The name of the root link of the robot. All other links should be descendants of this + */ + RobotModelBuilder(const std::string& name, const std::string& base_link_name); + + /** \name URDF Functions + \{ */ + + /** \brief Adds a chain of links and joints to the builder. + * The joint names are generated automatically as "--joint". + * \param[in] section A list of link names separated by "->". The first link should already be added to the build by + * the time this function is called + * \param[in] type The type of the joints connecting all of the given links, e.g. "revolute" or "continuous". All of + * the joints will be given this type. To add multiple types of joints, call this method multiple times + * \param[in] joint_origins The "parent to joint" origins for the joints connecting the links. If not used, all + * origins will default to the identity transform + * \param[in] joint_axis The joint axis specified in the joint frame defaults to (1,0,0) + */ + void addChain(const std::string& section, const std::string& type, + const std::vector& joint_origins = {}, + urdf::Vector3 joint_axis = urdf::Vector3(1.0, 0.0, 0.0)); + + /** \brief Adds a collision mesh to a specific link. + * \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder + * \param[in] filename The path to the mesh file, e.g. + * "package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl" + * \param[in] origin The origin pose of this collision mesh relative to the link origin + */ + void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::msg::Pose origin); + /** \brief Adds a collision box to a specific link. + * \param[in] link_name The name of the link to which the box will be added. Must already be in the builder. + * \param[in] size The dimensions of the box + * \param[in] origin The origin pose of this collision box relative to the link origin + */ + void addCollisionBox(const std::string& link_name, const std::vector& dims, geometry_msgs::msg::Pose origin); + + /** \brief Adds a visual box to a specific link. + * \param[in] link_name The name of the link to which the box will be added. Must already be in the builder. + * \param[in] size The dimensions of the box + * \param[in] origin The origin pose of this visual box relative to the link origin + */ + void addVisualBox(const std::string& link_name, const std::vector& size, geometry_msgs::msg::Pose origin); + + /** + * Adds an inertial component to a link. + * \param[in] link_name The name of the link for this inertial information + * \param[in] mass The mass of the link + * \param[in] origin The origin center pose of the center of mass of this link + */ + void addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy, + double ixz, double iyy, double iyz, double izz); + + /** \} */ + + /** \name SRDF functions + \{ */ + + /** \brief Adds a virtual joint to the SRDF. + * \param[in] parent_frame The parent, e.g. "odom" + * \param[in] child_link The child link of this virtual joint, usually the base link + * \param[in] type The type of joint, can be "fixed", "floating", or "planar" + * \param[in] name The name of the virtual joint, if not given it's automatically made to be + * "--virtual-joint" + */ + void addVirtualJoint(const std::string& parent_frame, const std::string& child_link, const std::string& type, + const std::string& name = ""); + + /** \brief Adds a new group using a chain of links. The group is the parent joint of each link in the chain. + * \param[in] base_link The starting link of the chain + * \param[in] tip_link The ending link of the chain. + * \param[in] name The name of the group, if not given it's set as "--chain-group" + */ + void addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name = ""); + + /** \brief Adds a new group using a list of links and a list of joints. + * \param[in] links The links (really their parent joints) to include in the group + * \param[in] joints The joints to include in the group + * \param[in] name The name of the group, required + */ + void addGroup(const std::vector& links, const std::vector& joints, const std::string& name); + + void addEndEffector(const std::string& name, const std::string& parent_link, const std::string& parent_group = "", + const std::string& component_group = ""); + + /** \brief Adds a new joint property + * \param[in] joint_name The name of the joint the property is specified for + * \param[in] property_name The joint property name + * \param[in] value The value of the joint property + */ + void addJointProperty(const std::string& joint_name, const std::string& property_name, const std::string& value); + + /** \} */ + + /** \brief Returns true if the building process so far has been valid. */ + bool isValid(); + + /** \brief Builds and returns the robot model added to the builder. + */ + moveit::core::RobotModelPtr build(); + +private: + /** \brief Adds different collision geometries to a link. */ + void addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& coll, + geometry_msgs::msg::Pose origin); + + /** \brief Adds different visual geometries to a link. */ + void addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis, geometry_msgs::msg::Pose origin); + + /// The URDF model, holds all of the URDF components of the robot added so far. + urdf::ModelInterfaceSharedPtr urdf_model_; + /// The SRDF model, holds all of the SRDF components of the robot added so far. + srdf::SRDFWriterPtr srdf_writer_; + + /// Whether the current builder state is valid. If any 'add' method fails, this becomes false. + bool is_valid_ = true; +}; +} // namespace core +} // namespace moveit diff --git a/moveit_core/utils/src/lexical_casts.cpp b/moveit_core/utils/src/lexical_casts.cpp index 2682344bcc..6139a86120 100644 --- a/moveit_core/utils/src/lexical_casts.cpp +++ b/moveit_core/utils/src/lexical_casts.cpp @@ -34,7 +34,7 @@ /* Author: Simon Schmeisser */ -#include +#include #include #include diff --git a/moveit_core/utils/src/message_checks.cpp b/moveit_core/utils/src/message_checks.cpp index 686ae5a7f1..8d4758ce99 100644 --- a/moveit_core/utils/src/message_checks.cpp +++ b/moveit_core/utils/src/message_checks.cpp @@ -34,7 +34,7 @@ /* Author: Michael Goerner */ -#include +#include namespace moveit { diff --git a/moveit_core/utils/src/rclcpp_utils.cpp b/moveit_core/utils/src/rclcpp_utils.cpp index 5979b9e353..df84934bce 100644 --- a/moveit_core/utils/src/rclcpp_utils.cpp +++ b/moveit_core/utils/src/rclcpp_utils.cpp @@ -25,7 +25,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include namespace rclcpp { diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index cc39f6caf8..eea311fbec 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include -#include +#include namespace moveit { diff --git a/moveit_core/version/CMakeLists.txt b/moveit_core/version/CMakeLists.txt index c67eec7170..96a8474221 100644 --- a/moveit_core/version/CMakeLists.txt +++ b/moveit_core/version/CMakeLists.txt @@ -13,7 +13,7 @@ message( # https://stackoverflow.com/questions/13920072/how-to-always-run-command-when-building-regardless-of-any-dependency add_custom_command( - OUTPUT ${CMAKE_BINARY_DIR}/include/moveit/version.h always_rebuild + OUTPUT ${CMAKE_BINARY_DIR}/include/moveit/version.hpp always_rebuild COMMENT "Generate version header" COMMAND ${CMAKE_COMMAND} -DVERSION_FILE_PATH="${CMAKE_BINARY_DIR}/include" @@ -23,16 +23,16 @@ add_custom_command( WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) add_custom_target( - version_h + version_hpp DEPENDS always_rebuild - COMMENT "Generating version.h") + COMMENT "Generating version.hpp") add_executable(moveit_version version.cpp) -add_dependencies(moveit_version version_h) +add_dependencies(moveit_version version_hpp) target_include_directories(moveit_version PRIVATE ${CMAKE_BINARY_DIR}/include/moveit_core) -install(FILES "${CMAKE_BINARY_DIR}/include/moveit_core/moveit/version.h" +install(FILES "${CMAKE_BINARY_DIR}/include/moveit_core/moveit/version.hpp" DESTINATION include/moveit_core/moveit) install(TARGETS moveit_version RUNTIME DESTINATION bin) diff --git a/moveit_core/version/version.cmake b/moveit_core/version/version.cmake index 60c4723703..3c1b7bc4bc 100644 --- a/moveit_core/version/version.cmake +++ b/moveit_core/version/version.cmake @@ -34,5 +34,5 @@ if(NOT "${MOVEIT_VERSION_EXTRA}" STREQUAL "") string(APPEND MOVEIT_VERSION "-${MOVEIT_VERSION_EXTRA}") endif() -configure_file("version.h.in" - "${VERSION_FILE_PATH}/moveit_core/moveit/version.h") +configure_file("version.hpp.in" + "${VERSION_FILE_PATH}/moveit_core/moveit/version.hpp") diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp index 498c61cd4c..bd526c63f6 100644 --- a/moveit_core/version/version.cpp +++ b/moveit_core/version/version.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_core/version/version.h.in b/moveit_core/version/version.hpp.in similarity index 100% rename from moveit_core/version/version.h.in rename to moveit_core/version/version.hpp.in diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/README.md b/moveit_kinematics/cached_ik_kinematics_plugin/README.md index 3d7f7c3b05..b7fe385543 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/README.md +++ b/moveit_kinematics/cached_ik_kinematics_plugin/README.md @@ -56,8 +56,8 @@ Below is a complete list of all arguments: The Cached IK Kinematics Plugin is implemented as a wrapper around classed derived from the `kinematics::KinematicsBase` [abstract base class](http://docs.ros.org/en/latest/api/moveit_core/html/cpp/classkinematics_1_1KinematicsBase.html). Wrappers for the `kdl_kinematics_plugin::KDLKinematicsPlugin` and `srv_kinematics_plugin::SrvKinematicsPlugin` classes are already included in the plugin. For any other solver, you can create a new kinematics plugin. The C++ code for doing so is extremely simple; here is the code to create a wrapper for the KDL solver: - #include "cached_ik_kinematics_plugin.h" - #include + #include "cached_ik_kinematics_plugin.hpp" + #include #include PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, kinematics::KinematicsBase); diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h index affcf771ce..a30943df99 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -34,187 +46,6 @@ /* Author: Mark Moll */ -#include -#include - -namespace cached_ik_kinematics_plugin -{ -template -CachedIKKinematicsPlugin::CachedIKKinematicsPlugin() -{ -} - -template -CachedIKKinematicsPlugin::~CachedIKKinematicsPlugin() -{ -} - -template -void CachedIKKinematicsPlugin::initCache(const std::string& robot_id, const std::string& group_name, - const std::string& cache_name) -{ - IKCache::Options opts; - opts.max_cache_size = params_.max_cache_size; - opts.min_pose_distance = params_.min_pose_distance; - opts.min_joint_config_distance = params_.min_joint_config_distance; - opts.cached_ik_path = params_.cached_ik_path; - - cache_.initializeCache(robot_id, group_name, cache_name, KinematicsPlugin::getJointNames().size(), opts); - - // for debugging purposes: - // kdl_kinematics_plugin::KDLKinematicsPlugin fk; - // fk.initialize(robot_description, group_name, base_frame, tip_frame, search_discretization); - // cache_.verifyCache(fk); -} - -template -bool CachedMultiTipIKKinematicsPlugin::initialize( - const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, const std::string& group_name, - const std::string& base_frame, const std::vector& tip_frames, double search_discretization) -{ - // call initialize method of wrapped class - if (!KinematicsPlugin::initialize(node, robot_model, group_name, base_frame, tip_frames, search_discretization)) - return false; - - std::string cache_name = base_frame; - std::accumulate(tip_frames.begin(), tip_frames.end(), cache_name); - CachedIKKinematicsPlugin::cache_.initializeCache(robot_model.getName(), group_name, cache_name, - KinematicsPlugin::getJointNames().size()); - return true; -} - -template -bool CachedIKKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, - std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options) const -{ - Pose pose(ik_pose); - const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); - bool solution_found = KinematicsPlugin::getPositionIK(ik_pose, nearest.second, solution, error_code, options) || - KinematicsPlugin::getPositionIK(ik_pose, ik_seed_state, solution, error_code, options); - if (solution_found) - cache_.updateCache(nearest, pose, solution); - return solution_found; -} - -template -bool CachedIKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, - double timeout, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options) const -{ - std::chrono::time_point start(std::chrono::system_clock::now()); - Pose pose(ik_pose); - const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); - bool solution_found = - KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, error_code, options); - if (!solution_found) - { - std::chrono::duration diff = std::chrono::system_clock::now() - start; - solution_found = - KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, error_code, options); - } - if (solution_found) - cache_.updateCache(nearest, pose, solution); - return solution_found; -} - -template -bool CachedIKKinematicsPlugin::searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options) const -{ - std::chrono::time_point start(std::chrono::system_clock::now()); - Pose pose(ik_pose); - const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); - bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits, - solution, error_code, options); - if (!solution_found) - { - std::chrono::duration diff = std::chrono::system_clock::now() - start; - solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits, - solution, error_code, options); - } - if (solution_found) - cache_.updateCache(nearest, pose, solution); - return solution_found; -} - -template -bool CachedIKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, - double timeout, std::vector& solution, - const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options) const -{ - std::chrono::time_point start(std::chrono::system_clock::now()); - Pose pose(ik_pose); - const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); - bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, - solution_callback, error_code, options); - if (!solution_found) - { - std::chrono::duration diff = std::chrono::system_clock::now() - start; - solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, - solution_callback, error_code, options); - } - if (solution_found) - cache_.updateCache(nearest, pose, solution); - return solution_found; -} - -template -bool CachedIKKinematicsPlugin::searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options) const -{ - std::chrono::time_point start(std::chrono::system_clock::now()); - Pose pose(ik_pose); - const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); - bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits, - solution, solution_callback, error_code, options); - if (!solution_found) - { - std::chrono::duration diff = std::chrono::system_clock::now() - start; - solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits, - solution, solution_callback, error_code, options); - } - if (solution_found) - cache_.updateCache(nearest, pose, solution); - return solution_found; -} - -template -bool CachedMultiTipIKKinematicsPlugin::searchPositionIK( - const std::vector& ik_poses, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options, - const moveit::core::RobotState* context_state) const -{ - std::chrono::time_point start(std::chrono::system_clock::now()); - std::vector poses(ik_poses.size()); - for (unsigned int i = 0; i < poses.size(); ++i) - poses[i] = Pose(ik_poses[i]); - const IKEntry& nearest = CachedIKKinematicsPlugin::cache_.getBestApproximateIKSolution(poses); - bool solution_found = - KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution, - solution_callback, error_code, options, context_state); - if (!solution_found) - { - std::chrono::duration diff = std::chrono::system_clock::now() - start; - solution_found = - KinematicsPlugin::searchPositionIK(ik_poses, ik_seed_state, diff.count(), consistency_limits, solution, - solution_callback, error_code, options, context_state); - } - - if (solution_found) - CachedIKKinematicsPlugin::cache_.updateCache(nearest, poses, solution); - return solution_found; -} -} // namespace cached_ik_kinematics_plugin +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.hpp b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.hpp new file mode 100644 index 0000000000..5d79f23f71 --- /dev/null +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin-inl.hpp @@ -0,0 +1,222 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Rice University + * 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 Rice University 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: Mark Moll */ + +#pragma once + +#include +#include + +namespace cached_ik_kinematics_plugin +{ +template +CachedIKKinematicsPlugin::CachedIKKinematicsPlugin() +{ +} + +template +CachedIKKinematicsPlugin::~CachedIKKinematicsPlugin() +{ +} + +template +void CachedIKKinematicsPlugin::initCache(const std::string& robot_id, const std::string& group_name, + const std::string& cache_name) +{ + IKCache::Options opts; + opts.max_cache_size = params_.max_cache_size; + opts.min_pose_distance = params_.min_pose_distance; + opts.min_joint_config_distance = params_.min_joint_config_distance; + opts.cached_ik_path = params_.cached_ik_path; + + cache_.initializeCache(robot_id, group_name, cache_name, KinematicsPlugin::getJointNames().size(), opts); + + // for debugging purposes: + // kdl_kinematics_plugin::KDLKinematicsPlugin fk; + // fk.initialize(robot_description, group_name, base_frame, tip_frame, search_discretization); + // cache_.verifyCache(fk); +} + +template +bool CachedMultiTipIKKinematicsPlugin::initialize( + const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, const std::string& group_name, + const std::string& base_frame, const std::vector& tip_frames, double search_discretization) +{ + // call initialize method of wrapped class + if (!KinematicsPlugin::initialize(node, robot_model, group_name, base_frame, tip_frames, search_discretization)) + return false; + + std::string cache_name = base_frame; + std::accumulate(tip_frames.begin(), tip_frames.end(), cache_name); + CachedIKKinematicsPlugin::cache_.initializeCache(robot_model.getName(), group_name, cache_name, + KinematicsPlugin::getJointNames().size()); + return true; +} + +template +bool CachedIKKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, + std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options) const +{ + Pose pose(ik_pose); + const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); + bool solution_found = KinematicsPlugin::getPositionIK(ik_pose, nearest.second, solution, error_code, options) || + KinematicsPlugin::getPositionIK(ik_pose, ik_seed_state, solution, error_code, options); + if (solution_found) + cache_.updateCache(nearest, pose, solution); + return solution_found; +} + +template +bool CachedIKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, + double timeout, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options) const +{ + std::chrono::time_point start(std::chrono::system_clock::now()); + Pose pose(ik_pose); + const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); + bool solution_found = + KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, error_code, options); + if (!solution_found) + { + std::chrono::duration diff = std::chrono::system_clock::now() - start; + solution_found = + KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, error_code, options); + } + if (solution_found) + cache_.updateCache(nearest, pose, solution); + return solution_found; +} + +template +bool CachedIKKinematicsPlugin::searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options) const +{ + std::chrono::time_point start(std::chrono::system_clock::now()); + Pose pose(ik_pose); + const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); + bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits, + solution, error_code, options); + if (!solution_found) + { + std::chrono::duration diff = std::chrono::system_clock::now() - start; + solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits, + solution, error_code, options); + } + if (solution_found) + cache_.updateCache(nearest, pose, solution); + return solution_found; +} + +template +bool CachedIKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, + double timeout, std::vector& solution, + const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options) const +{ + std::chrono::time_point start(std::chrono::system_clock::now()); + Pose pose(ik_pose); + const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); + bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, solution, + solution_callback, error_code, options); + if (!solution_found) + { + std::chrono::duration diff = std::chrono::system_clock::now() - start; + solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), solution, + solution_callback, error_code, options); + } + if (solution_found) + cache_.updateCache(nearest, pose, solution); + return solution_found; +} + +template +bool CachedIKKinematicsPlugin::searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options) const +{ + std::chrono::time_point start(std::chrono::system_clock::now()); + Pose pose(ik_pose); + const IKEntry& nearest = cache_.getBestApproximateIKSolution(pose); + bool solution_found = KinematicsPlugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits, + solution, solution_callback, error_code, options); + if (!solution_found) + { + std::chrono::duration diff = std::chrono::system_clock::now() - start; + solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits, + solution, solution_callback, error_code, options); + } + if (solution_found) + cache_.updateCache(nearest, pose, solution); + return solution_found; +} + +template +bool CachedMultiTipIKKinematicsPlugin::searchPositionIK( + const std::vector& ik_poses, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options, + const moveit::core::RobotState* context_state) const +{ + std::chrono::time_point start(std::chrono::system_clock::now()); + std::vector poses(ik_poses.size()); + for (unsigned int i = 0; i < poses.size(); ++i) + poses[i] = Pose(ik_poses[i]); + const IKEntry& nearest = CachedIKKinematicsPlugin::cache_.getBestApproximateIKSolution(poses); + bool solution_found = + KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution, + solution_callback, error_code, options, context_state); + if (!solution_found) + { + std::chrono::duration diff = std::chrono::system_clock::now() - start; + solution_found = + KinematicsPlugin::searchPositionIK(ik_poses, ik_seed_state, diff.count(), consistency_limits, solution, + solution_callback, error_code, options, context_state); + } + + if (solution_found) + CachedIKKinematicsPlugin::cache_.updateCache(nearest, poses, solution); + return solution_found; +} +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 0ba16b4142..994f97224a 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,323 +47,5 @@ /* Author: Mark Moll */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cached_ik_kinematics_plugin -{ - -/** \brief A cache of inverse kinematic solutions */ -class IKCache -{ -public: - struct Options - { - Options() : max_cache_size(5000), min_pose_distance(1.0), min_joint_config_distance(1.0), cached_ik_path("") - { - } - unsigned int max_cache_size; - double min_pose_distance; - double min_joint_config_distance; - std::string cached_ik_path; - }; - - /** - \brief class to represent end effector pose - - tf2::Transform stores orientation as a matrix, so we define our - own pose class that maps more directly to geometry_msgs::msg::Pose and - for which we can more easily define a distance metric. - */ - struct Pose - { - Pose() = default; - Pose(const geometry_msgs::msg::Pose& pose); - tf2::Vector3 position; - tf2::Quaternion orientation; - /** compute the distance between this pose and another pose */ - double distance(const Pose& pose) const; - }; - - /** - the IK cache entries are simply a pair formed by a vector of poses - (one for each end effector) and a configuration that achieves those - poses - */ - using IKEntry = std::pair, std::vector>; - - IKCache(); - ~IKCache(); - IKCache(const IKCache&) = delete; - - /** get the entry from the IK cache that best matches a given pose */ - const IKEntry& getBestApproximateIKSolution(const Pose& pose) const; - /** get the entry from the IK cache that best matches a given vector of poses */ - const IKEntry& getBestApproximateIKSolution(const std::vector& poses) const; - /** initialize cache, read from disk if found */ - void initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name, - const unsigned int num_joints, const Options& opts = Options()); - /** - insert (pose,config) as an entry if it's different enough from the - most similar cache entry - */ - void updateCache(const IKEntry& nearest, const Pose& pose, const std::vector& config) const; - /** - insert (pose,config) as an entry if it's different enough from the - most similar cache entry - */ - void updateCache(const IKEntry& nearest, const std::vector& poses, const std::vector& config) const; - /** verify with forward kinematics that the cache entries are correct */ - void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin& fk) const; - -protected: - /** compute the distance between two joint configurations */ - double configDistance2(const std::vector& config1, const std::vector& config2) const; - /** save current state of cache to disk */ - void saveCache() const; - - /** number of joints in the system */ - unsigned int num_joints_; - - /** for all cache entries, the poses are at least minPoseDistance_ apart ... */ - double min_pose_distance_; - /** ... or the configurations are at least minConfigDistance2_^.5 apart. */ - double min_config_distance2_; - /** maximum size of the cache */ - unsigned int max_cache_size_; - /** file name for loading / saving cache */ - std::filesystem::path cache_file_name_; - - /** - the IK methods are declared const in the base class, but the - wrapped methods need to modify the cache, so the next four members - are mutable - cache of IK solutions - */ - mutable std::vector ik_cache_; - /** nearest neighbor data structure over IK cache entries */ - mutable NearestNeighborsGNAT ik_nn_; - /** size of the cache when it was last saved */ - mutable unsigned int last_saved_cache_size_{ 0 }; - /** mutex for changing IK cache */ - mutable std::mutex lock_; -}; - -/** a container of IK caches for cases where there is no fixed base frame */ -class IKCacheMap : public std::unordered_map -{ -public: - using IKEntry = IKCache::IKEntry; - using Pose = IKCache::Pose; - - IKCacheMap(const std::string& robot_description, const std::string& group_name, unsigned int num_joints); - ~IKCacheMap(); - /** - get the entry from the IK cache that best matches a given vector of - poses, with a specified set of fixed and active tip links - */ - const IKEntry& getBestApproximateIKSolution(const std::vector& fixed, - const std::vector& active, - const std::vector& poses) const; - /** - insert (pose,config) as an entry if it's different enough from the - most similar cache entry - */ - void updateCache(const IKEntry& nearest, const std::vector& fixed, - const std::vector& active, const std::vector& poses, - const std::vector& config); - -protected: - std::string getKey(const std::vector& fixed, const std::vector& active) const; - std::string robot_description_; - std::string group_name_; - unsigned int num_joints_; -}; - -// Helper class to enable/disable initialize() methods with new/old API -// HasRobotModelApi::value provides a true/false constexpr depending on KinematicsPlugin offers the new Api -// This uses SFINAE magic: https://jguegant.github.io/blogs/tech/sfinae-introduction.html -template -struct HasRobotModelApi : std::false_type -{ -}; - -template -struct HasRobotModelApi().initialize( - std::declval(), - std::declval(), std::string(), - std::string(), std::vector(), 0.0))> : std::true_type -{ -}; - -template -struct HasRobotDescApi : std::false_type -{ -}; - -template -struct HasRobotDescApi().KinematicsPlugin::initialize( - std::string(), std::string(), std::string(), std::vector(), 0.0))> - : std::true_type -{ -}; - -/** Caching wrapper for kinematics::KinematicsBase-derived IK solvers. */ -template -class CachedIKKinematicsPlugin : public KinematicsPlugin -{ -public: - using Pose = IKCache::Pose; - using IKEntry = IKCache::IKEntry; - using IKCallbackFn = kinematics::KinematicsBase::IKCallbackFn; - using KinematicsQueryOptions = kinematics::KinematicsQueryOptions; - - CachedIKKinematicsPlugin(); - - ~CachedIKKinematicsPlugin() override; - - // virtual methods that need to be wrapped: - - bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization) override - { - node_ = node; - - std::string kinematics_param_prefix = "robot_description_kinematics." + group_name; - param_listener_ = std::make_shared(node, kinematics_param_prefix); - params_ = param_listener_->get_params(); - - return initializeImpl(node, robot_model, group_name, base_frame, tip_frames, search_discretization); - } - - bool getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; - - bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; - - bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; - - bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; - - bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; - -private: - rclcpp::Node::SharedPtr node_; - std::shared_ptr param_listener_; - cached_ik_kinematics::Params params_; - - IKCache cache_; - - void initCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name); - - /* Using templates and SFINAE magic, we can selectively enable/disable methods depending on - availability of API in wrapped KinematicsPlugin class. - However, as templates and virtual functions cannot be combined, we need helpers initializeImpl(). */ - template - typename std::enable_if::value, bool>::type - initializeImpl(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization) - { - if (tip_frames.size() != 1) - { - RCLCPP_ERROR(moveit::getLogger("moveit.core.cached_ik_kinematics_plugin"), - "This solver does not support multiple tip frames"); - return false; - } - - // call initialize method of wrapped class - if (!KinematicsPlugin::initialize(node, robot_model, group_name, base_frame, tip_frames, search_discretization)) - return false; - initCache(robot_model.getName(), group_name, base_frame + tip_frames[0]); - return true; - } - - template - typename std::enable_if::value, bool>::type - initializeImpl(const rclcpp::Node::SharedPtr& /*unused*/, const moveit::core::RobotModel& /*unused*/, - const std::string& /*unused*/, const std::string& /*unused*/, - const std::vector& /*unused*/, double /*unused*/) - { - return false; // API not supported - } - - template - typename std::enable_if::value, bool>::type - initializeImpl(const std::string& robot_description, const std::string& group_name, const std::string& base_frame, - const std::string& tip_frame, double search_discretization) - { - // call initialize method of wrapped class - if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frame, search_discretization)) - return false; - initCache(robot_description, group_name, base_frame + tip_frame); - return true; - } - - template - typename std::enable_if::value, bool>::type - initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, - const std::string& /*unused*/, double /*unused*/) - { - return false; // API not supported - } -}; - -/** - Caching wrapper for IK solvers that implement the multi-tip API. - - Most solvers don't implement this, so the CachedIKKinematicsPlugin - base class simply doesn't wrap the relevant methods and the - implementation in the abstract base class will be called. Ideally, - the two cache wrapper classes would be combined, but it's tricky to - call a method in kinematics::KinematicsBase or KinematicsPlugin - depending on whether KinematicsPlugin has overridden that method or - not (although it can be done with some template meta-programming). -*/ -template -class CachedMultiTipIKKinematicsPlugin : public CachedIKKinematicsPlugin -{ -public: - using Pose = IKCache::Pose; - using IKEntry = IKCache::IKEntry; - using IKCallbackFn = kinematics::KinematicsBase::IKCallbackFn; - using KinematicsQueryOptions = kinematics::KinematicsQueryOptions; - - bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization) override; - - bool searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const KinematicsQueryOptions& options = KinematicsQueryOptions(), - const moveit::core::RobotState* context_state = nullptr) const override; -}; -} // namespace cached_ik_kinematics_plugin - -#include "cached_ik_kinematics_plugin-inl.h" +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.hpp b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.hpp new file mode 100644 index 0000000000..0ca3ff009e --- /dev/null +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.hpp @@ -0,0 +1,357 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Rice University + * 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 Rice University 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: Mark Moll */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cached_ik_kinematics_plugin +{ + +/** \brief A cache of inverse kinematic solutions */ +class IKCache +{ +public: + struct Options + { + Options() : max_cache_size(5000), min_pose_distance(1.0), min_joint_config_distance(1.0), cached_ik_path("") + { + } + unsigned int max_cache_size; + double min_pose_distance; + double min_joint_config_distance; + std::string cached_ik_path; + }; + + /** + \brief class to represent end effector pose + + tf2::Transform stores orientation as a matrix, so we define our + own pose class that maps more directly to geometry_msgs::msg::Pose and + for which we can more easily define a distance metric. + */ + struct Pose + { + Pose() = default; + Pose(const geometry_msgs::msg::Pose& pose); + tf2::Vector3 position; + tf2::Quaternion orientation; + /** compute the distance between this pose and another pose */ + double distance(const Pose& pose) const; + }; + + /** + the IK cache entries are simply a pair formed by a vector of poses + (one for each end effector) and a configuration that achieves those + poses + */ + using IKEntry = std::pair, std::vector>; + + IKCache(); + ~IKCache(); + IKCache(const IKCache&) = delete; + + /** get the entry from the IK cache that best matches a given pose */ + const IKEntry& getBestApproximateIKSolution(const Pose& pose) const; + /** get the entry from the IK cache that best matches a given vector of poses */ + const IKEntry& getBestApproximateIKSolution(const std::vector& poses) const; + /** initialize cache, read from disk if found */ + void initializeCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name, + const unsigned int num_joints, const Options& opts = Options()); + /** + insert (pose,config) as an entry if it's different enough from the + most similar cache entry + */ + void updateCache(const IKEntry& nearest, const Pose& pose, const std::vector& config) const; + /** + insert (pose,config) as an entry if it's different enough from the + most similar cache entry + */ + void updateCache(const IKEntry& nearest, const std::vector& poses, const std::vector& config) const; + /** verify with forward kinematics that the cache entries are correct */ + void verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin& fk) const; + +protected: + /** compute the distance between two joint configurations */ + double configDistance2(const std::vector& config1, const std::vector& config2) const; + /** save current state of cache to disk */ + void saveCache() const; + + /** number of joints in the system */ + unsigned int num_joints_; + + /** for all cache entries, the poses are at least minPoseDistance_ apart ... */ + double min_pose_distance_; + /** ... or the configurations are at least minConfigDistance2_^.5 apart. */ + double min_config_distance2_; + /** maximum size of the cache */ + unsigned int max_cache_size_; + /** file name for loading / saving cache */ + std::filesystem::path cache_file_name_; + + /** + the IK methods are declared const in the base class, but the + wrapped methods need to modify the cache, so the next four members + are mutable + cache of IK solutions + */ + mutable std::vector ik_cache_; + /** nearest neighbor data structure over IK cache entries */ + mutable NearestNeighborsGNAT ik_nn_; + /** size of the cache when it was last saved */ + mutable unsigned int last_saved_cache_size_{ 0 }; + /** mutex for changing IK cache */ + mutable std::mutex lock_; +}; + +/** a container of IK caches for cases where there is no fixed base frame */ +class IKCacheMap : public std::unordered_map +{ +public: + using IKEntry = IKCache::IKEntry; + using Pose = IKCache::Pose; + + IKCacheMap(const std::string& robot_description, const std::string& group_name, unsigned int num_joints); + ~IKCacheMap(); + /** + get the entry from the IK cache that best matches a given vector of + poses, with a specified set of fixed and active tip links + */ + const IKEntry& getBestApproximateIKSolution(const std::vector& fixed, + const std::vector& active, + const std::vector& poses) const; + /** + insert (pose,config) as an entry if it's different enough from the + most similar cache entry + */ + void updateCache(const IKEntry& nearest, const std::vector& fixed, + const std::vector& active, const std::vector& poses, + const std::vector& config); + +protected: + std::string getKey(const std::vector& fixed, const std::vector& active) const; + std::string robot_description_; + std::string group_name_; + unsigned int num_joints_; +}; + +// Helper class to enable/disable initialize() methods with new/old API +// HasRobotModelApi::value provides a true/false constexpr depending on KinematicsPlugin offers the new Api +// This uses SFINAE magic: https://jguegant.github.io/blogs/tech/sfinae-introduction.html +template +struct HasRobotModelApi : std::false_type +{ +}; + +template +struct HasRobotModelApi().initialize( + std::declval(), + std::declval(), std::string(), + std::string(), std::vector(), 0.0))> : std::true_type +{ +}; + +template +struct HasRobotDescApi : std::false_type +{ +}; + +template +struct HasRobotDescApi().KinematicsPlugin::initialize( + std::string(), std::string(), std::string(), std::vector(), 0.0))> + : std::true_type +{ +}; + +/** Caching wrapper for kinematics::KinematicsBase-derived IK solvers. */ +template +class CachedIKKinematicsPlugin : public KinematicsPlugin +{ +public: + using Pose = IKCache::Pose; + using IKEntry = IKCache::IKEntry; + using IKCallbackFn = kinematics::KinematicsBase::IKCallbackFn; + using KinematicsQueryOptions = kinematics::KinematicsQueryOptions; + + CachedIKKinematicsPlugin(); + + ~CachedIKKinematicsPlugin() override; + + // virtual methods that need to be wrapped: + + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization) override + { + node_ = node; + + std::string kinematics_param_prefix = "robot_description_kinematics." + group_name; + param_listener_ = std::make_shared(node, kinematics_param_prefix); + params_ = param_listener_->get_params(); + + return initializeImpl(node, robot_model, group_name, base_frame, tip_frames, search_discretization); + } + + bool getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; + + bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; + + bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; + + bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; + + bool searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options = KinematicsQueryOptions()) const override; + +private: + rclcpp::Node::SharedPtr node_; + std::shared_ptr param_listener_; + cached_ik_kinematics::Params params_; + + IKCache cache_; + + void initCache(const std::string& robot_id, const std::string& group_name, const std::string& cache_name); + + /* Using templates and SFINAE magic, we can selectively enable/disable methods depending on + availability of API in wrapped KinematicsPlugin class. + However, as templates and virtual functions cannot be combined, we need helpers initializeImpl(). */ + template + typename std::enable_if::value, bool>::type + initializeImpl(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization) + { + if (tip_frames.size() != 1) + { + RCLCPP_ERROR(moveit::getLogger("moveit.core.cached_ik_kinematics_plugin"), + "This solver does not support multiple tip frames"); + return false; + } + + // call initialize method of wrapped class + if (!KinematicsPlugin::initialize(node, robot_model, group_name, base_frame, tip_frames, search_discretization)) + return false; + initCache(robot_model.getName(), group_name, base_frame + tip_frames[0]); + return true; + } + + template + typename std::enable_if::value, bool>::type + initializeImpl(const rclcpp::Node::SharedPtr& /*unused*/, const moveit::core::RobotModel& /*unused*/, + const std::string& /*unused*/, const std::string& /*unused*/, + const std::vector& /*unused*/, double /*unused*/) + { + return false; // API not supported + } + + template + typename std::enable_if::value, bool>::type + initializeImpl(const std::string& robot_description, const std::string& group_name, const std::string& base_frame, + const std::string& tip_frame, double search_discretization) + { + // call initialize method of wrapped class + if (!KinematicsPlugin::initialize(robot_description, group_name, base_frame, tip_frame, search_discretization)) + return false; + initCache(robot_description, group_name, base_frame + tip_frame); + return true; + } + + template + typename std::enable_if::value, bool>::type + initializeImpl(const std::string& /*unused*/, const std::string& /*unused*/, const std::string& /*unused*/, + const std::string& /*unused*/, double /*unused*/) + { + return false; // API not supported + } +}; + +/** + Caching wrapper for IK solvers that implement the multi-tip API. + + Most solvers don't implement this, so the CachedIKKinematicsPlugin + base class simply doesn't wrap the relevant methods and the + implementation in the abstract base class will be called. Ideally, + the two cache wrapper classes would be combined, but it's tricky to + call a method in kinematics::KinematicsBase or KinematicsPlugin + depending on whether KinematicsPlugin has overridden that method or + not (although it can be done with some template meta-programming). +*/ +template +class CachedMultiTipIKKinematicsPlugin : public CachedIKKinematicsPlugin +{ +public: + using Pose = IKCache::Pose; + using IKEntry = IKCache::IKEntry; + using IKCallbackFn = kinematics::KinematicsBase::IKCallbackFn; + using KinematicsQueryOptions = kinematics::KinematicsQueryOptions; + + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization) override; + + bool searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const KinematicsQueryOptions& options = KinematicsQueryOptions(), + const moveit::core::RobotState* context_state = nullptr) const override; +}; +} // namespace cached_ik_kinematics_plugin + +#include "cached_ik_kinematics_plugin-inl.hpp" diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h index 173471c7ed..bcabf09cf2 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,94 +49,5 @@ // This file is a slightly modified version of #pragma once - -#include -#include -#include - -namespace cached_ik_kinematics_plugin -{ -/** \brief An instance of this class can be used to greedily select a given - number of representatives from a set of data points that are all far - apart from each other. */ -template -class GreedyKCenters -{ -public: - /** \brief The definition of a distance function */ - using DistanceFunction = std::function; - /** \brief A matrix type for storing distances between points and centers */ - using Matrix = Eigen::MatrixXd; - - GreedyKCenters() = default; - GreedyKCenters(const GreedyKCenters&) = default; - GreedyKCenters(GreedyKCenters&&) noexcept = default; - GreedyKCenters& operator=(const GreedyKCenters&) = default; - GreedyKCenters& operator=(GreedyKCenters&&) noexcept = default; - virtual ~GreedyKCenters() = default; - - /** \brief Set the distance function to use */ - void setDistanceFunction(const DistanceFunction& distFun) - { - distFun_ = distFun; - } - - /** \brief Get the distance function used */ - const DistanceFunction& getDistanceFunction() const - { - return distFun_; - } - - /** \brief Greedy algorithm for selecting k centers - \param data a vector of data points - \param k the desired number of centers - \param centers a vector of length k containing the indices into - data of the k centers - \param dists a matrix such that dists(i,j) is the distance - between data[i] and data[center[j]] - */ - void kcenters(const std::vector<_T>& data, unsigned int k, std::vector& centers, Matrix& dists) - { - // array containing the minimum distance between each data point - // and the centers computed so far - std::vector min_dist(data.size(), std::numeric_limits::infinity()); - - centers.clear(); - centers.reserve(k); - if (static_cast(dists.rows()) < data.size() || static_cast(dists.cols()) < k) - dists.resize(std::max(2 * static_cast(dists.rows()) + 1, data.size()), k); - // first center is picked randomly - centers.push_back(std::uniform_int_distribution{ 0, data.size() - 1 }(rsl::rng())); - for (unsigned i = 1; i < k; ++i) - { - unsigned ind = 0; - const _T& center = data[centers[i - 1]]; - double max_dist = -std::numeric_limits::infinity(); - for (unsigned j = 0; j < data.size(); ++j) - { - if ((dists(j, i - 1) = distFun_(data[j], center)) < min_dist[j]) - min_dist[j] = dists(j, i - 1); - // the j-th center is the one furthest away from center 0,..,j-1 - if (min_dist[j] > max_dist) - { - ind = j; - max_dist = min_dist[j]; - } - } - // no more centers available - if (max_dist < std::numeric_limits::epsilon()) - break; - centers.push_back(ind); - } - - const _T& center = data[centers.back()]; - unsigned i = centers.size() - 1; - for (unsigned j = 0; j < data.size(); ++j) - dists(j, i) = distFun_(data[j], center); - } - -protected: - /** \brief The used distance function */ - DistanceFunction distFun_; -}; -} // namespace cached_ik_kinematics_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.hpp b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.hpp new file mode 100644 index 0000000000..173471c7ed --- /dev/null +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.hpp @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Rice University + * 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 Rice University 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: Mark Moll */ + +// This file is a slightly modified version of + +#pragma once + +#include +#include +#include + +namespace cached_ik_kinematics_plugin +{ +/** \brief An instance of this class can be used to greedily select a given + number of representatives from a set of data points that are all far + apart from each other. */ +template +class GreedyKCenters +{ +public: + /** \brief The definition of a distance function */ + using DistanceFunction = std::function; + /** \brief A matrix type for storing distances between points and centers */ + using Matrix = Eigen::MatrixXd; + + GreedyKCenters() = default; + GreedyKCenters(const GreedyKCenters&) = default; + GreedyKCenters(GreedyKCenters&&) noexcept = default; + GreedyKCenters& operator=(const GreedyKCenters&) = default; + GreedyKCenters& operator=(GreedyKCenters&&) noexcept = default; + virtual ~GreedyKCenters() = default; + + /** \brief Set the distance function to use */ + void setDistanceFunction(const DistanceFunction& distFun) + { + distFun_ = distFun; + } + + /** \brief Get the distance function used */ + const DistanceFunction& getDistanceFunction() const + { + return distFun_; + } + + /** \brief Greedy algorithm for selecting k centers + \param data a vector of data points + \param k the desired number of centers + \param centers a vector of length k containing the indices into + data of the k centers + \param dists a matrix such that dists(i,j) is the distance + between data[i] and data[center[j]] + */ + void kcenters(const std::vector<_T>& data, unsigned int k, std::vector& centers, Matrix& dists) + { + // array containing the minimum distance between each data point + // and the centers computed so far + std::vector min_dist(data.size(), std::numeric_limits::infinity()); + + centers.clear(); + centers.reserve(k); + if (static_cast(dists.rows()) < data.size() || static_cast(dists.cols()) < k) + dists.resize(std::max(2 * static_cast(dists.rows()) + 1, data.size()), k); + // first center is picked randomly + centers.push_back(std::uniform_int_distribution{ 0, data.size() - 1 }(rsl::rng())); + for (unsigned i = 1; i < k; ++i) + { + unsigned ind = 0; + const _T& center = data[centers[i - 1]]; + double max_dist = -std::numeric_limits::infinity(); + for (unsigned j = 0; j < data.size(); ++j) + { + if ((dists(j, i - 1) = distFun_(data[j], center)) < min_dist[j]) + min_dist[j] = dists(j, i - 1); + // the j-th center is the one furthest away from center 0,..,j-1 + if (min_dist[j] > max_dist) + { + ind = j; + max_dist = min_dist[j]; + } + } + // no more centers available + if (max_dist < std::numeric_limits::epsilon()) + break; + centers.push_back(ind); + } + + const _T& center = data[centers.back()]; + unsigned i = centers.size() - 1; + for (unsigned j = 0; j < data.size(); ++j) + dists(j, i) = distFun_(data[j], center); + } + +protected: + /** \brief The used distance function */ + DistanceFunction distFun_; +}; +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h index 359dfe5157..9d5afa103e 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,84 +49,5 @@ // This file is a slightly modified version of #pragma once - -#include -#include - -namespace cached_ik_kinematics_plugin -{ -/** \brief Abstract representation of a container that can perform nearest neighbors queries */ -template -class NearestNeighbors -{ -public: - /** \brief The definition of a distance function */ - typedef std::function DistanceFunction; - - NearestNeighbors() = default; - NearestNeighbors(const NearestNeighbors&) = default; - NearestNeighbors(NearestNeighbors&&) noexcept = default; - NearestNeighbors& operator=(const NearestNeighbors&) = default; - NearestNeighbors& operator=(NearestNeighbors&&) noexcept = default; - virtual ~NearestNeighbors() = default; - - /** \brief Set the distance function to use */ - virtual void setDistanceFunction(const DistanceFunction& distFun) - { - distFun_ = distFun; - } - - /** \brief Get the distance function used */ - const DistanceFunction& getDistanceFunction() const - { - return distFun_; - } - - /** \brief Return true if the solutions reported by this data structure - are sorted, when calling nearestK / nearestR. */ - virtual bool reportsSortedResults() const = 0; - - /** \brief Clear the datastructure */ - virtual void clear() = 0; - - /** \brief Add an element to the datastructure */ - virtual void add(const _T& data) = 0; - - /** \brief Add a vector of points */ - virtual void add(const std::vector<_T>& data) - { - for (auto elt = data.begin(); elt != data.end(); ++elt) - add(*elt); - } - - /** \brief Remove an element from the datastructure */ - virtual bool remove(const _T& data) = 0; - - /** \brief Get the nearest neighbor of a point */ - virtual _T nearest(const _T& data) const = 0; - - /** \brief Get the k-nearest neighbors of a point - * - * All the nearest neighbor structures currently return the neighbors in - * sorted order, but this is not required. - */ - virtual void nearestK(const _T& data, std::size_t k, std::vector<_T>& nbh) const = 0; - - /** \brief Get the nearest neighbors of a point, within a specified radius - * - * All the nearest neighbor structures currently return the neighbors in - * sorted order, but this is not required. - */ - virtual void nearestR(const _T& data, double radius, std::vector<_T>& nbh) const = 0; - - /** \brief Get the number of elements in the datastructure */ - virtual std::size_t size() const = 0; - - /** \brief Get all the elements in the datastructure */ - virtual void list(std::vector<_T>& data) const = 0; - -protected: - /** \brief The used distance function */ - DistanceFunction distFun_; -}; -} // namespace cached_ik_kinematics_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.hpp b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.hpp new file mode 100644 index 0000000000..359dfe5157 --- /dev/null +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighbors.hpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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 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 */ + +// This file is a slightly modified version of + +#pragma once + +#include +#include + +namespace cached_ik_kinematics_plugin +{ +/** \brief Abstract representation of a container that can perform nearest neighbors queries */ +template +class NearestNeighbors +{ +public: + /** \brief The definition of a distance function */ + typedef std::function DistanceFunction; + + NearestNeighbors() = default; + NearestNeighbors(const NearestNeighbors&) = default; + NearestNeighbors(NearestNeighbors&&) noexcept = default; + NearestNeighbors& operator=(const NearestNeighbors&) = default; + NearestNeighbors& operator=(NearestNeighbors&&) noexcept = default; + virtual ~NearestNeighbors() = default; + + /** \brief Set the distance function to use */ + virtual void setDistanceFunction(const DistanceFunction& distFun) + { + distFun_ = distFun; + } + + /** \brief Get the distance function used */ + const DistanceFunction& getDistanceFunction() const + { + return distFun_; + } + + /** \brief Return true if the solutions reported by this data structure + are sorted, when calling nearestK / nearestR. */ + virtual bool reportsSortedResults() const = 0; + + /** \brief Clear the datastructure */ + virtual void clear() = 0; + + /** \brief Add an element to the datastructure */ + virtual void add(const _T& data) = 0; + + /** \brief Add a vector of points */ + virtual void add(const std::vector<_T>& data) + { + for (auto elt = data.begin(); elt != data.end(); ++elt) + add(*elt); + } + + /** \brief Remove an element from the datastructure */ + virtual bool remove(const _T& data) = 0; + + /** \brief Get the nearest neighbor of a point */ + virtual _T nearest(const _T& data) const = 0; + + /** \brief Get the k-nearest neighbors of a point + * + * All the nearest neighbor structures currently return the neighbors in + * sorted order, but this is not required. + */ + virtual void nearestK(const _T& data, std::size_t k, std::vector<_T>& nbh) const = 0; + + /** \brief Get the nearest neighbors of a point, within a specified radius + * + * All the nearest neighbor structures currently return the neighbors in + * sorted order, but this is not required. + */ + virtual void nearestR(const _T& data, double radius, std::vector<_T>& nbh) const = 0; + + /** \brief Get the number of elements in the datastructure */ + virtual std::size_t size() const = 0; + + /** \brief Get all the elements in the datastructure */ + virtual void list(std::vector<_T>& data) const = 0; + +protected: + /** \brief The used distance function */ + DistanceFunction distFun_; +}; +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index aaeed641fb..d6f2edaa17 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,719 +49,5 @@ // This file is a slightly modified version of #pragma once - -#include -#include -#include -#include -#include -#include -#include "GreedyKCenters.h" -#include "NearestNeighbors.h" -#include - -namespace cached_ik_kinematics_plugin -{ -/** \brief Geometric Near-neighbor Access Tree (GNAT), a data - structure for nearest neighbor search. - - If GNAT_SAMPLER is defined, then extra code will be enabled to sample - elements from the GNAT with probability inversely proportial to their - local density. - - @par External documentation - S. Brin, Near neighbor search in large metric spaces, in Proc. 21st - Conf. on Very Large Databases (VLDB), pp. 574–584, 1995. - - B. Gipson, M. Moll, and L.E. Kavraki, Resolution independent density - estimation for motion planning in high-dimensional spaces, in - IEEE Intl. Conf. on Robotics and Automation, 2013. - [[PDF]](http://kavrakilab.org/sites/default/files/2013%20resolution%20independent%20density%20estimation%20for%20motion.pdf) -*/ -template -class NearestNeighborsGNAT : public NearestNeighbors<_T> -{ -protected: - // \cond IGNORE - // internally, we use a priority queue for nearest neighbors, paired - // with their distance to the query point - using DataDist = std::pair; - struct DataDistCompare - { - bool operator()(const DataDist& d0, const DataDist& d1) - { - return d0.second < d1.second; - } - }; - using NearQueue = std::priority_queue, DataDistCompare>; - - // another internal data structure is a priority queue of nodes to - // check next for possible nearest neighbors - class Node; - using NodeDist = std::pair; - struct NodeDistCompare - { - bool operator()(const NodeDist& n0, const NodeDist& n1) const - { - return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_); - } - }; - using NodeQueue = std::priority_queue, NodeDistCompare>; - // \endcond - -public: - NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4, unsigned int maxDegree = 12, - unsigned int maxNumPtsPerLeaf = 50, unsigned int removedCacheSize = 500, - bool rebalancing = false) - : NearestNeighbors<_T>() - , degree_(degree) - , minDegree_(std::min(degree, minDegree)) - , maxDegree_(std::max(maxDegree, degree)) - , maxNumPtsPerLeaf_(maxNumPtsPerLeaf) - , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits::max()) - , removedCacheSize_(removedCacheSize) - { - } - - ~NearestNeighborsGNAT() override - { - if (tree_) - delete tree_; - } - // \brief Set the distance function to use - void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction& distFun) override - { - NearestNeighbors<_T>::setDistanceFunction(distFun); - pivotSelector_.setDistanceFunction(distFun); - if (tree_) - rebuildDataStructure(); - } - - void clear() override - { - if (tree_) - { - delete tree_; - tree_ = nullptr; - } - size_ = 0; - removed_.clear(); - if (rebuildSize_ != std::numeric_limits::max()) - rebuildSize_ = maxNumPtsPerLeaf_ * degree_; - } - - bool reportsSortedResults() const override - { - return true; - } - - void add(const _T& data) override - { - if (tree_) - { - if (isRemoved(data)) - rebuildDataStructure(); - tree_->add(*this, data); - } - else - { - tree_ = new Node(degree_, maxNumPtsPerLeaf_, data); - size_ = 1; - } - } - void add(const std::vector<_T>& data) override - { - if (tree_) - { - NearestNeighbors<_T>::add(data); - } - else if (!data.empty()) - { - tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]); - for (unsigned int i = 1; i < data.size(); ++i) - tree_->data_.push_back(data[i]); - size_ += data.size(); - if (tree_->needToSplit(*this)) - tree_->split(*this); - } - } - // \brief Rebuild the internal data structure. - void rebuildDataStructure() - { - std::vector<_T> lst; - list(lst); - clear(); - add(lst); - } - // \brief Remove data from the tree. - // The element won't actually be removed immediately, but just marked - // for removal in the removed_ cache. When the cache is full, the tree - // will be rebuilt and the elements marked for removal will actually - // be removed. - bool remove(const _T& data) override - { - if (size_ == 0u) - return false; - NearQueue nbh_queue; - // find data in tree - bool is_pivot = nearestKInternal(data, 1, nbh_queue); - const _T* d = nbh_queue.top().first; - if (*d != data) - return false; - removed_.insert(d); - size_--; - // if we removed a pivot or if the capacity of removed elements - // has been reached, we rebuild the entire GNAT - if (is_pivot || removed_.size() >= removedCacheSize_) - rebuildDataStructure(); - return true; - } - - _T nearest(const _T& data) const override - { - if (size_) - { - NearQueue nbh_queue; - nearestKInternal(data, 1, nbh_queue); - if (!nbh_queue.empty()) - return *nbh_queue.top().first; - } - throw moveit::Exception("No elements found in nearest neighbors data structure"); - } - - // Return the k nearest neighbors in sorted order - void nearestK(const _T& data, std::size_t k, std::vector<_T>& nbh) const override - { - nbh.clear(); - if (k == 0) - return; - if (size_) - { - NearQueue nbh_queue; - nearestKInternal(data, k, nbh_queue); - postprocessNearest(nbh_queue, nbh); - } - } - - // Return the nearest neighbors within distance \c radius in sorted order - void nearestR(const _T& data, double radius, std::vector<_T>& nbh) const override - { - nbh.clear(); - if (size_) - { - NearQueue nbh_queue; - nearestRInternal(data, radius, nbh_queue); - postprocessNearest(nbh_queue, nbh); - } - } - - std::size_t size() const override - { - return size_; - } - - void list(std::vector<_T>& data) const override - { - data.clear(); - data.reserve(size()); - if (tree_) - tree_->list(*this, data); - } - - // \brief Print a GNAT structure (mostly useful for debugging purposes). - friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat) - { - if (gnat.tree_) - { - out << *gnat.tree_; - if (!gnat.removed_.empty()) - { - out << "Elements marked for removal:\n"; - for (typename std::unordered_set::const_iterator it = gnat.removed_.begin(); - it != gnat.removed_.end(); ++it) - out << **it << '\t'; - out << '\n'; - } - } - return out; - } - - // for debugging purposes - void integrityCheck() - { - std::vector<_T> lst; - std::unordered_set tmp; - // get all elements, including those marked for removal - removed_.swap(tmp); - list(lst); - // check if every element marked for removal is also in the tree - for (typename std::unordered_set::iterator it = tmp.begin(); it != tmp.end(); ++it) - { - unsigned int i; - for (i = 0; i < lst.size(); ++i) - { - if (lst[i] == **it) - break; - } - if (i == lst.size()) - { - // an element marked for removal is not actually in the tree - std::cout << "***** FAIL!! ******\n" << *this << '\n'; - for (unsigned int j = 0; j < lst.size(); ++j) - std::cout << lst[j] << '\t'; - std::cout << '\n'; - } - assert(i != lst.size()); - } - // restore - removed_.swap(tmp); - // get elements in the tree with elements marked for removal purged from the list - list(lst); - if (lst.size() != size_) - std::cout << "#########################################\n" << *this << '\n'; - assert(lst.size() == size_); - } - -protected: - using GNAT = NearestNeighborsGNAT<_T>; - - // Return true iff data has been marked for removal. - bool isRemoved(const _T& data) const - { - return !removed_.empty() && removed_.find(&data) != removed_.end(); - } - - // \brief Return in nbhQueue the k nearest neighbors of data. - // For k=1, return true if the nearest neighbor is a pivot. - // (which is important during removal; removing pivots is a - // special case). - bool nearestKInternal(const _T& data, std::size_t k, NearQueue& nbhQueue) const - { - bool is_pivot; - double dist; - NodeDist node_dist; - NodeQueue node_queue; - - dist = NearestNeighbors<_T>::distFun_(data, tree_->pivot_); - is_pivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist); - tree_->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); - while (!node_queue.empty()) - { - dist = nbhQueue.top().second; // note the difference with nearestRInternal - node_dist = node_queue.top(); - node_queue.pop(); - if (nbhQueue.size() == k && (node_dist.second > node_dist.first->maxRadius_ + dist || - node_dist.second < node_dist.first->minRadius_ - dist)) - continue; - node_dist.first->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); - } - return is_pivot; - } - // \brief Return in nbhQueue the elements that are within distance radius of data. - void nearestRInternal(const _T& data, double radius, NearQueue& nbhQueue) const - { - double dist = radius; // note the difference with nearestKInternal - NodeQueue node_queue; - NodeDist node_dist; - - tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_, NearestNeighbors<_T>::distFun_(data, tree_->pivot_)); - tree_->nearestR(*this, data, radius, nbhQueue, node_queue); - while (!node_queue.empty()) - { - node_dist = node_queue.top(); - node_queue.pop(); - if (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist) - continue; - node_dist.first->nearestR(*this, data, radius, nbhQueue, node_queue); - } - } - // \brief Convert the internal data structure used for storing neighbors - // to the vector that NearestNeighbor API requires. - void postprocessNearest(NearQueue& nbhQueue, std::vector<_T>& nbh) const - { - typename std::vector<_T>::reverse_iterator it; - nbh.resize(nbhQueue.size()); - for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop()) - *it = *nbhQueue.top().first; - } - - // The class used internally to define the GNAT. - class Node - { - public: - // \brief Construct a node of given degree with at most - // \e capacity data elements and with given pivot. - Node(int degree, int capacity, _T pivot) - : degree_(degree) - , pivot_(std::move(pivot)) - , minRadius_(std::numeric_limits::infinity()) - , maxRadius_(-minRadius_) - , minRange_(degree, minRadius_) - , maxRange_(degree, maxRadius_) - { - // The "+1" is needed because we add an element before we check whether to split - data_.reserve(capacity + 1); - } - - ~Node() - { - for (unsigned int i = 0; i < children_.size(); ++i) - delete children_[i]; - } - - // \brief Update minRadius_ and maxRadius_, given that an element - // was added with distance dist to the pivot. - void updateRadius(double dist) - { - if (minRadius_ > dist) - minRadius_ = dist; -#ifndef GNAT_SAMPLER - if (maxRadius_ < dist) - maxRadius_ = dist; -#else - if (maxRadius_ < dist) - { - maxRadius_ = dist; - activity_ = 0; - } - else - activity_ = std::max(-32, activity_ - 1); -#endif - } - // \brief Update minRange_[i] and maxRange_[i], given that an - // element was added to the i-th child of the parent that has - // distance dist to this Node's pivot. - void updateRange(unsigned int i, double dist) - { - if (minRange_[i] > dist) - minRange_[i] = dist; - if (maxRange_[i] < dist) - maxRange_[i] = dist; - } - // Add an element to the tree rooted at this node. - void add(GNAT& gnat, const _T& data) - { - if (children_.empty()) - { - data_.push_back(data); - gnat.size_++; - if (needToSplit(gnat)) - { - if (!gnat.removed_.empty()) - { - gnat.rebuildDataStructure(); - } - else if (gnat.size_ >= gnat.rebuildSize_) - { - gnat.rebuildSize_ <<= 1; - gnat.rebuildDataStructure(); - } - else - split(gnat); - } - } - else - { - std::vector dist(children_.size()); - double min_dist = dist[0] = gnat.distFun_(data, children_[0]->pivot_); - int min_ind = 0; - - for (unsigned int i = 1; i < children_.size(); ++i) - { - if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < min_dist) - { - min_dist = dist[i]; - min_ind = i; - } - } - for (unsigned int i = 0; i < children_.size(); ++i) - children_[i]->updateRange(min_ind, dist[i]); - children_[min_ind]->updateRadius(min_dist); - children_[min_ind]->add(gnat, data); - } - } - // Return true iff the node needs to be split into child nodes. - bool needToSplit(const GNAT& gnat) const - { - unsigned int sz = data_.size(); - return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_; - } - // \brief The split operation finds pivot elements for the child - // nodes and moves each data element of this node to the appropriate - // child node. - void split(GNAT& gnat) - { - typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_); - std::vector pivots; - - children_.reserve(degree_); - gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists); - for (unsigned int& pivot : pivots) - children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot])); - degree_ = pivots.size(); // in case fewer than degree_ pivots were found - for (unsigned int j = 0; j < data_.size(); ++j) - { - unsigned int k = 0; - for (unsigned int i = 1; i < degree_; ++i) - { - if (dists(j, i) < dists(j, k)) - k = i; - } - Node* child = children_[k]; - if (j != pivots[k]) - { - child->data_.push_back(data_[j]); - child->updateRadius(dists(j, k)); - } - for (unsigned int i = 0; i < degree_; ++i) - children_[i]->updateRange(k, dists(j, i)); - } - - for (unsigned int i = 0; i < degree_; ++i) - { - // make sure degree lies between minDegree_ and maxDegree_ - children_[i]->degree_ = - std::min(std::max(static_cast(((degree_ * children_[i]->data_.size()) / data_.size())), - gnat.minDegree_), - gnat.maxDegree_); - // singleton - if (children_[i]->minRadius_ >= std::numeric_limits::infinity()) - children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.; - } - // this does more than clear(); it also sets capacity to 0 and frees the memory - std::vector<_T> tmp; - data_.swap(tmp); - // check if new leaves need to be split - for (unsigned int i = 0; i < degree_; ++i) - { - if (children_[i]->needToSplit(gnat)) - children_[i]->split(gnat); - } - } - - // Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh. - bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const - { - if (nbh.size() < k) - { - nbh.push(std::make_pair(&data, dist)); - return true; - } - if (dist < nbh.top().second || (dist < std::numeric_limits::epsilon() && data == key)) - { - nbh.pop(); - nbh.push(std::make_pair(&data, dist)); - return true; - } - return false; - } - - // \brief Compute the k nearest neighbors of data in the tree. - // For k=1, isPivot is true if the nearest neighbor is a pivot - // (which is important during removal; removing pivots is a - // special case). The nodeQueue, which contains other Nodes - // that need to be checked for nearest neighbors, is updated. - void nearestK(const GNAT& gnat, const _T& data, std::size_t k, NearQueue& nbh, NodeQueue& nodeQueue, - bool& isPivot) const - { - for (unsigned int i = 0; i < data_.size(); ++i) - { - if (!gnat.isRemoved(data_[i])) - { - if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i]))) - isPivot = false; - } - } - if (!children_.empty()) - { - double dist; - Node* child; - std::vector dist_to_pivot(children_.size()); - std::vector permutation(children_.size()); - for (unsigned int i = 0; i < permutation.size(); ++i) - permutation[i] = i; - std::shuffle(permutation.begin(), permutation.end(), rsl::rng()); - - for (unsigned int i = 0; i < children_.size(); ++i) - { - if (permutation[i] >= 0) - { - child = children_[permutation[i]]; - dist_to_pivot[permutation[i]] = gnat.distFun_(data, child->pivot_); - if (insertNeighborK(nbh, k, child->pivot_, data, dist_to_pivot[permutation[i]])) - isPivot = true; - if (nbh.size() == k) - { - dist = nbh.top().second; // note difference with nearestR - for (unsigned int j = 0; j < children_.size(); ++j) - { - if (permutation[j] >= 0 && i != j && - (dist_to_pivot[permutation[i]] - dist > child->maxRange_[permutation[j]] || - dist_to_pivot[permutation[i]] + dist < child->minRange_[permutation[j]])) - permutation[j] = -1; - } - } - } - } - - dist = nbh.top().second; - for (unsigned int i = 0; i < children_.size(); ++i) - { - if (permutation[i] >= 0) - { - child = children_[permutation[i]]; - if (nbh.size() < k || (dist_to_pivot[permutation[i]] - dist <= child->maxRadius_ && - dist_to_pivot[permutation[i]] + dist >= child->minRadius_)) - nodeQueue.push(std::make_pair(child, dist_to_pivot[permutation[i]])); - } - } - } - } - // Insert data in nbh if it is a near neighbor. - void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const - { - if (dist <= r) - nbh.push(std::make_pair(&data, dist)); - } - // \brief Return all elements that are within distance r in nbh. - // The nodeQueue, which contains other Nodes that need to - // be checked for nearest neighbors, is updated. - void nearestR(const GNAT& gnat, const _T& data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const - { - double dist = r; // note difference with nearestK - - for (unsigned int i = 0; i < data_.size(); ++i) - { - if (!gnat.isRemoved(data_[i])) - insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i])); - } - if (!children_.empty()) - { - Node* child; - std::vector dist_to_pivot(children_.size()); - std::vector permutation(children_.size()); - for (unsigned int i = 0; i < permutation.size(); ++i) - permutation[i] = i; - std::shuffle(permutation.begin(), permutation.end(), rsl::rng()); - - for (unsigned int i = 0; i < children_.size(); ++i) - { - if (permutation[i] >= 0) - { - child = children_[permutation[i]]; - dist_to_pivot[i] = gnat.distFun_(data, child->pivot_); - insertNeighborR(nbh, r, child->pivot_, dist_to_pivot[i]); - for (unsigned int j = 0; j < children_.size(); ++j) - { - if (permutation[j] >= 0 && i != j && - (dist_to_pivot[i] - dist > child->maxRange_[permutation[j]] || - dist_to_pivot[i] + dist < child->minRange_[permutation[j]])) - permutation[j] = -1; - } - } - } - - for (unsigned int i = 0; i < children_.size(); ++i) - { - if (permutation[i] >= 0) - { - child = children_[permutation[i]]; - if (dist_to_pivot[i] - dist <= child->maxRadius_ && dist_to_pivot[i] + dist >= child->minRadius_) - nodeQueue.push(std::make_pair(child, dist_to_pivot[i])); - } - } - } - } - - void list(const GNAT& gnat, std::vector<_T>& data) const - { - if (!gnat.isRemoved(pivot_)) - data.push_back(pivot_); - for (unsigned int i = 0; i < data_.size(); ++i) - { - if (!gnat.isRemoved(data_[i])) - data.push_back(data_[i]); - } - for (unsigned int i = 0; i < children_.size(); ++i) - children_[i]->list(gnat, data); - } - - friend std::ostream& operator<<(std::ostream& out, const Node& node) - { - out << "\ndegree:\t" << node.degree_; - out << "\nminRadius:\t" << node.minRadius_; - out << "\nmaxRadius:\t" << node.maxRadius_; - out << "\nminRange:\t"; - for (unsigned int i = 0; i < node.minRange_.size(); ++i) - out << node.minRange_[i] << '\t'; - out << "\nmaxRange: "; - for (unsigned int i = 0; i < node.maxRange_.size(); ++i) - out << node.maxRange_[i] << '\t'; - out << "\npivot:\t" << node.pivot_; - out << "\ndata: "; - for (unsigned int i = 0; i < node.data_.size(); ++i) - out << node.data_[i] << '\t'; - out << "\nthis:\t" << &node; - out << "\nchildren:\n"; - for (unsigned int i = 0; i < node.children_.size(); ++i) - out << node.children_[i] << '\t'; - out << '\n'; - for (unsigned int i = 0; i < node.children_.size(); ++i) - out << *node.children_[i] << '\n'; - return out; - } - - // Number of child nodes - unsigned int degree_; - // Data element stored in this Node - const _T pivot_; - // Minimum distance between the pivot element and the elements stored in data_ - double minRadius_; - // Maximum distance between the pivot element and the elements stored in data_ - double maxRadius_; - // \brief The i-th element in minRange_ is the minimum distance between the - // pivot and any data_ element in the i-th child node of this node's parent. - std::vector minRange_; - // \brief The i-th element in maxRange_ is the maximum distance between the - // pivot and any data_ element in the i-th child node of this node's parent. - std::vector maxRange_; - // \brief The data elements stored in this node (in addition to the pivot - // element). An internal node has no elements stored in data_. - std::vector<_T> data_; - // \brief The child nodes of this node. By definition, only internal nodes - // have child nodes. - std::vector children_; - }; - - // \brief The data structure containing the elements stored in this structure. - Node* tree_{ nullptr }; - // The desired degree of each node. - unsigned int degree_; - // \brief After splitting a Node, each child Node has degree equal to - // the default degree times the fraction of data elements from the - // original node that got assigned to that child Node. However, its - // degree can be no less than minDegree_. - unsigned int minDegree_; - // \brief After splitting a Node, each child Node has degree equal to - // the default degree times the fraction of data elements from the - // original node that got assigned to that child Node. However, its - // degree can be no larger than maxDegree_. - unsigned int maxDegree_; - // \brief Maximum number of elements allowed to be stored in a Node before - // it needs to be split into several nodes. - unsigned int maxNumPtsPerLeaf_; - // \brief Number of elements stored in the tree. - std::size_t size_{ 0 }; - // \brief If size_ exceeds rebuildSize_, the tree will be rebuilt (and - // automatically rebalanced), and rebuildSize_ will be doubled. - std::size_t rebuildSize_; - // \brief Maximum number of removed elements that can be stored in the - // removed_ cache. If the cache is full, the tree will be rebuilt with - // the elements in removed_ actually removed from the tree. - std::size_t removedCacheSize_; - // \brief The data structure used to split data into subtrees. - GreedyKCenters<_T> pivotSelector_; - // \brief Cache of removed elements. - std::unordered_set removed_; -}; -} // namespace cached_ik_kinematics_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.hpp b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.hpp new file mode 100644 index 0000000000..1c60e31934 --- /dev/null +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.hpp @@ -0,0 +1,755 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Rice University + * 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 Rice University 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: Mark Moll, Bryant Gipson */ + +// This file is a slightly modified version of + +#pragma once + +#include +#include +#include +#include +#include +#include +#include "GreedyKCenters.hpp" +#include "NearestNeighbors.hpp" +#include + +namespace cached_ik_kinematics_plugin +{ +/** \brief Geometric Near-neighbor Access Tree (GNAT), a data + structure for nearest neighbor search. + + If GNAT_SAMPLER is defined, then extra code will be enabled to sample + elements from the GNAT with probability inversely proportial to their + local density. + + @par External documentation + S. Brin, Near neighbor search in large metric spaces, in Proc. 21st + Conf. on Very Large Databases (VLDB), pp. 574–584, 1995. + + B. Gipson, M. Moll, and L.E. Kavraki, Resolution independent density + estimation for motion planning in high-dimensional spaces, in + IEEE Intl. Conf. on Robotics and Automation, 2013. + [[PDF]](http://kavrakilab.org/sites/default/files/2013%20resolution%20independent%20density%20estimation%20for%20motion.pdf) +*/ +template +class NearestNeighborsGNAT : public NearestNeighbors<_T> +{ +protected: + // \cond IGNORE + // internally, we use a priority queue for nearest neighbors, paired + // with their distance to the query point + using DataDist = std::pair; + struct DataDistCompare + { + bool operator()(const DataDist& d0, const DataDist& d1) + { + return d0.second < d1.second; + } + }; + using NearQueue = std::priority_queue, DataDistCompare>; + + // another internal data structure is a priority queue of nodes to + // check next for possible nearest neighbors + class Node; + using NodeDist = std::pair; + struct NodeDistCompare + { + bool operator()(const NodeDist& n0, const NodeDist& n1) const + { + return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_); + } + }; + using NodeQueue = std::priority_queue, NodeDistCompare>; + // \endcond + +public: + NearestNeighborsGNAT(unsigned int degree = 8, unsigned int minDegree = 4, unsigned int maxDegree = 12, + unsigned int maxNumPtsPerLeaf = 50, unsigned int removedCacheSize = 500, + bool rebalancing = false) + : NearestNeighbors<_T>() + , degree_(degree) + , minDegree_(std::min(degree, minDegree)) + , maxDegree_(std::max(maxDegree, degree)) + , maxNumPtsPerLeaf_(maxNumPtsPerLeaf) + , rebuildSize_(rebalancing ? maxNumPtsPerLeaf * degree : std::numeric_limits::max()) + , removedCacheSize_(removedCacheSize) + { + } + + ~NearestNeighborsGNAT() override + { + if (tree_) + delete tree_; + } + // \brief Set the distance function to use + void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction& distFun) override + { + NearestNeighbors<_T>::setDistanceFunction(distFun); + pivotSelector_.setDistanceFunction(distFun); + if (tree_) + rebuildDataStructure(); + } + + void clear() override + { + if (tree_) + { + delete tree_; + tree_ = nullptr; + } + size_ = 0; + removed_.clear(); + if (rebuildSize_ != std::numeric_limits::max()) + rebuildSize_ = maxNumPtsPerLeaf_ * degree_; + } + + bool reportsSortedResults() const override + { + return true; + } + + void add(const _T& data) override + { + if (tree_) + { + if (isRemoved(data)) + rebuildDataStructure(); + tree_->add(*this, data); + } + else + { + tree_ = new Node(degree_, maxNumPtsPerLeaf_, data); + size_ = 1; + } + } + void add(const std::vector<_T>& data) override + { + if (tree_) + { + NearestNeighbors<_T>::add(data); + } + else if (!data.empty()) + { + tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]); + for (unsigned int i = 1; i < data.size(); ++i) + tree_->data_.push_back(data[i]); + size_ += data.size(); + if (tree_->needToSplit(*this)) + tree_->split(*this); + } + } + // \brief Rebuild the internal data structure. + void rebuildDataStructure() + { + std::vector<_T> lst; + list(lst); + clear(); + add(lst); + } + // \brief Remove data from the tree. + // The element won't actually be removed immediately, but just marked + // for removal in the removed_ cache. When the cache is full, the tree + // will be rebuilt and the elements marked for removal will actually + // be removed. + bool remove(const _T& data) override + { + if (size_ == 0u) + return false; + NearQueue nbh_queue; + // find data in tree + bool is_pivot = nearestKInternal(data, 1, nbh_queue); + const _T* d = nbh_queue.top().first; + if (*d != data) + return false; + removed_.insert(d); + size_--; + // if we removed a pivot or if the capacity of removed elements + // has been reached, we rebuild the entire GNAT + if (is_pivot || removed_.size() >= removedCacheSize_) + rebuildDataStructure(); + return true; + } + + _T nearest(const _T& data) const override + { + if (size_) + { + NearQueue nbh_queue; + nearestKInternal(data, 1, nbh_queue); + if (!nbh_queue.empty()) + return *nbh_queue.top().first; + } + throw moveit::Exception("No elements found in nearest neighbors data structure"); + } + + // Return the k nearest neighbors in sorted order + void nearestK(const _T& data, std::size_t k, std::vector<_T>& nbh) const override + { + nbh.clear(); + if (k == 0) + return; + if (size_) + { + NearQueue nbh_queue; + nearestKInternal(data, k, nbh_queue); + postprocessNearest(nbh_queue, nbh); + } + } + + // Return the nearest neighbors within distance \c radius in sorted order + void nearestR(const _T& data, double radius, std::vector<_T>& nbh) const override + { + nbh.clear(); + if (size_) + { + NearQueue nbh_queue; + nearestRInternal(data, radius, nbh_queue); + postprocessNearest(nbh_queue, nbh); + } + } + + std::size_t size() const override + { + return size_; + } + + void list(std::vector<_T>& data) const override + { + data.clear(); + data.reserve(size()); + if (tree_) + tree_->list(*this, data); + } + + // \brief Print a GNAT structure (mostly useful for debugging purposes). + friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat) + { + if (gnat.tree_) + { + out << *gnat.tree_; + if (!gnat.removed_.empty()) + { + out << "Elements marked for removal:\n"; + for (typename std::unordered_set::const_iterator it = gnat.removed_.begin(); + it != gnat.removed_.end(); ++it) + out << **it << '\t'; + out << '\n'; + } + } + return out; + } + + // for debugging purposes + void integrityCheck() + { + std::vector<_T> lst; + std::unordered_set tmp; + // get all elements, including those marked for removal + removed_.swap(tmp); + list(lst); + // check if every element marked for removal is also in the tree + for (typename std::unordered_set::iterator it = tmp.begin(); it != tmp.end(); ++it) + { + unsigned int i; + for (i = 0; i < lst.size(); ++i) + { + if (lst[i] == **it) + break; + } + if (i == lst.size()) + { + // an element marked for removal is not actually in the tree + std::cout << "***** FAIL!! ******\n" << *this << '\n'; + for (unsigned int j = 0; j < lst.size(); ++j) + std::cout << lst[j] << '\t'; + std::cout << '\n'; + } + assert(i != lst.size()); + } + // restore + removed_.swap(tmp); + // get elements in the tree with elements marked for removal purged from the list + list(lst); + if (lst.size() != size_) + std::cout << "#########################################\n" << *this << '\n'; + assert(lst.size() == size_); + } + +protected: + using GNAT = NearestNeighborsGNAT<_T>; + + // Return true iff data has been marked for removal. + bool isRemoved(const _T& data) const + { + return !removed_.empty() && removed_.find(&data) != removed_.end(); + } + + // \brief Return in nbhQueue the k nearest neighbors of data. + // For k=1, return true if the nearest neighbor is a pivot. + // (which is important during removal; removing pivots is a + // special case). + bool nearestKInternal(const _T& data, std::size_t k, NearQueue& nbhQueue) const + { + bool is_pivot; + double dist; + NodeDist node_dist; + NodeQueue node_queue; + + dist = NearestNeighbors<_T>::distFun_(data, tree_->pivot_); + is_pivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data, dist); + tree_->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); + while (!node_queue.empty()) + { + dist = nbhQueue.top().second; // note the difference with nearestRInternal + node_dist = node_queue.top(); + node_queue.pop(); + if (nbhQueue.size() == k && (node_dist.second > node_dist.first->maxRadius_ + dist || + node_dist.second < node_dist.first->minRadius_ - dist)) + continue; + node_dist.first->nearestK(*this, data, k, nbhQueue, node_queue, is_pivot); + } + return is_pivot; + } + // \brief Return in nbhQueue the elements that are within distance radius of data. + void nearestRInternal(const _T& data, double radius, NearQueue& nbhQueue) const + { + double dist = radius; // note the difference with nearestKInternal + NodeQueue node_queue; + NodeDist node_dist; + + tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_, NearestNeighbors<_T>::distFun_(data, tree_->pivot_)); + tree_->nearestR(*this, data, radius, nbhQueue, node_queue); + while (!node_queue.empty()) + { + node_dist = node_queue.top(); + node_queue.pop(); + if (node_dist.second > node_dist.first->maxRadius_ + dist || node_dist.second < node_dist.first->minRadius_ - dist) + continue; + node_dist.first->nearestR(*this, data, radius, nbhQueue, node_queue); + } + } + // \brief Convert the internal data structure used for storing neighbors + // to the vector that NearestNeighbor API requires. + void postprocessNearest(NearQueue& nbhQueue, std::vector<_T>& nbh) const + { + typename std::vector<_T>::reverse_iterator it; + nbh.resize(nbhQueue.size()); + for (it = nbh.rbegin(); it != nbh.rend(); it++, nbhQueue.pop()) + *it = *nbhQueue.top().first; + } + + // The class used internally to define the GNAT. + class Node + { + public: + // \brief Construct a node of given degree with at most + // \e capacity data elements and with given pivot. + Node(int degree, int capacity, _T pivot) + : degree_(degree) + , pivot_(std::move(pivot)) + , minRadius_(std::numeric_limits::infinity()) + , maxRadius_(-minRadius_) + , minRange_(degree, minRadius_) + , maxRange_(degree, maxRadius_) + { + // The "+1" is needed because we add an element before we check whether to split + data_.reserve(capacity + 1); + } + + ~Node() + { + for (unsigned int i = 0; i < children_.size(); ++i) + delete children_[i]; + } + + // \brief Update minRadius_ and maxRadius_, given that an element + // was added with distance dist to the pivot. + void updateRadius(double dist) + { + if (minRadius_ > dist) + minRadius_ = dist; +#ifndef GNAT_SAMPLER + if (maxRadius_ < dist) + maxRadius_ = dist; +#else + if (maxRadius_ < dist) + { + maxRadius_ = dist; + activity_ = 0; + } + else + activity_ = std::max(-32, activity_ - 1); +#endif + } + // \brief Update minRange_[i] and maxRange_[i], given that an + // element was added to the i-th child of the parent that has + // distance dist to this Node's pivot. + void updateRange(unsigned int i, double dist) + { + if (minRange_[i] > dist) + minRange_[i] = dist; + if (maxRange_[i] < dist) + maxRange_[i] = dist; + } + // Add an element to the tree rooted at this node. + void add(GNAT& gnat, const _T& data) + { + if (children_.empty()) + { + data_.push_back(data); + gnat.size_++; + if (needToSplit(gnat)) + { + if (!gnat.removed_.empty()) + { + gnat.rebuildDataStructure(); + } + else if (gnat.size_ >= gnat.rebuildSize_) + { + gnat.rebuildSize_ <<= 1; + gnat.rebuildDataStructure(); + } + else + split(gnat); + } + } + else + { + std::vector dist(children_.size()); + double min_dist = dist[0] = gnat.distFun_(data, children_[0]->pivot_); + int min_ind = 0; + + for (unsigned int i = 1; i < children_.size(); ++i) + { + if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < min_dist) + { + min_dist = dist[i]; + min_ind = i; + } + } + for (unsigned int i = 0; i < children_.size(); ++i) + children_[i]->updateRange(min_ind, dist[i]); + children_[min_ind]->updateRadius(min_dist); + children_[min_ind]->add(gnat, data); + } + } + // Return true iff the node needs to be split into child nodes. + bool needToSplit(const GNAT& gnat) const + { + unsigned int sz = data_.size(); + return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_; + } + // \brief The split operation finds pivot elements for the child + // nodes and moves each data element of this node to the appropriate + // child node. + void split(GNAT& gnat) + { + typename GreedyKCenters<_T>::Matrix dists(data_.size(), degree_); + std::vector pivots; + + children_.reserve(degree_); + gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists); + for (unsigned int& pivot : pivots) + children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivot])); + degree_ = pivots.size(); // in case fewer than degree_ pivots were found + for (unsigned int j = 0; j < data_.size(); ++j) + { + unsigned int k = 0; + for (unsigned int i = 1; i < degree_; ++i) + { + if (dists(j, i) < dists(j, k)) + k = i; + } + Node* child = children_[k]; + if (j != pivots[k]) + { + child->data_.push_back(data_[j]); + child->updateRadius(dists(j, k)); + } + for (unsigned int i = 0; i < degree_; ++i) + children_[i]->updateRange(k, dists(j, i)); + } + + for (unsigned int i = 0; i < degree_; ++i) + { + // make sure degree lies between minDegree_ and maxDegree_ + children_[i]->degree_ = + std::min(std::max(static_cast(((degree_ * children_[i]->data_.size()) / data_.size())), + gnat.minDegree_), + gnat.maxDegree_); + // singleton + if (children_[i]->minRadius_ >= std::numeric_limits::infinity()) + children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.; + } + // this does more than clear(); it also sets capacity to 0 and frees the memory + std::vector<_T> tmp; + data_.swap(tmp); + // check if new leaves need to be split + for (unsigned int i = 0; i < degree_; ++i) + { + if (children_[i]->needToSplit(gnat)) + children_[i]->split(gnat); + } + } + + // Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh. + bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const + { + if (nbh.size() < k) + { + nbh.push(std::make_pair(&data, dist)); + return true; + } + if (dist < nbh.top().second || (dist < std::numeric_limits::epsilon() && data == key)) + { + nbh.pop(); + nbh.push(std::make_pair(&data, dist)); + return true; + } + return false; + } + + // \brief Compute the k nearest neighbors of data in the tree. + // For k=1, isPivot is true if the nearest neighbor is a pivot + // (which is important during removal; removing pivots is a + // special case). The nodeQueue, which contains other Nodes + // that need to be checked for nearest neighbors, is updated. + void nearestK(const GNAT& gnat, const _T& data, std::size_t k, NearQueue& nbh, NodeQueue& nodeQueue, + bool& isPivot) const + { + for (unsigned int i = 0; i < data_.size(); ++i) + { + if (!gnat.isRemoved(data_[i])) + { + if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i]))) + isPivot = false; + } + } + if (!children_.empty()) + { + double dist; + Node* child; + std::vector dist_to_pivot(children_.size()); + std::vector permutation(children_.size()); + for (unsigned int i = 0; i < permutation.size(); ++i) + permutation[i] = i; + std::shuffle(permutation.begin(), permutation.end(), rsl::rng()); + + for (unsigned int i = 0; i < children_.size(); ++i) + { + if (permutation[i] >= 0) + { + child = children_[permutation[i]]; + dist_to_pivot[permutation[i]] = gnat.distFun_(data, child->pivot_); + if (insertNeighborK(nbh, k, child->pivot_, data, dist_to_pivot[permutation[i]])) + isPivot = true; + if (nbh.size() == k) + { + dist = nbh.top().second; // note difference with nearestR + for (unsigned int j = 0; j < children_.size(); ++j) + { + if (permutation[j] >= 0 && i != j && + (dist_to_pivot[permutation[i]] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[permutation[i]] + dist < child->minRange_[permutation[j]])) + permutation[j] = -1; + } + } + } + } + + dist = nbh.top().second; + for (unsigned int i = 0; i < children_.size(); ++i) + { + if (permutation[i] >= 0) + { + child = children_[permutation[i]]; + if (nbh.size() < k || (dist_to_pivot[permutation[i]] - dist <= child->maxRadius_ && + dist_to_pivot[permutation[i]] + dist >= child->minRadius_)) + nodeQueue.push(std::make_pair(child, dist_to_pivot[permutation[i]])); + } + } + } + } + // Insert data in nbh if it is a near neighbor. + void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const + { + if (dist <= r) + nbh.push(std::make_pair(&data, dist)); + } + // \brief Return all elements that are within distance r in nbh. + // The nodeQueue, which contains other Nodes that need to + // be checked for nearest neighbors, is updated. + void nearestR(const GNAT& gnat, const _T& data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const + { + double dist = r; // note difference with nearestK + + for (unsigned int i = 0; i < data_.size(); ++i) + { + if (!gnat.isRemoved(data_[i])) + insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i])); + } + if (!children_.empty()) + { + Node* child; + std::vector dist_to_pivot(children_.size()); + std::vector permutation(children_.size()); + for (unsigned int i = 0; i < permutation.size(); ++i) + permutation[i] = i; + std::shuffle(permutation.begin(), permutation.end(), rsl::rng()); + + for (unsigned int i = 0; i < children_.size(); ++i) + { + if (permutation[i] >= 0) + { + child = children_[permutation[i]]; + dist_to_pivot[i] = gnat.distFun_(data, child->pivot_); + insertNeighborR(nbh, r, child->pivot_, dist_to_pivot[i]); + for (unsigned int j = 0; j < children_.size(); ++j) + { + if (permutation[j] >= 0 && i != j && + (dist_to_pivot[i] - dist > child->maxRange_[permutation[j]] || + dist_to_pivot[i] + dist < child->minRange_[permutation[j]])) + permutation[j] = -1; + } + } + } + + for (unsigned int i = 0; i < children_.size(); ++i) + { + if (permutation[i] >= 0) + { + child = children_[permutation[i]]; + if (dist_to_pivot[i] - dist <= child->maxRadius_ && dist_to_pivot[i] + dist >= child->minRadius_) + nodeQueue.push(std::make_pair(child, dist_to_pivot[i])); + } + } + } + } + + void list(const GNAT& gnat, std::vector<_T>& data) const + { + if (!gnat.isRemoved(pivot_)) + data.push_back(pivot_); + for (unsigned int i = 0; i < data_.size(); ++i) + { + if (!gnat.isRemoved(data_[i])) + data.push_back(data_[i]); + } + for (unsigned int i = 0; i < children_.size(); ++i) + children_[i]->list(gnat, data); + } + + friend std::ostream& operator<<(std::ostream& out, const Node& node) + { + out << "\ndegree:\t" << node.degree_; + out << "\nminRadius:\t" << node.minRadius_; + out << "\nmaxRadius:\t" << node.maxRadius_; + out << "\nminRange:\t"; + for (unsigned int i = 0; i < node.minRange_.size(); ++i) + out << node.minRange_[i] << '\t'; + out << "\nmaxRange: "; + for (unsigned int i = 0; i < node.maxRange_.size(); ++i) + out << node.maxRange_[i] << '\t'; + out << "\npivot:\t" << node.pivot_; + out << "\ndata: "; + for (unsigned int i = 0; i < node.data_.size(); ++i) + out << node.data_[i] << '\t'; + out << "\nthis:\t" << &node; + out << "\nchildren:\n"; + for (unsigned int i = 0; i < node.children_.size(); ++i) + out << node.children_[i] << '\t'; + out << '\n'; + for (unsigned int i = 0; i < node.children_.size(); ++i) + out << *node.children_[i] << '\n'; + return out; + } + + // Number of child nodes + unsigned int degree_; + // Data element stored in this Node + const _T pivot_; + // Minimum distance between the pivot element and the elements stored in data_ + double minRadius_; + // Maximum distance between the pivot element and the elements stored in data_ + double maxRadius_; + // \brief The i-th element in minRange_ is the minimum distance between the + // pivot and any data_ element in the i-th child node of this node's parent. + std::vector minRange_; + // \brief The i-th element in maxRange_ is the maximum distance between the + // pivot and any data_ element in the i-th child node of this node's parent. + std::vector maxRange_; + // \brief The data elements stored in this node (in addition to the pivot + // element). An internal node has no elements stored in data_. + std::vector<_T> data_; + // \brief The child nodes of this node. By definition, only internal nodes + // have child nodes. + std::vector children_; + }; + + // \brief The data structure containing the elements stored in this structure. + Node* tree_{ nullptr }; + // The desired degree of each node. + unsigned int degree_; + // \brief After splitting a Node, each child Node has degree equal to + // the default degree times the fraction of data elements from the + // original node that got assigned to that child Node. However, its + // degree can be no less than minDegree_. + unsigned int minDegree_; + // \brief After splitting a Node, each child Node has degree equal to + // the default degree times the fraction of data elements from the + // original node that got assigned to that child Node. However, its + // degree can be no larger than maxDegree_. + unsigned int maxDegree_; + // \brief Maximum number of elements allowed to be stored in a Node before + // it needs to be split into several nodes. + unsigned int maxNumPtsPerLeaf_; + // \brief Number of elements stored in the tree. + std::size_t size_{ 0 }; + // \brief If size_ exceeds rebuildSize_, the tree will be rebuilt (and + // automatically rebalanced), and rebuildSize_ will be doubled. + std::size_t rebuildSize_; + // \brief Maximum number of removed elements that can be stored in the + // removed_ cache. If the cache is full, the tree will be rebuilt with + // the elements in removed_ actually removed from the tree. + std::size_t removedCacheSize_; + // \brief The data structure used to split data into subtrees. + GreedyKCenters<_T> pivotSelector_; + // \brief Cache of removed elements. + std::unordered_set removed_; +}; +} // namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp index e103aa2637..fbe26c3e5c 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp @@ -34,9 +34,9 @@ /* Author: Mark Moll */ -#include -#include -#include +#include +#include +#include #include // register CachedIKKinematicsPlugin as a KinematicsBase implementation diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ur_kinematics_plugin.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ur_kinematics_plugin.cpp index c13b2d5a06..14a6ef98d8 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ur_kinematics_plugin.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ur_kinematics_plugin.cpp @@ -34,7 +34,7 @@ /* Author: Mark Moll */ -#include +#include #include #include diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp index 65d2ad4c0c..e6bf5bdbe7 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include namespace cached_ik_kinematics_plugin diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index fdba5f1f5b..49f506b420 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -42,8 +42,8 @@ */ #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.h new file mode 100644 index 0000000000..883be380eb --- /dev/null +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.h @@ -0,0 +1,40 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +// Copyright (C) 2007 Ruben Smits + +// Version: 1.0 +// Author: Ruben Smits +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +// Modified to account for "mimic" joints, i.e. joints whose motion has a +// linear relationship to that of another joint. +// Copyright (C) 2013 Sachin Chitta, Willow Garage + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.h new file mode 100644 index 0000000000..ff60467ac2 --- /dev/null +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Sachin Chitta */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h index 6a0901a497..6f12af21ff 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,145 +47,5 @@ /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */ #pragma once - -// ROS -#include -#include -#include - -// ROS msgs -#include -#include -#include -#include -#include - -// KDL -#include -#include -#include - -// MoveIt -#include -#include -#include -#include - -#include - -namespace KDL -{ -class ChainIkSolverVelMimicSVD; -} - -namespace kdl_kinematics_plugin -{ -/** - * @brief Specific implementation of kinematics using KDL. - * This version supports any kinematic chain, also including mimic joints. - */ -class KDLKinematicsPlugin : public kinematics::KinematicsBase -{ -public: - /** - * @brief Default constructor - */ - KDLKinematicsPlugin(); - - bool - getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const override; - - bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization) override; - - /** - * @brief Return all the joint names in the order they are used internally - */ - const std::vector& getJointNames() const override; - - /** - * @brief Return all the link names in the order they are represented internally - */ - const std::vector& getLinkNames() const override; - -protected: - typedef Eigen::Matrix Twist; - - /// Solve position IK given initial joint values - // NOLINTNEXTLINE(readability-identifier-naming) - int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in, - KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights, - const Twist& cartesian_weights) const; - -private: - void getJointWeights(); - bool timedOut(const rclcpp::Time& start_time, double duration) const; - - /** @brief Check whether the solution lies within the consistency limits of the seed state - * @param seed_state Seed state - * @param consistency_limits - * @param solution solution configuration - * @return true if check succeeds - */ - bool checkConsistency(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, - const Eigen::VectorXd& solution) const; - - void getRandomConfiguration(Eigen::VectorXd& jnt_array) const; - - /** @brief Get a random configuration within consistency limits close to the seed state - * @param seed_state Seed state - * @param consistency_limits - * @param jnt_array Returned random configuration - */ - void getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, - Eigen::VectorXd& jnt_array) const; - - /// clip q_delta such that joint limits will not be violated - void clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting) const; - - bool initialized_; ///< Internal variable that indicates whether solver is configured and ready - - unsigned int dimension_; ///< Dimension of the group - moveit_msgs::msg::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver - - const moveit::core::JointModelGroup* joint_model_group_; - moveit::core::RobotStatePtr state_; - KDL::Chain kdl_chain_; - std::unique_ptr fk_solver_; - std::vector mimic_joints_; - std::vector joint_weights_; - Eigen::VectorXd joint_min_, joint_max_; ///< joint limits - - std::shared_ptr param_listener_; - kdl_kinematics::Params params_; -}; -} // namespace kdl_kinematics_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.hpp b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.hpp new file mode 100644 index 0000000000..f6bb09f0e5 --- /dev/null +++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.hpp @@ -0,0 +1,179 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Sachin Chitta, David Lu!!, Ugo Cupcic */ + +#pragma once + +// ROS +#include +#include +#include + +// ROS msgs +#include +#include +#include +#include +#include + +// KDL +#include +#include +#include + +// MoveIt +#include +#include +#include +#include + +#include + +namespace KDL +{ +class ChainIkSolverVelMimicSVD; +} + +namespace kdl_kinematics_plugin +{ +/** + * @brief Specific implementation of kinematics using KDL. + * This version supports any kinematic chain, also including mimic joints. + */ +class KDLKinematicsPlugin : public kinematics::KinematicsBase +{ +public: + /** + * @brief Default constructor + */ + KDLKinematicsPlugin(); + + bool + getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const override; + + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization) override; + + /** + * @brief Return all the joint names in the order they are used internally + */ + const std::vector& getJointNames() const override; + + /** + * @brief Return all the link names in the order they are represented internally + */ + const std::vector& getLinkNames() const override; + +protected: + typedef Eigen::Matrix Twist; + + /// Solve position IK given initial joint values + // NOLINTNEXTLINE(readability-identifier-naming) + int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in, + KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights, + const Twist& cartesian_weights) const; + +private: + void getJointWeights(); + bool timedOut(const rclcpp::Time& start_time, double duration) const; + + /** @brief Check whether the solution lies within the consistency limits of the seed state + * @param seed_state Seed state + * @param consistency_limits + * @param solution solution configuration + * @return true if check succeeds + */ + bool checkConsistency(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, + const Eigen::VectorXd& solution) const; + + void getRandomConfiguration(Eigen::VectorXd& jnt_array) const; + + /** @brief Get a random configuration within consistency limits close to the seed state + * @param seed_state Seed state + * @param consistency_limits + * @param jnt_array Returned random configuration + */ + void getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, + Eigen::VectorXd& jnt_array) const; + + /// clip q_delta such that joint limits will not be violated + void clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting) const; + + bool initialized_; ///< Internal variable that indicates whether solver is configured and ready + + unsigned int dimension_; ///< Dimension of the group + moveit_msgs::msg::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver + + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr state_; + KDL::Chain kdl_chain_; + std::unique_ptr fk_solver_; + std::vector mimic_joints_; + std::vector joint_weights_; + Eigen::VectorXd joint_min_, joint_max_; ///< joint limits + + std::shared_ptr param_listener_; + kdl_kinematics::Params params_; +}; +} // namespace kdl_kinematics_plugin diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index 54d3f0aa4f..3d7f69bac1 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -34,7 +34,7 @@ /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */ -#include +#include #include #include diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 2283bb510b..a99cf7c1f0 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -40,120 +52,5 @@ */ #pragma once - -// ROS2 -#include -#include - -// System -#include - -// ROS msgs -#include -#include -#include - -// MoveIt -#include -#include - -namespace srv_kinematics_plugin -{ -/** - * @brief Specific implementation of kinematics using ROS service calls to communicate with - external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups - */ -class SrvKinematicsPlugin : public kinematics::KinematicsBase -{ -public: - /** - * @brief Default constructor - */ - SrvKinematicsPlugin(); - - bool - getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, - double timeout, const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - const moveit::core::RobotState* context_state = nullptr) const override; - - bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const override; - - bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_name, - const std::vector& tip_frames, double search_discretization) override; - - /** - * @brief Return all the joint names in the order they are used internally - */ - const std::vector& getJointNames() const override; - - /** - * @brief Return all the link names in the order they are represented internally - */ - const std::vector& getLinkNames() const override; - - /** - * @brief Return all the variable names in the order they are represented internally - */ - const std::vector& getVariableNames() const; - -protected: - bool setRedundantJoints(const std::vector& redundant_joint_indices) override; - -private: - bool timedOut(const rclcpp::Time& start_time, double duration) const; - - int getJointIndex(const std::string& name) const; - - bool isRedundantJoint(unsigned int index) const; - - bool active_; /** Internal variable that indicates whether solvers are configured and ready */ - - moveit_msgs::msg::KinematicSolverInfo ik_group_info_; /** Stores information for the inverse kinematics solver */ - - unsigned int dimension_; /** Dimension of the group */ - - const moveit::core::JointModelGroup* joint_model_group_; - - moveit::core::RobotStatePtr robot_state_; - - int num_possible_redundant_joints_; - - rclcpp::Client::SharedPtr ik_service_client_; - - rclcpp::Node::SharedPtr node_; - - std::shared_ptr param_listener_; - srv_kinematics::Params params_; -}; -} // namespace srv_kinematics_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.hpp b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.hpp new file mode 100644 index 0000000000..84b2289ad3 --- /dev/null +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.hpp @@ -0,0 +1,159 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, JSK, The University of Tokyo. + * 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 JSK, The University of Tokyo 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: Dave Coleman, Masaki Murooka + Desc: Connects MoveIt to any inverse kinematics solver via a ROS service call + Supports planning groups with multiple tip frames + \todo: better support for mimic joints + \todo: better support for redundant joints +*/ + +#pragma once + +// ROS2 +#include +#include + +// System +#include + +// ROS msgs +#include +#include +#include + +// MoveIt +#include +#include + +namespace srv_kinematics_plugin +{ +/** + * @brief Specific implementation of kinematics using ROS service calls to communicate with + external IK solvers. This version can be used with any robot. Supports non-chain kinematic groups + */ +class SrvKinematicsPlugin : public kinematics::KinematicsBase +{ +public: + /** + * @brief Default constructor + */ + SrvKinematicsPlugin(); + + bool + getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const moveit::core::RobotState* context_state = nullptr) const override; + + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const override; + + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_name, + const std::vector& tip_frames, double search_discretization) override; + + /** + * @brief Return all the joint names in the order they are used internally + */ + const std::vector& getJointNames() const override; + + /** + * @brief Return all the link names in the order they are represented internally + */ + const std::vector& getLinkNames() const override; + + /** + * @brief Return all the variable names in the order they are represented internally + */ + const std::vector& getVariableNames() const; + +protected: + bool setRedundantJoints(const std::vector& redundant_joint_indices) override; + +private: + bool timedOut(const rclcpp::Time& start_time, double duration) const; + + int getJointIndex(const std::string& name) const; + + bool isRedundantJoint(unsigned int index) const; + + bool active_; /** Internal variable that indicates whether solvers are configured and ready */ + + moveit_msgs::msg::KinematicSolverInfo ik_group_info_; /** Stores information for the inverse kinematics solver */ + + unsigned int dimension_; /** Dimension of the group */ + + const moveit::core::JointModelGroup* joint_model_group_; + + moveit::core::RobotStatePtr robot_state_; + + int num_possible_redundant_joints_; + + rclcpp::Client::SharedPtr ik_service_client_; + + rclcpp::Node::SharedPtr node_; + + std::shared_ptr param_listener_; + srv_kinematics::Params params_; +}; +} // namespace srv_kinematics_plugin diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index 9538cf080c..44c86b0691 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -35,9 +35,9 @@ /* Author: Dave Coleman, Masaki Murooka */ #include -#include +#include #include -#include +#include #include #include diff --git a/moveit_kinematics/test/benchmark_ik.cpp b/moveit_kinematics/test/benchmark_ik.cpp index da3a0ca323..a09a149735 100644 --- a/moveit_kinematics/test/benchmark_ik.cpp +++ b/moveit_kinematics/test/benchmark_ik.cpp @@ -37,9 +37,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include namespace po = boost::program_options; diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 87f870550c..54a082b329 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -43,17 +43,17 @@ #include // MoveIt -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include #include -#include +#include -#include +#include #include rclcpp::Logger getLogger() diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h index 1b11f6396d..0ff890315f 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,32 +47,5 @@ /* Author: E. Gil Jones */ #pragma once - -#include -#include - -#include - -namespace chomp_interface -{ -MOVEIT_CLASS_FORWARD(CHOMPInterface); // Defines CHOMPInterfacePtr, ConstPtr, WeakPtr... etc - -class CHOMPInterface : public chomp::ChompPlanner -{ -public: - CHOMPInterface(const rclcpp::Node::SharedPtr& node); - - const chomp::ChompParameters& getParams() const - { - return params_; - } - -protected: - /** @brief Configure everything using the param server */ - void loadParams(); - - std::shared_ptr node_; /// The ROS node - - chomp::ChompParameters params_; -}; -} // namespace chomp_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.hpp b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.hpp new file mode 100644 index 0000000000..d00462fffa --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_interface.hpp @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: E. Gil Jones */ + +#pragma once + +#include +#include + +#include + +namespace chomp_interface +{ +MOVEIT_CLASS_FORWARD(CHOMPInterface); // Defines CHOMPInterfacePtr, ConstPtr, WeakPtr... etc + +class CHOMPInterface : public chomp::ChompPlanner +{ +public: + CHOMPInterface(const rclcpp::Node::SharedPtr& node); + + const chomp::ChompParameters& getParams() const + { + return params_; + } + +protected: + /** @brief Configure everything using the param server */ + void loadParams(); + + std::shared_ptr node_; /// The ROS node + + chomp::ChompParameters params_; +}; +} // namespace chomp_interface diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h index 908d315fc8..30db2519ad 100644 --- a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,35 +47,5 @@ /* Author: Chittaranjan Srinivas Swaminathan */ #pragma once - -#include -#include - -#include - -namespace chomp_interface -{ -MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); // Defines CHOMPPlanningContextPtr, ConstPtr, WeakPtr... etc - -class CHOMPPlanningContext : public planning_interface::PlanningContext -{ -public: - void solve(planning_interface::MotionPlanResponse& res) override; - void solve(planning_interface::MotionPlanDetailedResponse& res) override; - - void clear() override; - bool terminate() override; - - CHOMPPlanningContext(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const rclcpp::Node::SharedPtr& node); - - ~CHOMPPlanningContext() override = default; - - void initialize(); - -private: - CHOMPInterfacePtr chomp_interface_; - moveit::core::RobotModelConstPtr robot_model_; -}; - -} // namespace chomp_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.hpp b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.hpp new file mode 100644 index 0000000000..dcb69871c6 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, 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: Chittaranjan Srinivas Swaminathan */ + +#pragma once + +#include +#include + +#include + +namespace chomp_interface +{ +MOVEIT_CLASS_FORWARD(CHOMPPlanningContext); // Defines CHOMPPlanningContextPtr, ConstPtr, WeakPtr... etc + +class CHOMPPlanningContext : public planning_interface::PlanningContext +{ +public: + void solve(planning_interface::MotionPlanResponse& res) override; + void solve(planning_interface::MotionPlanDetailedResponse& res) override; + + void clear() override; + bool terminate() override; + + CHOMPPlanningContext(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const rclcpp::Node::SharedPtr& node); + + ~CHOMPPlanningContext() override = default; + + void initialize(); + +private: + CHOMPInterfacePtr chomp_interface_; + moveit::core::RobotModelConstPtr robot_model_; +}; + +} // namespace chomp_interface diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp index f7cd00ad37..dd1446435d 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp @@ -34,7 +34,7 @@ /* Author: E. Gil Jones */ -#include +#include #include namespace chomp_interface diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index 5f93522139..e24c953718 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -34,8 +34,8 @@ /* Author: Chittaranjan Srinivas Swaminathan */ -#include -#include +#include +#include namespace chomp_interface { diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index 997bf4a964..569742c10d 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -32,11 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp index 0d84b43521..7c92e7135d 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp @@ -37,8 +37,8 @@ #include #include -#include -#include +#include +#include class CHOMPMoveitTest : public ::testing::Test { diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp index 7e69f51c41..30b2e3343c 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp @@ -35,8 +35,8 @@ /// \author Bence Magyar #include -#include -#include +#include +#include #include class CHOMPMoveitTest : public ::testing::Test diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h index 086b352f0a..35954d379f 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,66 +47,5 @@ /* Author: Mrinal Kalakrishnan */ #pragma once - -#include - -#include -#include - -namespace chomp -{ -/** - * \brief Represents the smoothness cost for CHOMP, for a single joint - */ -class ChompCost -{ -public: - ChompCost(const ChompTrajectory& trajectory, int joint_number, const std::vector& derivative_costs, - double ridge_factor = 0.0); - virtual ~ChompCost(); - - template - void getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase& derivative) const; - - const Eigen::MatrixXd& getQuadraticCostInverse() const; - - const Eigen::MatrixXd& getQuadraticCost() const; - - double getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const; - - double getMaxQuadCostInvValue() const; - - void scale(double scale); - -private: - Eigen::MatrixXd quad_cost_full_; - Eigen::MatrixXd quad_cost_; - // Eigen::VectorXd linear_cost_; - Eigen::MatrixXd quad_cost_inv_; - - Eigen::MatrixXd getDiffMatrix(int size, const double* diff_rule) const; -}; - -template -void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, - Eigen::MatrixBase& derivative) const -{ - derivative = (quad_cost_full_ * (2.0 * joint_trajectory)); -} - -inline const Eigen::MatrixXd& ChompCost::getQuadraticCostInverse() const -{ - return quad_cost_inv_; -} - -inline const Eigen::MatrixXd& ChompCost::getQuadraticCost() const -{ - return quad_cost_; -} - -inline double ChompCost::getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const -{ - return joint_trajectory.dot(quad_cost_full_ * joint_trajectory); -} - -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.hpp new file mode 100644 index 0000000000..f0abfc365d --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_cost.hpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan */ + +#pragma once + +#include + +#include +#include + +namespace chomp +{ +/** + * \brief Represents the smoothness cost for CHOMP, for a single joint + */ +class ChompCost +{ +public: + ChompCost(const ChompTrajectory& trajectory, int joint_number, const std::vector& derivative_costs, + double ridge_factor = 0.0); + virtual ~ChompCost(); + + template + void getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, Eigen::MatrixBase& derivative) const; + + const Eigen::MatrixXd& getQuadraticCostInverse() const; + + const Eigen::MatrixXd& getQuadraticCost() const; + + double getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const; + + double getMaxQuadCostInvValue() const; + + void scale(double scale); + +private: + Eigen::MatrixXd quad_cost_full_; + Eigen::MatrixXd quad_cost_; + // Eigen::VectorXd linear_cost_; + Eigen::MatrixXd quad_cost_inv_; + + Eigen::MatrixXd getDiffMatrix(int size, const double* diff_rule) const; +}; + +template +void ChompCost::getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory, + Eigen::MatrixBase& derivative) const +{ + derivative = (quad_cost_full_ * (2.0 * joint_trajectory)); +} + +inline const Eigen::MatrixXd& ChompCost::getQuadraticCostInverse() const +{ + return quad_cost_inv_; +} + +inline const Eigen::MatrixXd& ChompCost::getQuadraticCost() const +{ + return quad_cost_; +} + +inline double ChompCost::getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const +{ + return joint_trajectory.dot(quad_cost_full_ * joint_trajectory); +} + +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h index ceb68ab3a5..d8b69aeeb4 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,185 +47,5 @@ /* Author: Mrinal Kalakrishnan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace chomp -{ -class ChompOptimizer -{ -public: - ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene, - const std::string& planning_group, const ChompParameters* parameters, - const moveit::core::RobotState& start_state); - - virtual ~ChompOptimizer(); - - /** - * Optimizes the CHOMP cost function and tries to find an optimal path - * @return true if an optimal collision free path is found else returns false - */ - bool optimize(); - - inline void destroy() - { - // Nothing for now. - } - - bool isInitialized() const - { - return initialized_; - } - - bool isCollisionFree() const - { - return is_collision_free_; - } - -private: - inline double getPotential(double field_distance, double radius, double clearance) - { - double d = field_distance - radius; - - if (d >= clearance) // everything is fine - { - return 0.0; - } - else if (d >= 0.0) // transition phase, no collision yet - { - const double diff = (d - clearance); - const double gradient_magnitude = diff / clearance; - return 0.5 * gradient_magnitude * diff; // 0.5 * (d - clearance)^2 / clearance - } - else // d < 0.0: collision - { - return -d + 0.5 * clearance; // linearly increase, starting from 0.5 * clearance - } - } - template - void getJacobian(int trajectoryPoint, Eigen::Vector3d& collision_point_pos, std::string& jointName, - Eigen::MatrixBase& jacobian) const; - - // void getRandomState(const moveit::core::RobotState& currentState, - // const std::string& group_name, - // Eigen::VectorXd& state_vec); - - void setRobotStateFromPoint(ChompTrajectory& group_trajectory, int i); - - // collision_proximity::CollisionProximitySpace::TrajectorySafety checkCurrentIterValidity(); - - int num_joints_; - int num_vars_free_; - int num_vars_all_; - int num_collision_points_; - int free_vars_start_; - int free_vars_end_; - int iteration_; - unsigned int collision_free_iteration_; - - ChompTrajectory* full_trajectory_; - const moveit::core::RobotModelConstPtr& robot_model_; - std::string planning_group_; - const ChompParameters* parameters_; - ChompTrajectory group_trajectory_; - planning_scene::PlanningSceneConstPtr planning_scene_; - moveit::core::RobotState state_; - moveit::core::RobotState start_state_; - const moveit::core::JointModelGroup* joint_model_group_; - const collision_detection::CollisionEnvHybrid* hy_env_; - - std::vector joint_costs_; - collision_detection::GroupStateRepresentationPtr gsr_; - bool initialized_; - - std::vector > collision_point_joint_names_; - std::vector collision_point_pos_eigen_; - std::vector collision_point_vel_eigen_; - std::vector collision_point_acc_eigen_; - std::vector > collision_point_potential_; - std::vector > collision_point_vel_mag_; - std::vector collision_point_potential_gradient_; - std::vector joint_axes_; - std::vector joint_positions_; - Eigen::MatrixXd group_trajectory_backup_; - Eigen::MatrixXd best_group_trajectory_; - double best_group_trajectory_cost_; - int last_improvement_iteration_; - unsigned int num_collision_free_iterations_; - - // HMC stuff: - Eigen::MatrixXd momentum_; - Eigen::MatrixXd random_momentum_; - Eigen::VectorXd random_joint_momentum_; // temporary variable - std::vector multivariate_gaussian_; - double stochasticity_factor_; - - std::vector state_is_in_collision_; /**< Array containing a boolean about collision info for each point in the - trajectory */ - std::vector > point_is_in_collision_; - bool is_collision_free_; - double worst_collision_cost_state_; - - Eigen::MatrixXd smoothness_increments_; - Eigen::MatrixXd collision_increments_; - Eigen::MatrixXd final_increments_; - - // temporary variables for all functions: - Eigen::VectorXd smoothness_derivative_; - Eigen::MatrixXd jacobian_; - Eigen::MatrixXd jacobian_pseudo_inverse_; - Eigen::MatrixXd jacobian_jacobian_tranpose_; - Eigen::VectorXd random_state_; - Eigen::VectorXd joint_state_velocities_; - - std::vector joint_names_; - std::map > joint_parent_map_; - - inline bool isParent(const std::string& childLink, const std::string& parentLink) const - { - if (childLink == parentLink) - { - return true; - } - - if (joint_parent_map_.find(childLink) == joint_parent_map_.end()) - { - // ROS_ERROR("%s was not in joint parent map! for lookup of %s", childLink.c_str(), parentLink.c_str()); - return false; - } - const std::map& parents = joint_parent_map_.at(childLink); - return (parents.find(parentLink) != parents.end() && parents.at(parentLink)); - } - - void registerParents(const moveit::core::JointModel* model); - void initialize(); - void calculateSmoothnessIncrements(); - void calculateCollisionIncrements(); - void calculateTotalIncrements(); - void performForwardKinematics(); - void addIncrementsToTrajectory(); - void updateFullTrajectory(); - void debugCost(); - void handleJointLimits(); - double getTrajectoryCost(); - double getSmoothnessCost(); - double getCollisionCost(); - void perturbTrajectory(); - void getRandomMomentum(); - void updateMomentum(); - void updatePositionFromMomentum(); - void calculatePseudoInverse(); - void computeJointProperties(int trajectoryPoint); - bool isCurrentTrajectoryMeshToMeshCollisionFree() const; -}; -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.hpp new file mode 100644 index 0000000000..f0025808d1 --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_optimizer.hpp @@ -0,0 +1,219 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace chomp +{ +class ChompOptimizer +{ +public: + ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene, + const std::string& planning_group, const ChompParameters* parameters, + const moveit::core::RobotState& start_state); + + virtual ~ChompOptimizer(); + + /** + * Optimizes the CHOMP cost function and tries to find an optimal path + * @return true if an optimal collision free path is found else returns false + */ + bool optimize(); + + inline void destroy() + { + // Nothing for now. + } + + bool isInitialized() const + { + return initialized_; + } + + bool isCollisionFree() const + { + return is_collision_free_; + } + +private: + inline double getPotential(double field_distance, double radius, double clearance) + { + double d = field_distance - radius; + + if (d >= clearance) // everything is fine + { + return 0.0; + } + else if (d >= 0.0) // transition phase, no collision yet + { + const double diff = (d - clearance); + const double gradient_magnitude = diff / clearance; + return 0.5 * gradient_magnitude * diff; // 0.5 * (d - clearance)^2 / clearance + } + else // d < 0.0: collision + { + return -d + 0.5 * clearance; // linearly increase, starting from 0.5 * clearance + } + } + template + void getJacobian(int trajectoryPoint, Eigen::Vector3d& collision_point_pos, std::string& jointName, + Eigen::MatrixBase& jacobian) const; + + // void getRandomState(const moveit::core::RobotState& currentState, + // const std::string& group_name, + // Eigen::VectorXd& state_vec); + + void setRobotStateFromPoint(ChompTrajectory& group_trajectory, int i); + + // collision_proximity::CollisionProximitySpace::TrajectorySafety checkCurrentIterValidity(); + + int num_joints_; + int num_vars_free_; + int num_vars_all_; + int num_collision_points_; + int free_vars_start_; + int free_vars_end_; + int iteration_; + unsigned int collision_free_iteration_; + + ChompTrajectory* full_trajectory_; + const moveit::core::RobotModelConstPtr& robot_model_; + std::string planning_group_; + const ChompParameters* parameters_; + ChompTrajectory group_trajectory_; + planning_scene::PlanningSceneConstPtr planning_scene_; + moveit::core::RobotState state_; + moveit::core::RobotState start_state_; + const moveit::core::JointModelGroup* joint_model_group_; + const collision_detection::CollisionEnvHybrid* hy_env_; + + std::vector joint_costs_; + collision_detection::GroupStateRepresentationPtr gsr_; + bool initialized_; + + std::vector > collision_point_joint_names_; + std::vector collision_point_pos_eigen_; + std::vector collision_point_vel_eigen_; + std::vector collision_point_acc_eigen_; + std::vector > collision_point_potential_; + std::vector > collision_point_vel_mag_; + std::vector collision_point_potential_gradient_; + std::vector joint_axes_; + std::vector joint_positions_; + Eigen::MatrixXd group_trajectory_backup_; + Eigen::MatrixXd best_group_trajectory_; + double best_group_trajectory_cost_; + int last_improvement_iteration_; + unsigned int num_collision_free_iterations_; + + // HMC stuff: + Eigen::MatrixXd momentum_; + Eigen::MatrixXd random_momentum_; + Eigen::VectorXd random_joint_momentum_; // temporary variable + std::vector multivariate_gaussian_; + double stochasticity_factor_; + + std::vector state_is_in_collision_; /**< Array containing a boolean about collision info for each point in the + trajectory */ + std::vector > point_is_in_collision_; + bool is_collision_free_; + double worst_collision_cost_state_; + + Eigen::MatrixXd smoothness_increments_; + Eigen::MatrixXd collision_increments_; + Eigen::MatrixXd final_increments_; + + // temporary variables for all functions: + Eigen::VectorXd smoothness_derivative_; + Eigen::MatrixXd jacobian_; + Eigen::MatrixXd jacobian_pseudo_inverse_; + Eigen::MatrixXd jacobian_jacobian_tranpose_; + Eigen::VectorXd random_state_; + Eigen::VectorXd joint_state_velocities_; + + std::vector joint_names_; + std::map > joint_parent_map_; + + inline bool isParent(const std::string& childLink, const std::string& parentLink) const + { + if (childLink == parentLink) + { + return true; + } + + if (joint_parent_map_.find(childLink) == joint_parent_map_.end()) + { + // ROS_ERROR("%s was not in joint parent map! for lookup of %s", childLink.c_str(), parentLink.c_str()); + return false; + } + const std::map& parents = joint_parent_map_.at(childLink); + return (parents.find(parentLink) != parents.end() && parents.at(parentLink)); + } + + void registerParents(const moveit::core::JointModel* model); + void initialize(); + void calculateSmoothnessIncrements(); + void calculateCollisionIncrements(); + void calculateTotalIncrements(); + void performForwardKinematics(); + void addIncrementsToTrajectory(); + void updateFullTrajectory(); + void debugCost(); + void handleJointLimits(); + double getTrajectoryCost(); + double getSmoothnessCost(); + double getCollisionCost(); + void perturbTrajectory(); + void getRandomMomentum(); + void updateMomentum(); + void updatePositionFromMomentum(); + void calculatePseudoInverse(); + void computeJointProperties(int trajectoryPoint); + bool isCurrentTrajectoryMeshToMeshCollisionFree() const; +}; +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h index aea7dde68b..c6391cf9cc 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,62 +50,5 @@ #include #pragma once - -namespace chomp -{ -class ChompParameters -{ -public: - ChompParameters(); - virtual ~ChompParameters(); - - /** - * sets the recovery parameters which can be changed in case CHOMP is not able to find a solution with the parameters - * set from the chomp_planning.yaml file - * @param learning_rate - * @param ridge_factor - * @param planning_time_limit - * @param max_iterations - */ - void setRecoveryParams(double learning_rate, double ridge_factor, int planning_time_limit, int max_iterations); - - /** - * sets a valid trajectory initialization method - * @return true if a valid method (one of VALID_INITIALIZATION_METHODS) was specified - */ - bool setTrajectoryInitializationMethod(std::string method); - -public: - double planning_time_limit_; /*!< maximum time the optimizer can take to find a solution before terminating */ - int max_iterations_; /*!< maximum number of iterations that the planner can take to find a good solution while optimization */ - int max_iterations_after_collision_free_; /*!< maximum iterations to be performed after a collision free path is found. */ - double smoothness_cost_weight_; /*!< smoothness_cost_weight parameters controls its weight in the final cost that - CHOMP is actually optimizing over */ - double obstacle_cost_weight_; /*!< controls the weight to be given to obstacles towards the final cost CHOMP optimizes over */ - double - learning_rate_; /*!< learning rate used by the optimizer to find the local / global minima while reducing the total cost */ - double smoothness_cost_velocity_; /*!< variables associated with the cost in velocity */ - double smoothness_cost_acceleration_; /*!< variables associated with the cost in acceleration */ - double smoothness_cost_jerk_; /*!< variables associated with the cost in jerk */ - bool use_stochastic_descent_; /*!< set this to true/false if you want to use stochastic descent while optimizing the cost. */ - - double - ridge_factor_; /*!< the noise added to the diagnal of the total quadratic cost matrix in the objective function */ - bool use_pseudo_inverse_; /*!< enable pseudo inverse calculations or not. */ - double pseudo_inverse_ridge_factor_; /*!< set the ridge factor if pseudo inverse is enabled */ - - double joint_update_limit_; /*!< set the update limit for the robot joints */ - double min_clearance_; /*!< the minimum distance that needs to be maintained to avoid obstacles */ - double collision_threshold_; /*!< the collision threshold cost that needs to be maintained to avoid collisions */ - bool filter_mode_; - - static const std::vector VALID_INITIALIZATION_METHODS; - std::string trajectory_initialization_method_; /*!< trajectory initialization method to be specified */ - - bool enable_failure_recovery_; /*!< if set to true, CHOMP tries to vary certain parameters to try and find a path if - an initial path is not found with the specified chomp parameters */ - int max_recovery_attempts_; /*!< this the maximum recovery attempts to find a collision free path after an initial - failure to find a solution */ -}; - -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.hpp new file mode 100644 index 0000000000..aea7dde68b --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.hpp @@ -0,0 +1,99 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan */ + +#include +#include + +#pragma once + +namespace chomp +{ +class ChompParameters +{ +public: + ChompParameters(); + virtual ~ChompParameters(); + + /** + * sets the recovery parameters which can be changed in case CHOMP is not able to find a solution with the parameters + * set from the chomp_planning.yaml file + * @param learning_rate + * @param ridge_factor + * @param planning_time_limit + * @param max_iterations + */ + void setRecoveryParams(double learning_rate, double ridge_factor, int planning_time_limit, int max_iterations); + + /** + * sets a valid trajectory initialization method + * @return true if a valid method (one of VALID_INITIALIZATION_METHODS) was specified + */ + bool setTrajectoryInitializationMethod(std::string method); + +public: + double planning_time_limit_; /*!< maximum time the optimizer can take to find a solution before terminating */ + int max_iterations_; /*!< maximum number of iterations that the planner can take to find a good solution while optimization */ + int max_iterations_after_collision_free_; /*!< maximum iterations to be performed after a collision free path is found. */ + double smoothness_cost_weight_; /*!< smoothness_cost_weight parameters controls its weight in the final cost that + CHOMP is actually optimizing over */ + double obstacle_cost_weight_; /*!< controls the weight to be given to obstacles towards the final cost CHOMP optimizes over */ + double + learning_rate_; /*!< learning rate used by the optimizer to find the local / global minima while reducing the total cost */ + double smoothness_cost_velocity_; /*!< variables associated with the cost in velocity */ + double smoothness_cost_acceleration_; /*!< variables associated with the cost in acceleration */ + double smoothness_cost_jerk_; /*!< variables associated with the cost in jerk */ + bool use_stochastic_descent_; /*!< set this to true/false if you want to use stochastic descent while optimizing the cost. */ + + double + ridge_factor_; /*!< the noise added to the diagnal of the total quadratic cost matrix in the objective function */ + bool use_pseudo_inverse_; /*!< enable pseudo inverse calculations or not. */ + double pseudo_inverse_ridge_factor_; /*!< set the ridge factor if pseudo inverse is enabled */ + + double joint_update_limit_; /*!< set the update limit for the robot joints */ + double min_clearance_; /*!< the minimum distance that needs to be maintained to avoid obstacles */ + double collision_threshold_; /*!< the collision threshold cost that needs to be maintained to avoid collisions */ + bool filter_mode_; + + static const std::vector VALID_INITIALIZATION_METHODS; + std::string trajectory_initialization_method_; /*!< trajectory initialization method to be specified */ + + bool enable_failure_recovery_; /*!< if set to true, CHOMP tries to vary certain parameters to try and find a path if + an initial path is not found with the specified chomp parameters */ + int max_recovery_attempts_; /*!< this the maximum recovery attempts to find a collision free path after an initial + failure to find a solution */ +}; + +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h index a6bca7c57e..dd70021f71 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,23 +47,5 @@ /* Author: E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include - -namespace chomp -{ -class ChompPlanner -{ -public: - ChompPlanner() = default; - virtual ~ChompPlanner() = default; - - void solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, const ChompParameters& params, - planning_interface::MotionPlanDetailedResponse& res) const; -}; -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.hpp new file mode 100644 index 0000000000..fd107fa7e6 --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_planner.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace chomp +{ +class ChompPlanner +{ +public: + ChompPlanner() = default; + virtual ~ChompPlanner() = default; + + void solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, const ChompParameters& params, + planning_interface::MotionPlanDetailedResponse& res) const; +}; +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h index 45e2b8af0d..3c9ce483df 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,274 +47,5 @@ /* Author: Mrinal Kalakrishnan */ #pragma once - -#include -#include - -#include -#include -#include -#include -#include - -namespace chomp -{ -/** - * \brief Represents a discretized joint-space trajectory for CHOMP - */ -class ChompTrajectory -{ -public: - /** - * \brief Constructs a trajectory for a given robot model, trajectory duration, and discretization - */ - ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration, double discretization, - const std::string& group_name); - - /** - * \brief Constructs a trajectory for a given robot model, number of trajectory points, and discretization - */ - ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, size_t num_points, double discretization, - const std::string& group_name); - - /** - * \brief Creates a new containing only the joints of interest, and adds padding to the start - * and end if needed, to have enough trajectory points for the differentiation rules - */ - ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, int diff_rule_length); - - ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, - const trajectory_msgs::msg::JointTrajectory& traj); - - /** - * \brief Destructor - */ - virtual ~ChompTrajectory() = default; - - double& operator()(size_t traj_point, size_t joint); - - double operator()(size_t traj_point, size_t joint) const; - - Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point); - - Eigen::MatrixXd::ColXpr getJointTrajectory(int joint); - - /** - * \brief Gets the number of points in the trajectory - */ - size_t getNumPoints() const; - - /** - * \brief Gets the number of points (that are free to be optimized) in the trajectory - */ - size_t getNumFreePoints() const; - - /** - * \brief Gets the number of joints in each trajectory point - */ - size_t getNumJoints() const; - - /** - * \brief Gets the discretization time interval of the trajectory - */ - double getDiscretization() const; - - /** - * \brief Generates a minimum jerk trajectory from the start index to end index - * - * Only modifies points from start_index_ to end_index_, inclusive. - */ - void fillInMinJerk(); - - /** - * \brief Generates a linearly interpolated trajectory from the start index to end index - * - * Only modifies points from start_index_ to end_index_, inclusive - */ - void fillInLinearInterpolation(); - - /** - * \brief Generates a cubic interpolation of the trajectory from the start index to end index - * - * Only modifies points from start_index_ to end_index_, inclusive - */ - void fillInCubicInterpolation(); - - /** - * \brief Receives the path obtained from a given MotionPlanDetailedResponse res object's trajectory (e.g., trajectory - * produced by OMPL) and puts it into the appropriate trajectory format required for CHOMP - * @param res - */ - bool fillInFromTrajectory(const robot_trajectory::RobotTrajectory& trajectory); - - /** - * \brief This function assigns the given \a source RobotState to the row at index \a chomp_trajectory_point - * - * @param source The source RobotState - * @param chomp_trajectory_point index of the chomp_trajectory's point (row) - * @param group JointModelGroup determining the joints to copy - */ - void assignCHOMPTrajectoryPointFromRobotState(const moveit::core::RobotState& source, size_t chomp_trajectory_point, - const moveit::core::JointModelGroup* group); - - /** - * \brief Sets the start and end index for the modifiable part of the trajectory - * - * (Everything before the start and after the end index is considered fixed) - * The values default to 1 and getNumPoints()-2 - */ - void setStartEndIndex(size_t start_index, size_t end_index); - - /** - * \brief Gets the start index - */ - size_t getStartIndex() const; - - /** - * \brief Gets the end index - */ - size_t getEndIndex() const; - - /** - * \brief Gets the entire trajectory matrix - */ - Eigen::MatrixXd& getTrajectory(); - - /** - * \brief Gets the block of the trajectory which can be optimized - */ - Eigen::Block getFreeTrajectoryBlock(); - - /** - * \brief Gets the block of free (optimizable) trajectory for a single joint - */ - Eigen::Block getFreeJointTrajectoryBlock(size_t joint); - - /** - * \brief Updates the full trajectory (*this) from the group trajectory - */ - void updateFromGroupTrajectory(const ChompTrajectory& group_trajectory); - - /** - * \brief Gets the index in the full trajectory which was copied to this group trajectory - */ - size_t getFullTrajectoryIndex(size_t i) const; - - /** - * \brief Gets the joint velocities at the given trajectory point - */ - template - void getJointVelocities(size_t traj_point, Eigen::MatrixBase& velocities); - - double getDuration() const; - -private: - void init(); /**< \brief Allocates memory for the trajectory */ - - std::string planning_group_name_; //< Planning group that this trajectory corresponds to, if any - size_t num_points_; //< Number of points in the trajectory - size_t num_joints_; //< Number of joints in each trajectory point - double discretization_; //< Discretization of the trajectory - double duration_; //< Duration of the trajectory - Eigen::MatrixXd trajectory_; //< Storage for the actual trajectory - size_t start_index_; // Start index (inclusive) of trajectory to be optimized (everything before will be ignored) - size_t end_index_; //< End index (inclusive) of trajectory to be optimized (everything after will be ignored) - std::vector full_trajectory_index_; //< If this is a "group" trajectory, the indices from the original traj -}; - -///////////////////////// inline functions follow ////////////////////// - -inline double& ChompTrajectory::operator()(size_t traj_point, size_t joint) -{ - return trajectory_(traj_point, joint); -} - -inline double ChompTrajectory::operator()(size_t traj_point, size_t joint) const -{ - return trajectory_(traj_point, joint); -} - -inline Eigen::MatrixXd::RowXpr ChompTrajectory::getTrajectoryPoint(int traj_point) -{ - return trajectory_.row(traj_point); -} - -inline Eigen::MatrixXd::ColXpr ChompTrajectory::getJointTrajectory(int joint) -{ - return trajectory_.col(joint); -} - -inline size_t ChompTrajectory::getNumPoints() const -{ - return num_points_; -} - -inline size_t ChompTrajectory::getNumFreePoints() const -{ - return (end_index_ - start_index_) + 1; -} - -inline size_t ChompTrajectory::getNumJoints() const -{ - return num_joints_; -} - -inline double ChompTrajectory::getDiscretization() const -{ - return discretization_; -} - -inline void ChompTrajectory::setStartEndIndex(size_t start_index, size_t end_index) -{ - start_index_ = start_index; - end_index_ = end_index; -} - -inline size_t ChompTrajectory::getStartIndex() const -{ - return start_index_; -} - -inline size_t ChompTrajectory::getEndIndex() const -{ - return end_index_; -} - -inline Eigen::MatrixXd& ChompTrajectory::getTrajectory() -{ - return trajectory_; -} - -inline Eigen::Block ChompTrajectory::getFreeTrajectoryBlock() -{ - return trajectory_.block(start_index_, 0, getNumFreePoints(), getNumJoints()); -} - -inline Eigen::Block -ChompTrajectory::getFreeJointTrajectoryBlock(size_t joint) -{ - return trajectory_.block(start_index_, joint, getNumFreePoints(), 1); -} - -inline size_t ChompTrajectory::getFullTrajectoryIndex(size_t i) const -{ - return full_trajectory_index_[i]; -} - -template -void ChompTrajectory::getJointVelocities(size_t traj_point, Eigen::MatrixBase& velocities) -{ - velocities.setZero(); - double inv_time = 1.0 / discretization_; - - for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; ++k) - { - velocities += (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose(); - } -} - -inline double ChompTrajectory::getDuration() const -{ - return duration_; -} -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.hpp new file mode 100644 index 0000000000..08d63b1a92 --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.hpp @@ -0,0 +1,308 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +namespace chomp +{ +/** + * \brief Represents a discretized joint-space trajectory for CHOMP + */ +class ChompTrajectory +{ +public: + /** + * \brief Constructs a trajectory for a given robot model, trajectory duration, and discretization + */ + ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration, double discretization, + const std::string& group_name); + + /** + * \brief Constructs a trajectory for a given robot model, number of trajectory points, and discretization + */ + ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, size_t num_points, double discretization, + const std::string& group_name); + + /** + * \brief Creates a new containing only the joints of interest, and adds padding to the start + * and end if needed, to have enough trajectory points for the differentiation rules + */ + ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, int diff_rule_length); + + ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name, + const trajectory_msgs::msg::JointTrajectory& traj); + + /** + * \brief Destructor + */ + virtual ~ChompTrajectory() = default; + + double& operator()(size_t traj_point, size_t joint); + + double operator()(size_t traj_point, size_t joint) const; + + Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point); + + Eigen::MatrixXd::ColXpr getJointTrajectory(int joint); + + /** + * \brief Gets the number of points in the trajectory + */ + size_t getNumPoints() const; + + /** + * \brief Gets the number of points (that are free to be optimized) in the trajectory + */ + size_t getNumFreePoints() const; + + /** + * \brief Gets the number of joints in each trajectory point + */ + size_t getNumJoints() const; + + /** + * \brief Gets the discretization time interval of the trajectory + */ + double getDiscretization() const; + + /** + * \brief Generates a minimum jerk trajectory from the start index to end index + * + * Only modifies points from start_index_ to end_index_, inclusive. + */ + void fillInMinJerk(); + + /** + * \brief Generates a linearly interpolated trajectory from the start index to end index + * + * Only modifies points from start_index_ to end_index_, inclusive + */ + void fillInLinearInterpolation(); + + /** + * \brief Generates a cubic interpolation of the trajectory from the start index to end index + * + * Only modifies points from start_index_ to end_index_, inclusive + */ + void fillInCubicInterpolation(); + + /** + * \brief Receives the path obtained from a given MotionPlanDetailedResponse res object's trajectory (e.g., trajectory + * produced by OMPL) and puts it into the appropriate trajectory format required for CHOMP + * @param res + */ + bool fillInFromTrajectory(const robot_trajectory::RobotTrajectory& trajectory); + + /** + * \brief This function assigns the given \a source RobotState to the row at index \a chomp_trajectory_point + * + * @param source The source RobotState + * @param chomp_trajectory_point index of the chomp_trajectory's point (row) + * @param group JointModelGroup determining the joints to copy + */ + void assignCHOMPTrajectoryPointFromRobotState(const moveit::core::RobotState& source, size_t chomp_trajectory_point, + const moveit::core::JointModelGroup* group); + + /** + * \brief Sets the start and end index for the modifiable part of the trajectory + * + * (Everything before the start and after the end index is considered fixed) + * The values default to 1 and getNumPoints()-2 + */ + void setStartEndIndex(size_t start_index, size_t end_index); + + /** + * \brief Gets the start index + */ + size_t getStartIndex() const; + + /** + * \brief Gets the end index + */ + size_t getEndIndex() const; + + /** + * \brief Gets the entire trajectory matrix + */ + Eigen::MatrixXd& getTrajectory(); + + /** + * \brief Gets the block of the trajectory which can be optimized + */ + Eigen::Block getFreeTrajectoryBlock(); + + /** + * \brief Gets the block of free (optimizable) trajectory for a single joint + */ + Eigen::Block getFreeJointTrajectoryBlock(size_t joint); + + /** + * \brief Updates the full trajectory (*this) from the group trajectory + */ + void updateFromGroupTrajectory(const ChompTrajectory& group_trajectory); + + /** + * \brief Gets the index in the full trajectory which was copied to this group trajectory + */ + size_t getFullTrajectoryIndex(size_t i) const; + + /** + * \brief Gets the joint velocities at the given trajectory point + */ + template + void getJointVelocities(size_t traj_point, Eigen::MatrixBase& velocities); + + double getDuration() const; + +private: + void init(); /**< \brief Allocates memory for the trajectory */ + + std::string planning_group_name_; //< Planning group that this trajectory corresponds to, if any + size_t num_points_; //< Number of points in the trajectory + size_t num_joints_; //< Number of joints in each trajectory point + double discretization_; //< Discretization of the trajectory + double duration_; //< Duration of the trajectory + Eigen::MatrixXd trajectory_; //< Storage for the actual trajectory + size_t start_index_; // Start index (inclusive) of trajectory to be optimized (everything before will be ignored) + size_t end_index_; //< End index (inclusive) of trajectory to be optimized (everything after will be ignored) + std::vector full_trajectory_index_; //< If this is a "group" trajectory, the indices from the original traj +}; + +///////////////////////// inline functions follow ////////////////////// + +inline double& ChompTrajectory::operator()(size_t traj_point, size_t joint) +{ + return trajectory_(traj_point, joint); +} + +inline double ChompTrajectory::operator()(size_t traj_point, size_t joint) const +{ + return trajectory_(traj_point, joint); +} + +inline Eigen::MatrixXd::RowXpr ChompTrajectory::getTrajectoryPoint(int traj_point) +{ + return trajectory_.row(traj_point); +} + +inline Eigen::MatrixXd::ColXpr ChompTrajectory::getJointTrajectory(int joint) +{ + return trajectory_.col(joint); +} + +inline size_t ChompTrajectory::getNumPoints() const +{ + return num_points_; +} + +inline size_t ChompTrajectory::getNumFreePoints() const +{ + return (end_index_ - start_index_) + 1; +} + +inline size_t ChompTrajectory::getNumJoints() const +{ + return num_joints_; +} + +inline double ChompTrajectory::getDiscretization() const +{ + return discretization_; +} + +inline void ChompTrajectory::setStartEndIndex(size_t start_index, size_t end_index) +{ + start_index_ = start_index; + end_index_ = end_index; +} + +inline size_t ChompTrajectory::getStartIndex() const +{ + return start_index_; +} + +inline size_t ChompTrajectory::getEndIndex() const +{ + return end_index_; +} + +inline Eigen::MatrixXd& ChompTrajectory::getTrajectory() +{ + return trajectory_; +} + +inline Eigen::Block ChompTrajectory::getFreeTrajectoryBlock() +{ + return trajectory_.block(start_index_, 0, getNumFreePoints(), getNumJoints()); +} + +inline Eigen::Block +ChompTrajectory::getFreeJointTrajectoryBlock(size_t joint) +{ + return trajectory_.block(start_index_, joint, getNumFreePoints(), 1); +} + +inline size_t ChompTrajectory::getFullTrajectoryIndex(size_t i) const +{ + return full_trajectory_index_[i]; +} + +template +void ChompTrajectory::getJointVelocities(size_t traj_point, Eigen::MatrixBase& velocities) +{ + velocities.setZero(); + double inv_time = 1.0 / discretization_; + + for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; ++k) + { + velocities += (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose(); + } +} + +inline double ChompTrajectory::getDuration() const +{ + return duration_; +} +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h index 06596fa60c..db0f0ca80b 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,54 +47,5 @@ /* Author: Mrinal Kalakrishnan */ #pragma once - -#include - -#include -#include - -namespace chomp -{ -static const int DIFF_RULE_LENGTH = 7; - -// the differentiation rules (centered at the center) -static const double DIFF_RULES[3][DIFF_RULE_LENGTH] = { - { 0, 0, -2 / 6.0, -3 / 6.0, 6 / 6.0, -1 / 6.0, 0 }, // velocity - { 0, -1 / 12.0, 16 / 12.0, -30 / 12.0, 16 / 12.0, -1 / 12.0, 0 }, // acceleration - { 0, 1 / 12.0, -17 / 12.0, 46 / 12.0, -46 / 12.0, 17 / 12.0, -1 / 12.0 } // jerk -}; - -static inline void robotStateToArray(const moveit::core::RobotState& state, const std::string& planning_group_name, - Eigen::MatrixXd::RowXpr joint_array) -{ - const moveit::core::JointModelGroup* group = state.getJointModelGroup(planning_group_name); - size_t joint_index = 0; - for (const moveit::core::JointModel* jm : group->getActiveJointModels()) - joint_array[joint_index++] = state.getVariablePosition(jm->getFirstVariableIndex()); -} - -// copied from geometry/angles/angles.h -static inline double normalizeAnglePositive(double angle) -{ - return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI); -} - -static inline double normalizeAngle(double angle) -{ - double a = normalizeAnglePositive(angle); - if (a > M_PI) - a -= 2.0 * M_PI; - return a; -} - -static inline double shortestAngularDistance(double start, double end) -{ - double res = normalizeAnglePositive(normalizeAnglePositive(end) - normalizeAnglePositive(start)); - if (res > M_PI) - { - res = -(2.0 * M_PI - res); - } - return normalizeAngle(res); -} - -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.hpp new file mode 100644 index 0000000000..339d3de62b --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_utils.hpp @@ -0,0 +1,88 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan */ + +#pragma once + +#include + +#include +#include + +namespace chomp +{ +static const int DIFF_RULE_LENGTH = 7; + +// the differentiation rules (centered at the center) +static const double DIFF_RULES[3][DIFF_RULE_LENGTH] = { + { 0, 0, -2 / 6.0, -3 / 6.0, 6 / 6.0, -1 / 6.0, 0 }, // velocity + { 0, -1 / 12.0, 16 / 12.0, -30 / 12.0, 16 / 12.0, -1 / 12.0, 0 }, // acceleration + { 0, 1 / 12.0, -17 / 12.0, 46 / 12.0, -46 / 12.0, 17 / 12.0, -1 / 12.0 } // jerk +}; + +static inline void robotStateToArray(const moveit::core::RobotState& state, const std::string& planning_group_name, + Eigen::MatrixXd::RowXpr joint_array) +{ + const moveit::core::JointModelGroup* group = state.getJointModelGroup(planning_group_name); + size_t joint_index = 0; + for (const moveit::core::JointModel* jm : group->getActiveJointModels()) + joint_array[joint_index++] = state.getVariablePosition(jm->getFirstVariableIndex()); +} + +// copied from geometry/angles/angles.h +static inline double normalizeAnglePositive(double angle) +{ + return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI); +} + +static inline double normalizeAngle(double angle) +{ + double a = normalizeAnglePositive(angle); + if (a > M_PI) + a -= 2.0 * M_PI; + return a; +} + +static inline double shortestAngularDistance(double start, double end) +{ + double res = normalizeAnglePositive(normalizeAnglePositive(end) - normalizeAnglePositive(start)); + if (res > M_PI) + { + res = -(2.0 * M_PI - res); + } + return normalizeAngle(res); +} + +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h index 52fe95fbfd..ece45e35d6 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,51 +47,5 @@ /* Author: Mrinal Kalakrishnan */ #pragma once - -#include -#include -#include -#include -#include - -namespace chomp -{ -/** - * \brief Generates samples from a multivariate gaussian distribution - */ -class MultivariateGaussian -{ -public: - template - MultivariateGaussian(const Eigen::MatrixBase& mean, const Eigen::MatrixBase& covariance); - - template - void sample(Eigen::MatrixBase& output); - -private: - Eigen::VectorXd mean_; /**< Mean of the gaussian distribution */ - Eigen::MatrixXd covariance_; /**< Covariance of the gaussian distribution */ - Eigen::MatrixXd covariance_cholesky_; /**< Cholesky decomposition (LL^T) of the covariance */ - - int size_; - std::normal_distribution gaussian_; -}; - -//////////////////////// template function definitions follow ////////////////////////////// - -template -MultivariateGaussian::MultivariateGaussian(const Eigen::MatrixBase& mean, - const Eigen::MatrixBase& covariance) - : mean_(mean), covariance_(covariance), covariance_cholesky_(covariance_.llt().matrixL()), gaussian_(0.0, 1.0) -{ - size_ = mean.rows(); -} - -template -void MultivariateGaussian::sample(Eigen::MatrixBase& output) -{ - for (int i = 0; i < size_; ++i) - output(i) = gaussian_(rsl::rng()); - output = mean_ + covariance_cholesky_ * output; -} -} // namespace chomp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.hpp b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.hpp new file mode 100644 index 0000000000..52fe95fbfd --- /dev/null +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.hpp @@ -0,0 +1,85 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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: Mrinal Kalakrishnan */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace chomp +{ +/** + * \brief Generates samples from a multivariate gaussian distribution + */ +class MultivariateGaussian +{ +public: + template + MultivariateGaussian(const Eigen::MatrixBase& mean, const Eigen::MatrixBase& covariance); + + template + void sample(Eigen::MatrixBase& output); + +private: + Eigen::VectorXd mean_; /**< Mean of the gaussian distribution */ + Eigen::MatrixXd covariance_; /**< Covariance of the gaussian distribution */ + Eigen::MatrixXd covariance_cholesky_; /**< Cholesky decomposition (LL^T) of the covariance */ + + int size_; + std::normal_distribution gaussian_; +}; + +//////////////////////// template function definitions follow ////////////////////////////// + +template +MultivariateGaussian::MultivariateGaussian(const Eigen::MatrixBase& mean, + const Eigen::MatrixBase& covariance) + : mean_(mean), covariance_(covariance), covariance_cholesky_(covariance_.llt().matrixL()), gaussian_(0.0, 1.0) +{ + size_ = mean.rows(); +} + +template +void MultivariateGaussian::sample(Eigen::MatrixBase& output) +{ + for (int i = 0; i < size_; ++i) + output(i) = gaussian_(rsl::rng()); + output = mean_ + covariance_cholesky_ * output; +} +} // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp index ebe8ad2cd1..5b0e5341c7 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_cost.cpp @@ -34,8 +34,8 @@ /* Author: Mrinal Kalakrishnan */ -#include -#include +#include +#include #include diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index f31f48510d..250d8f6ffd 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -34,11 +34,11 @@ /* Author: Mrinal Kalakrishnan */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp index 58dc3aa2eb..3a45a47e5d 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp @@ -34,7 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#include +#include #include diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 9b78d26394..24ffe990b2 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -34,10 +34,10 @@ /* Author: E. Gil Jones */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp index c0dc2abe53..c22618930f 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp @@ -34,7 +34,7 @@ /* Author: Mrinal Kalakrishnan */ -#include +#include namespace chomp { diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index 2f533778c2..1158247890 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,41 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -#include -#include - -namespace ompl_interface -{ -class ModelBasedPlanningContext; - -/** @class ConstrainedGoalSampler - * An interface to the OMPL goal lazy sampler*/ -class ConstrainedGoalSampler : public ompl::base::GoalLazySamples -{ -public: - ConstrainedGoalSampler(const ModelBasedPlanningContext* pc, kinematic_constraints::KinematicConstraintSetPtr ks, - constraint_samplers::ConstraintSamplerPtr cs = constraint_samplers::ConstraintSamplerPtr()); - -private: - bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); - bool stateValidityCallback(ompl::base::State* new_goal, const moveit::core::RobotState* state, - const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, - bool verbose = false) const; - bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, - bool verbose = false) const; - - const ModelBasedPlanningContext* planning_context_; - kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_; - constraint_samplers::ConstraintSamplerPtr constraint_sampler_; - ompl::base::StateSamplerPtr default_sampler_; - moveit::core::RobotState work_state_; - unsigned int invalid_sampled_constraints_; - bool warned_invalid_samples_; - unsigned int verbose_display_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.hpp new file mode 100644 index 0000000000..b45c964601 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.hpp @@ -0,0 +1,75 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace ompl_interface +{ +class ModelBasedPlanningContext; + +/** @class ConstrainedGoalSampler + * An interface to the OMPL goal lazy sampler*/ +class ConstrainedGoalSampler : public ompl::base::GoalLazySamples +{ +public: + ConstrainedGoalSampler(const ModelBasedPlanningContext* pc, kinematic_constraints::KinematicConstraintSetPtr ks, + constraint_samplers::ConstraintSamplerPtr cs = constraint_samplers::ConstraintSamplerPtr()); + +private: + bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); + bool stateValidityCallback(ompl::base::State* new_goal, const moveit::core::RobotState* state, + const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, + bool verbose = false) const; + bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, + bool verbose = false) const; + + const ModelBasedPlanningContext* planning_context_; + kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_; + constraint_samplers::ConstraintSamplerPtr constraint_sampler_; + ompl::base::StateSamplerPtr default_sampler_; + moveit::core::RobotState work_state_; + unsigned int invalid_sampled_constraints_; + bool warned_invalid_samples_; + unsigned int verbose_display_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h index d0f1d4951d..1484dfed90 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,45 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include - -namespace ompl_interface -{ -class ModelBasedPlanningContext; - -/** @class ConstrainedSampler - * This class defines a sampler that tries to find a sample that satisfies the constraints*/ -class ConstrainedSampler : public ompl::base::StateSampler -{ -public: - /** @brief Default constructor - * @param pg The planning group - * @param cs A pointer to a kinematic constraint sampler - */ - ConstrainedSampler(const ModelBasedPlanningContext* pc, constraint_samplers::ConstraintSamplerPtr cs); - - /** @brief Sample a state (uniformly)*/ - void sampleUniform(ompl::base::State* state) override; - - /** @brief Sample a state (uniformly) within a certain distance of another state*/ - void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance) override; - - /** @brief Sample a state using the specified Gaussian*/ - void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev) override; - - double getConstrainedSamplingRate() const; - -private: - bool sampleC(ompl::base::State* state); - - const ModelBasedPlanningContext* planning_context_; - ompl::base::StateSamplerPtr default_; - constraint_samplers::ConstraintSamplerPtr constraint_sampler_; - moveit::core::RobotState work_state_; - unsigned int constrained_success_; - unsigned int constrained_failure_; - double inv_dim_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.hpp new file mode 100644 index 0000000000..c23babe0ab --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_sampler.hpp @@ -0,0 +1,79 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include + +namespace ompl_interface +{ +class ModelBasedPlanningContext; + +/** @class ConstrainedSampler + * This class defines a sampler that tries to find a sample that satisfies the constraints*/ +class ConstrainedSampler : public ompl::base::StateSampler +{ +public: + /** @brief Default constructor + * @param pg The planning group + * @param cs A pointer to a kinematic constraint sampler + */ + ConstrainedSampler(const ModelBasedPlanningContext* pc, constraint_samplers::ConstraintSamplerPtr cs); + + /** @brief Sample a state (uniformly)*/ + void sampleUniform(ompl::base::State* state) override; + + /** @brief Sample a state (uniformly) within a certain distance of another state*/ + void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance) override; + + /** @brief Sample a state using the specified Gaussian*/ + void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev) override; + + double getConstrainedSamplingRate() const; + +private: + bool sampleC(ompl::base::State* state); + + const ModelBasedPlanningContext* planning_context_; + ompl::base::StateSamplerPtr default_; + constraint_samplers::ConstraintSamplerPtr constraint_sampler_; + moveit::core::RobotState work_state_; + unsigned int constrained_success_; + unsigned int constrained_failure_; + double inv_dim_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h index fbd735310a..e72f03a660 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,42 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -namespace ompl_interface -{ -typedef ompl::base::StateStorageWithMetadata > ConstraintApproximationStateStorage; -typedef std::function ConstraintStateStorageOrderFn; - -struct ConstraintApproximation -{ - ConstraintApproximation(const planning_models::RobotModelConstPtr& kinematic_model, const std::string& group, - const std::string& factory, const std::string& serialization, const std::string& filename, - const ompl::base::StateStoragePtr& storage); - ConstraintApproximation(const planning_models::RobotModelConstPtr& kinematic_model, const std::string& group, - const std::string& factory, const moveit_msgs::msg::Constraints& msg, - const std::string& filename, const ompl::base::StateStoragePtr& storage); - - void visualizeDistribution(const std::string& link_name, unsigned int count, - visualization_msgs::msg::MarkerArray& arr) const; - - std::string group_; - std::string factory_; - std::string serialization_; - moveit_msgs::msg::Constraints constraint_msg_; - planning_models::RobotModelConstPtr robot_model_; - kinematic_constraints::KinematicConstraintSetPtr kconstraints_set_; - std::vector space_signature_; - - std::string ompldb_filename_; - ompl::base::StateStoragePtr state_storage_ptr_; - ConstraintApproximationStateStorage* state_storage_; -}; - -MOVEIT_DECLARE_PTR(ConstraintApproximations, std::vector) -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.hpp new file mode 100644 index 0000000000..e656fd04eb --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraint_approximations.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace ompl_interface +{ +typedef ompl::base::StateStorageWithMetadata > ConstraintApproximationStateStorage; +typedef std::function ConstraintStateStorageOrderFn; + +struct ConstraintApproximation +{ + ConstraintApproximation(const planning_models::RobotModelConstPtr& kinematic_model, const std::string& group, + const std::string& factory, const std::string& serialization, const std::string& filename, + const ompl::base::StateStoragePtr& storage); + ConstraintApproximation(const planning_models::RobotModelConstPtr& kinematic_model, const std::string& group, + const std::string& factory, const moveit_msgs::msg::Constraints& msg, + const std::string& filename, const ompl::base::StateStoragePtr& storage); + + void visualizeDistribution(const std::string& link_name, unsigned int count, + visualization_msgs::msg::MarkerArray& arr) const; + + std::string group_; + std::string factory_; + std::string serialization_; + moveit_msgs::msg::Constraints constraint_msg_; + planning_models::RobotModelConstPtr robot_model_; + kinematic_constraints::KinematicConstraintSetPtr kconstraints_set_; + std::vector space_signature_; + + std::string ompldb_filename_; + ompl::base::StateStoragePtr state_storage_ptr_; + ConstraintApproximationStateStorage* state_storage_; +}; + +MOVEIT_DECLARE_PTR(ConstraintApproximations, std::vector) +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index 7893627c11..f349982470 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,169 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace ompl_interface -{ -typedef std::pair, std::map > > - ConstrainedStateMetadata; -typedef ompl::base::StateStorageWithMetadata ConstraintApproximationStateStorage; - -MOVEIT_CLASS_FORWARD(ConstraintApproximation); - -class ConstraintApproximation -{ -public: - ConstraintApproximation(std::string group, std::string state_space_parameterization, bool explicit_motions, - moveit_msgs::msg::Constraints msg, std::string filename, ompl::base::StateStoragePtr storage, - std::size_t milestones = 0); - - virtual ~ConstraintApproximation() - { - } - - const std::string& getName() const - { - return constraint_msg_.name; - } - - ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::msg::Constraints& msg) const; - - InterpolationFunction getInterpolationFunction() const; - - const std::vector& getSpaceSignature() const - { - return space_signature_; - } - - const std::string& getGroup() const - { - return group_; - } - - bool hasExplicitMotions() const - { - return explicit_motions_; - } - - std::size_t getMilestoneCount() const - { - return milestones_; - } - - const std::string& getStateSpaceParameterization() const - { - return state_space_parameterization_; - } - - const moveit_msgs::msg::Constraints& getConstraintsMsg() const - { - return constraint_msg_; - } - - const ompl::base::StateStoragePtr& getStateStorage() const - { - return state_storage_ptr_; - } - - const std::string& getFilename() const - { - return ompldb_filename_; - } - -protected: - std::string group_; - std::string state_space_parameterization_; - bool explicit_motions_; - - moveit_msgs::msg::Constraints constraint_msg_; - - std::vector space_signature_; - - std::string ompldb_filename_; - ompl::base::StateStoragePtr state_storage_ptr_; - ConstraintApproximationStateStorage* state_storage_; - std::size_t milestones_; -}; - -struct ConstraintApproximationConstructionOptions -{ - ConstraintApproximationConstructionOptions() - : samples(0) - , edges_per_sample(0) - , max_edge_length(std::numeric_limits::infinity()) - , explicit_motions(false) - , explicit_points_resolution(0.0) - , max_explicit_points(0) - { - } - - std::string state_space_parameterization; - unsigned int samples; - unsigned int edges_per_sample; - double max_edge_length; - bool explicit_motions; - double explicit_points_resolution; - unsigned int max_explicit_points; -}; - -struct ConstraintApproximationConstructionResults -{ - ConstraintApproximationPtr approx; - std::size_t milestones; - double state_sampling_time; - double state_connection_time; - double sampling_success_rate; -}; - -MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc - -class ConstraintsLibrary -{ -public: - ConstraintsLibrary(ModelBasedPlanningContext* pcontext) : context_(pcontext) - { - } - - void loadConstraintApproximations(const std::string& path); - - void saveConstraintApproximations(const std::string& path); - - ConstraintApproximationConstructionResults - addConstraintApproximation(const moveit_msgs::msg::Constraints& constr_sampling, - const moveit_msgs::msg::Constraints& constr_hard, const std::string& group, - const planning_scene::PlanningSceneConstPtr& scene, - const ConstraintApproximationConstructionOptions& options); - - ConstraintApproximationConstructionResults - addConstraintApproximation(const moveit_msgs::msg::Constraints& constr, const std::string& group, - const planning_scene::PlanningSceneConstPtr& scene, - const ConstraintApproximationConstructionOptions& options); - - void printConstraintApproximations(std::ostream& out = std::cout) const; - void clearConstraintApproximations(); - - void registerConstraintApproximation(const ConstraintApproximationPtr& approx) - { - constraint_approximations_[approx->getName()] = approx; - } - - const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::msg::Constraints& msg) const; - -private: - ompl::base::StateStoragePtr constructConstraintApproximation(ModelBasedPlanningContext* pcontext, - const moveit_msgs::msg::Constraints& constr_sampling, - const moveit_msgs::msg::Constraints& constr_hard, - const ConstraintApproximationConstructionOptions& options, - ConstraintApproximationConstructionResults& result); - - ModelBasedPlanningContext* context_; - std::map constraint_approximations_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.hpp new file mode 100644 index 0000000000..7f831e6354 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.hpp @@ -0,0 +1,203 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace ompl_interface +{ +typedef std::pair, std::map > > + ConstrainedStateMetadata; +typedef ompl::base::StateStorageWithMetadata ConstraintApproximationStateStorage; + +MOVEIT_CLASS_FORWARD(ConstraintApproximation); + +class ConstraintApproximation +{ +public: + ConstraintApproximation(std::string group, std::string state_space_parameterization, bool explicit_motions, + moveit_msgs::msg::Constraints msg, std::string filename, ompl::base::StateStoragePtr storage, + std::size_t milestones = 0); + + virtual ~ConstraintApproximation() + { + } + + const std::string& getName() const + { + return constraint_msg_.name; + } + + ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::msg::Constraints& msg) const; + + InterpolationFunction getInterpolationFunction() const; + + const std::vector& getSpaceSignature() const + { + return space_signature_; + } + + const std::string& getGroup() const + { + return group_; + } + + bool hasExplicitMotions() const + { + return explicit_motions_; + } + + std::size_t getMilestoneCount() const + { + return milestones_; + } + + const std::string& getStateSpaceParameterization() const + { + return state_space_parameterization_; + } + + const moveit_msgs::msg::Constraints& getConstraintsMsg() const + { + return constraint_msg_; + } + + const ompl::base::StateStoragePtr& getStateStorage() const + { + return state_storage_ptr_; + } + + const std::string& getFilename() const + { + return ompldb_filename_; + } + +protected: + std::string group_; + std::string state_space_parameterization_; + bool explicit_motions_; + + moveit_msgs::msg::Constraints constraint_msg_; + + std::vector space_signature_; + + std::string ompldb_filename_; + ompl::base::StateStoragePtr state_storage_ptr_; + ConstraintApproximationStateStorage* state_storage_; + std::size_t milestones_; +}; + +struct ConstraintApproximationConstructionOptions +{ + ConstraintApproximationConstructionOptions() + : samples(0) + , edges_per_sample(0) + , max_edge_length(std::numeric_limits::infinity()) + , explicit_motions(false) + , explicit_points_resolution(0.0) + , max_explicit_points(0) + { + } + + std::string state_space_parameterization; + unsigned int samples; + unsigned int edges_per_sample; + double max_edge_length; + bool explicit_motions; + double explicit_points_resolution; + unsigned int max_explicit_points; +}; + +struct ConstraintApproximationConstructionResults +{ + ConstraintApproximationPtr approx; + std::size_t milestones; + double state_sampling_time; + double state_connection_time; + double sampling_success_rate; +}; + +MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc + +class ConstraintsLibrary +{ +public: + ConstraintsLibrary(ModelBasedPlanningContext* pcontext) : context_(pcontext) + { + } + + void loadConstraintApproximations(const std::string& path); + + void saveConstraintApproximations(const std::string& path); + + ConstraintApproximationConstructionResults + addConstraintApproximation(const moveit_msgs::msg::Constraints& constr_sampling, + const moveit_msgs::msg::Constraints& constr_hard, const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options); + + ConstraintApproximationConstructionResults + addConstraintApproximation(const moveit_msgs::msg::Constraints& constr, const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options); + + void printConstraintApproximations(std::ostream& out = std::cout) const; + void clearConstraintApproximations(); + + void registerConstraintApproximation(const ConstraintApproximationPtr& approx) + { + constraint_approximations_[approx->getName()] = approx; + } + + const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::msg::Constraints& msg) const; + +private: + ompl::base::StateStoragePtr constructConstraintApproximation(ModelBasedPlanningContext* pcontext, + const moveit_msgs::msg::Constraints& constr_sampling, + const moveit_msgs::msg::Constraints& constr_hard, + const ConstraintApproximationConstructionOptions& options, + ConstraintApproximationConstructionResults& result); + + ModelBasedPlanningContext* context_; + std::map constraint_approximations_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h index 62f1529c35..33ba25563b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -34,51 +46,6 @@ /* Author: Ioan Sucan */ -#include - -namespace ompl_interface -{ -/** @class GoalSampleableRegionMux*/ -class GoalSampleableRegionMux : public ompl::base::GoalSampleableRegion -{ -public: - /** @brief Constructor - * @param goals The input set of goals*/ - GoalSampleableRegionMux(const std::vector& goals); - - ~GoalSampleableRegionMux() override - { - } - - /** @brief Sample a goal*/ - void sampleGoal(ompl::base::State* st) const override; - - /** @brief Get the max sample count*/ - unsigned int maxSampleCount() const override; - - /** @brief Query if sampler can find any sample*/ - virtual bool canSample() const; - - /** @brief Query if sampler could find a sample in the future */ - bool couldSample() const override; - - /** @brief Is the goal satisfied for this state (given a distance)*/ - bool isSatisfied(const ompl::base::State* st, double* distance) const override; - - /** @brief Find the distance of this state from the goal*/ - double distanceGoal(const ompl::base::State* st) const override; - - /** @brief If there are any member lazy samplers, start them */ - void startSampling(); - - /** @brief If there are any member lazy samplers, stop them */ - void stopSampling(); - - /** @brief Pretty print goal information*/ - void print(std::ostream& out = std::cout) const override; - -protected: - std::vector goals_; - mutable unsigned int gindex_; -}; -} // namespace ompl_interface +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.hpp new file mode 100644 index 0000000000..9e9592afd3 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/goal_union.hpp @@ -0,0 +1,86 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include + +namespace ompl_interface +{ +/** @class GoalSampleableRegionMux*/ +class GoalSampleableRegionMux : public ompl::base::GoalSampleableRegion +{ +public: + /** @brief Constructor + * @param goals The input set of goals*/ + GoalSampleableRegionMux(const std::vector& goals); + + ~GoalSampleableRegionMux() override + { + } + + /** @brief Sample a goal*/ + void sampleGoal(ompl::base::State* st) const override; + + /** @brief Get the max sample count*/ + unsigned int maxSampleCount() const override; + + /** @brief Query if sampler can find any sample*/ + virtual bool canSample() const; + + /** @brief Query if sampler could find a sample in the future */ + bool couldSample() const override; + + /** @brief Is the goal satisfied for this state (given a distance)*/ + bool isSatisfied(const ompl::base::State* st, double* distance) const override; + + /** @brief Find the distance of this state from the goal*/ + double distanceGoal(const ompl::base::State* st) const override; + + /** @brief If there are any member lazy samplers, start them */ + void startSampling(); + + /** @brief If there are any member lazy samplers, stop them */ + void stopSampling(); + + /** @brief Pretty print goal information*/ + void print(std::ostream& out = std::cout) const override; + +protected: + std::vector goals_; + mutable unsigned int gindex_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h index 93b2cc8bc2..7c4321bd30 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,428 +47,5 @@ /* Author: Jeroen De Maeyer, Boston Cleek */ #pragma once - -#include - -#include - -#include -#include -#include -#include - -namespace ompl_interface -{ -MOVEIT_CLASS_FORWARD(BaseConstraint); -MOVEIT_CLASS_FORWARD(BoxConstraint); -MOVEIT_CLASS_FORWARD(OrientationConstraint); - -/** \brief Represents upper and lower bound on the elements of a vector. - * - * OMPL ConstrainedStateSpace requires a model of the constraints given as: - * f1(joint_values) = 0 - * f2(joint_values) = 0 - * f3(joint_values) = 0 - * ... - * - * So we use a penalty function to convert bounds to an equality constraint. - * If you do need equality constraint, you can represent them by setting the upper bound and lower bound almost equal. - * Or you can use the EqualityPositionConstraint version by setting the name of the constraint to - * "use_equality_constraints". But the latter will ignore bounds on other dimensions. - * **/ -class Bounds -{ -public: - Bounds(); - Bounds(const std::vector& lower, const std::vector& upper); - /** \brief Distance to region inside bounds - * - * Distance of a given value outside the bounds, zero inside the bounds. - * Creates a penalty function that looks like this: - * - * (penalty) ^ - * | \ / - * | \ / - * | \_____/ - * |----------------> (variable to be constrained) - * */ - Eigen::VectorXd penalty(const Eigen::Ref& x) const; - - /** \brief Derivative of the penalty function - * ^ - * | - * | -1-1-1 0 0 0 +1+1+1 - * |------------------------> - * **/ - Eigen::VectorXd derivative(const Eigen::Ref& x) const; - - std::size_t size() const; - -private: - std::vector lower_, upper_; - std::size_t size_; - - friend std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds); -}; - -/** \brief Pretty printing of bounds. **/ -std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds); - -/**************************** - * Base class for constraints - * **************************/ -/** \brief Abstract base class for different types of constraints, implementations of ompl::base::Constraint - * - * To create a constrained state space in OMPL, we need a model of the constraints, that can be written in the form of - * equality constraints F(joint_values) = 0. This class uses `Bounds` defined above, to convert: - * - * lower_bound < scalar value < upper bound - * - * into an equation of the form f(x) = 0. - * - * The 'scalar value' can be the difference between the position or orientation of a link and a target position or - * orientation, or any other error metric that can be calculated using the `moveit::core::RobotModel` and - * `moveit::core::JointModelGroup`. - * */ -class BaseConstraint : public ompl::base::Constraint -{ -public: - /** \brief Construct a BaseConstraint using 3 `num_cons` by default because all constraints currently implemented have - * 3 constraint equations. **/ - BaseConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs, const unsigned int num_cons = 3); - - /** \brief Initialize constraint based on message content. - * - * This is necessary because we cannot call the pure virtual - * parseConstraintsMsg method from the constructor of this class. - * */ - void init(const moveit_msgs::msg::Constraints& constraints); - - /** OMPL's main constraint evaluation function. - * - * OMPL requires you to override at least "function" which represents the constraint F(q) = 0 - * */ - void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; - - /** \brief Jacobian of the constraint function. - * - * Optionally you can also provide dF(q)/dq, the Jacobian of the constraint. - * - * */ - void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; - - /** \brief Wrapper for forward kinematics calculated by MoveIt's Robot State. - * - * TODO(jeroendm) Are these actually const, as the robot state is modified? How come it works? - * Also, output arguments could be significantly more performant, - * but MoveIt's robot state does not support passing Eigen::Ref objects at the moment. - * */ - Eigen::Isometry3d forwardKinematics(const Eigen::Ref& joint_values) const; - - /** \brief Calculate the robot's geometric Jacobian using MoveIt's Robot State. - * - * Ideally I would pass the output argument from OMPL's jacobian function directly, - * but I cannot pass an object of type , Eigen::Ref to MoveIt's - * Jacobian method. - * */ - Eigen::MatrixXd robotGeometricJacobian(const Eigen::Ref& joint_values) const; - - /** \brief Parse bounds on position and orientation parameters from MoveIt's constraint message. - * - * This can be non-trivial given the often complex structure of these messages. - * */ - virtual void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) = 0; - - /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. - * - * In this Position constraints case, it calculates the x, y and z position - * of the end-effector. This error is then converted in generic equality constraints in the implementation of - * `ompl_interface::BaseConstraint::function`. - * - * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore - * the bounds calculation. - * */ - virtual Eigen::VectorXd calcError(const Eigen::Ref& /*x*/) const; - - /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. - * * - * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. - * It does not take into account the derivative of the penalty functions defined in the Bounds class. - * This correction is added in the implementation of of BaseConstraint::jacobian. - * - * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore - * the bounds calculation. - * - * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better - * performance? - * */ - virtual Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& /*x*/) const; - - // the methods below are specifically for debugging and testing - - const std::string& getLinkName() - { - return link_name_; - } - - const Eigen::Vector3d getTargetPosition() - { - return target_position_; - } - - const Eigen::Quaterniond getTargetOrientation() - { - return target_orientation_; - } - -protected: - /** \brief Thread-safe storage of the robot state. - * - * The robot state is modified for kinematic calculations. As an instance of this class is possibly used in multiple - * threads due to OMPL's LazyGoalSampler, we need a separate robot state in every thread. - * */ - TSStateStorage state_storage_; - const moveit::core::JointModelGroup* joint_model_group_; - - // all attributes below can be considered const as soon as the constraint message is parsed - // but I (jeroendm) do not know how to elegantly express this in C++ - // parsing the constraints message and passing all this data members separately to the constructor - // is a solution, but it adds complexity outside this class, which is also not ideal. - - /** \brief Robot link the constraints are applied to. */ - std::string link_name_; - - /** \brief Upper and lower bounds on constrained variables. */ - Bounds bounds_; - - /** \brief target for equality constraints, nominal value for inequality constraints. */ - Eigen::Vector3d target_position_; - - /** \brief target for equality constraints, nominal value for inequality constraints. */ - Eigen::Quaterniond target_orientation_; - -public: - // Macro for classes containing fixed size eigen vectors that are dynamically allocated when used. - // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/****************************************** - * Box constraint - * ****************************************/ -/** \brief Box shaped position constraints - * - * Reads bounds on x, y and z position from a position constraint - * at constraint_region.primitives[0].dimensions. - * Where the primitive has to be of type `shape_msgs/SolidPrimitive.BOX`. - * - * These bounds are applied around the nominal position and orientation - * of the box. - * */ -class BoxConstraint : public BaseConstraint -{ -public: - BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs); - - /** \brief Parse bounds on position parameters from MoveIt's constraint message. - * - * This can be non-trivial given the often complex structure of these messages. - * */ - void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; - - /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. - * - * In this Position constraints case, it calculates the x, y and z position - * of the end-effector. This error is then converted in generic equality constraints in the implementation of - * `ompl_interface::BaseConstraint::function`. - * - * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore - * the bounds calculation. - * */ - Eigen::VectorXd calcError(const Eigen::Ref& x) const override; - - /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. - * * - * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. - * It does not take into account the derivative of the penalty functions defined in the Bounds class. - * This correction is added in the implementation of of BaseConstraint::jacobian. - * - * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore - * the bounds calculation. - * - * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better - * performance? - * */ - Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; -}; - -/****************************************** - * Equality Position Constraints - * ****************************************/ -/** \brief Equality constraints on a link's position. - * - * When you set the name of a constraint to 'use_equality_constraints', all constraints with a dimension lower than - * `EQUALITY_CONSTRAINT_THRESHOLD` will be modelled as equality constraints. - * - * The dimension value for the others are ignored. For example, a box with dimensions [1.0, 1e-5, 1.0] - * will result in equality constraints on the y-position, and no constraints on the x or z-position. - * - * TODO(jeroendm) We could make this a base class `EqualityConstraints` with a specialization for position and orientation - * constraints in the future. But the direct overriding of `function` and `jacobian` is probably more performant. - * */ -class EqualityPositionConstraint : public BaseConstraint -{ -public: - EqualityPositionConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs); - - /** \brief Parse bounds on position parameters from MoveIt's constraint message. - * - * This can be non-trivial given the often complex structure of these messages. - * */ - void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; - - void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; - - void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; - -private: - /** \brief Position bounds under this threshold are interpreted as equality constraints, the others as unbounded. - * - * WARNING: Below follows the explanation of an ugly hack. Ideally, the user could specify equality constraints by - * setting the constraint dimension to zero. However, this would result in a constraint region primitive with a zero - * dimension in MoveIt, which the planner can (almost) never satisfy. Therefore we use a threshold value, below which - * the constraint is interpreted as an equality constraint. This threshold value is not used in the planning algorithm itself! - * - * This threshold value should be larger than the tolerance of the constraints specified in OMPL - * (ompl::magic::CONSTRAINT_PROJECTION_TOLERANCE = 1e-4). - * - * This is necessary because the constraints are also checked by MoveIt in the StateValidity checker. If this check - * would use a stricter tolerance than was used to satisfy the constraints in OMPL, all states would be invalid. - * Therefore the dimension of an equality constraint specified in the constraint message should be at least equal the - * the tolerance used by OMPL to project onto the constraints. To avoid checking for exact equality on floats, the - * threshold is made a bit larger. - * - * EQUALITY_CONSTRAINT_THRESHOLD > tolerance in constraint message > OMPL projection tolerance - * - * That's why the value is 1e-3 > 1e-4. - * Now the user can specify any value between 1e-3 and 1e-4 to specify an equality constraint. - * **/ - static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; - - /** \brief Bool vector indicating which dimensions are constrained. **/ - std::vector is_dim_constrained_; -}; - -/****************************************** - * Orientation constraints - * ****************************************/ -/** \brief Orientation constraints parameterized using exponential coordinates. - * - * An orientation constraint is modeled as a deviation from a target orientation. - * The deviation is represented using exponential coordinates. A three element vector represents the rotation axis - * multiplied with the angle in radians around this axis. - * - * R_error = R_end_effector ^ (-1) * R_target - * R_error -> rotation angle and axis (using Eigen3) - * error = angle * axis (vector with three elements) - * - * And then the constraints can be written as - * - * - absolute_x_axis_tolerance < error[0] < absolute_x_axis_tolerance - * - absolute_y_axis_tolerance < error[1] < absolute_y_axis_tolerance - * - absolute_z_axis_tolerance < error[2] < absolute_z_axis_tolerance - * - * **IMPORTANT** It is NOT how orientation error is handled in the default MoveIt constraint samplers, where XYZ - * intrinsic euler angles are used. Using exponential coordinates is analog to how orientation error is calculated in - * the TrajOpt motion planner. - * - * */ -class OrientationConstraint : public BaseConstraint -{ -public: - OrientationConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, - const unsigned int num_dofs) - : BaseConstraint(robot_model, group, num_dofs) - { - } - - /** \brief Parse bounds on orientation parameters from MoveIt's constraint message. - * - * This can be non-trivial given the often complex structure of these messages. - * */ - void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; - - /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. - * - * In this orientation constraints case, it calculates the orientation - * of the end-effector. This error is then converted in generic equality constraints in the implementation of - * `ompl_interface::BaseConstraint::function`. - * - * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore - * the bounds calculation. - * */ - Eigen::VectorXd calcError(const Eigen::Ref& x) const override; - - /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. - * * - * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. - * It does not take into account the derivative of the penalty functions defined in the Bounds class. - * This correction is added in the implementation of of BaseConstraint::jacobian. - * - * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore - * the bounds calculation. - * - * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better - * performance? - * */ - Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; -}; - -/** \brief Extract position constraints from the MoveIt message. - * - * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. - * The dimensions of the box are the bounds on the deviation of the link origin from - * the target pose, given in constraint_regions.primitive_poses[0]. - * */ -Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con); - -/** \brief Extract orientation constraints from the MoveIt message - * - * These bounds are assumed to be centered around the target orientation / desired orientation - * given in the "orientation" field in the message. - * These bounds represent orientation error between the desired orientation and the current orientation of the - * end-effector. - * - * The "absolute_x_axis_tolerance", "absolute_y_axis_tolerance" and "absolute_z_axis_tolerance" are interpreted as - * the width of the tolerance regions around the target orientation, represented using exponential coordinates. - * - * */ -Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con); - -/** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/ -ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelConstPtr& robot_model, - const std::string& group, - const moveit_msgs::msg::Constraints& constraints); - -/** \brief Return a matrix to convert angular velocity to angle-axis velocity - * Based on: - * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf - * Eq. 2.107 - * */ -inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref& axis) -{ - double t{ std::abs(angle) }; - Eigen::Matrix3d r_skew; - r_skew << 0, -axis[2], axis[1], axis[2], 0, -axis[0], -axis[1], axis[0], 0; - r_skew *= angle; - - double c; - c = (1 - 0.5 * t * std::sin(t) / (1 - std::cos(t))); - - return Eigen::Matrix3d::Identity() - 0.5 * r_skew + (r_skew * r_skew / (t * t)) * c; -} - -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.hpp new file mode 100644 index 0000000000..7b240a80de --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.hpp @@ -0,0 +1,462 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * 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 KU Leuven 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: Jeroen De Maeyer, Boston Cleek */ + +#pragma once + +#include + +#include + +#include +#include +#include +#include + +namespace ompl_interface +{ +MOVEIT_CLASS_FORWARD(BaseConstraint); +MOVEIT_CLASS_FORWARD(BoxConstraint); +MOVEIT_CLASS_FORWARD(OrientationConstraint); + +/** \brief Represents upper and lower bound on the elements of a vector. + * + * OMPL ConstrainedStateSpace requires a model of the constraints given as: + * f1(joint_values) = 0 + * f2(joint_values) = 0 + * f3(joint_values) = 0 + * ... + * + * So we use a penalty function to convert bounds to an equality constraint. + * If you do need equality constraint, you can represent them by setting the upper bound and lower bound almost equal. + * Or you can use the EqualityPositionConstraint version by setting the name of the constraint to + * "use_equality_constraints". But the latter will ignore bounds on other dimensions. + * **/ +class Bounds +{ +public: + Bounds(); + Bounds(const std::vector& lower, const std::vector& upper); + /** \brief Distance to region inside bounds + * + * Distance of a given value outside the bounds, zero inside the bounds. + * Creates a penalty function that looks like this: + * + * (penalty) ^ + * | \ / + * | \ / + * | \_____/ + * |----------------> (variable to be constrained) + * */ + Eigen::VectorXd penalty(const Eigen::Ref& x) const; + + /** \brief Derivative of the penalty function + * ^ + * | + * | -1-1-1 0 0 0 +1+1+1 + * |------------------------> + * **/ + Eigen::VectorXd derivative(const Eigen::Ref& x) const; + + std::size_t size() const; + +private: + std::vector lower_, upper_; + std::size_t size_; + + friend std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds); +}; + +/** \brief Pretty printing of bounds. **/ +std::ostream& operator<<(std::ostream& os, const ompl_interface::Bounds& bounds); + +/**************************** + * Base class for constraints + * **************************/ +/** \brief Abstract base class for different types of constraints, implementations of ompl::base::Constraint + * + * To create a constrained state space in OMPL, we need a model of the constraints, that can be written in the form of + * equality constraints F(joint_values) = 0. This class uses `Bounds` defined above, to convert: + * + * lower_bound < scalar value < upper bound + * + * into an equation of the form f(x) = 0. + * + * The 'scalar value' can be the difference between the position or orientation of a link and a target position or + * orientation, or any other error metric that can be calculated using the `moveit::core::RobotModel` and + * `moveit::core::JointModelGroup`. + * */ +class BaseConstraint : public ompl::base::Constraint +{ +public: + /** \brief Construct a BaseConstraint using 3 `num_cons` by default because all constraints currently implemented have + * 3 constraint equations. **/ + BaseConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs, const unsigned int num_cons = 3); + + /** \brief Initialize constraint based on message content. + * + * This is necessary because we cannot call the pure virtual + * parseConstraintsMsg method from the constructor of this class. + * */ + void init(const moveit_msgs::msg::Constraints& constraints); + + /** OMPL's main constraint evaluation function. + * + * OMPL requires you to override at least "function" which represents the constraint F(q) = 0 + * */ + void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + + /** \brief Jacobian of the constraint function. + * + * Optionally you can also provide dF(q)/dq, the Jacobian of the constraint. + * + * */ + void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + + /** \brief Wrapper for forward kinematics calculated by MoveIt's Robot State. + * + * TODO(jeroendm) Are these actually const, as the robot state is modified? How come it works? + * Also, output arguments could be significantly more performant, + * but MoveIt's robot state does not support passing Eigen::Ref objects at the moment. + * */ + Eigen::Isometry3d forwardKinematics(const Eigen::Ref& joint_values) const; + + /** \brief Calculate the robot's geometric Jacobian using MoveIt's Robot State. + * + * Ideally I would pass the output argument from OMPL's jacobian function directly, + * but I cannot pass an object of type , Eigen::Ref to MoveIt's + * Jacobian method. + * */ + Eigen::MatrixXd robotGeometricJacobian(const Eigen::Ref& joint_values) const; + + /** \brief Parse bounds on position and orientation parameters from MoveIt's constraint message. + * + * This can be non-trivial given the often complex structure of these messages. + * */ + virtual void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) = 0; + + /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. + * + * In this Position constraints case, it calculates the x, y and z position + * of the end-effector. This error is then converted in generic equality constraints in the implementation of + * `ompl_interface::BaseConstraint::function`. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore + * the bounds calculation. + * */ + virtual Eigen::VectorXd calcError(const Eigen::Ref& /*x*/) const; + + /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. + * * + * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. + * It does not take into account the derivative of the penalty functions defined in the Bounds class. + * This correction is added in the implementation of of BaseConstraint::jacobian. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore + * the bounds calculation. + * + * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better + * performance? + * */ + virtual Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& /*x*/) const; + + // the methods below are specifically for debugging and testing + + const std::string& getLinkName() + { + return link_name_; + } + + const Eigen::Vector3d getTargetPosition() + { + return target_position_; + } + + const Eigen::Quaterniond getTargetOrientation() + { + return target_orientation_; + } + +protected: + /** \brief Thread-safe storage of the robot state. + * + * The robot state is modified for kinematic calculations. As an instance of this class is possibly used in multiple + * threads due to OMPL's LazyGoalSampler, we need a separate robot state in every thread. + * */ + TSStateStorage state_storage_; + const moveit::core::JointModelGroup* joint_model_group_; + + // all attributes below can be considered const as soon as the constraint message is parsed + // but I (jeroendm) do not know how to elegantly express this in C++ + // parsing the constraints message and passing all this data members separately to the constructor + // is a solution, but it adds complexity outside this class, which is also not ideal. + + /** \brief Robot link the constraints are applied to. */ + std::string link_name_; + + /** \brief Upper and lower bounds on constrained variables. */ + Bounds bounds_; + + /** \brief target for equality constraints, nominal value for inequality constraints. */ + Eigen::Vector3d target_position_; + + /** \brief target for equality constraints, nominal value for inequality constraints. */ + Eigen::Quaterniond target_orientation_; + +public: + // Macro for classes containing fixed size eigen vectors that are dynamically allocated when used. + // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/****************************************** + * Box constraint + * ****************************************/ +/** \brief Box shaped position constraints + * + * Reads bounds on x, y and z position from a position constraint + * at constraint_region.primitives[0].dimensions. + * Where the primitive has to be of type `shape_msgs/SolidPrimitive.BOX`. + * + * These bounds are applied around the nominal position and orientation + * of the box. + * */ +class BoxConstraint : public BaseConstraint +{ +public: + BoxConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs); + + /** \brief Parse bounds on position parameters from MoveIt's constraint message. + * + * This can be non-trivial given the often complex structure of these messages. + * */ + void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; + + /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. + * + * In this Position constraints case, it calculates the x, y and z position + * of the end-effector. This error is then converted in generic equality constraints in the implementation of + * `ompl_interface::BaseConstraint::function`. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore + * the bounds calculation. + * */ + Eigen::VectorXd calcError(const Eigen::Ref& x) const override; + + /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. + * * + * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. + * It does not take into account the derivative of the penalty functions defined in the Bounds class. + * This correction is added in the implementation of of BaseConstraint::jacobian. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore + * the bounds calculation. + * + * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better + * performance? + * */ + Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; +}; + +/****************************************** + * Equality Position Constraints + * ****************************************/ +/** \brief Equality constraints on a link's position. + * + * When you set the name of a constraint to 'use_equality_constraints', all constraints with a dimension lower than + * `EQUALITY_CONSTRAINT_THRESHOLD` will be modelled as equality constraints. + * + * The dimension value for the others are ignored. For example, a box with dimensions [1.0, 1e-5, 1.0] + * will result in equality constraints on the y-position, and no constraints on the x or z-position. + * + * TODO(jeroendm) We could make this a base class `EqualityConstraints` with a specialization for position and orientation + * constraints in the future. But the direct overriding of `function` and `jacobian` is probably more performant. + * */ +class EqualityPositionConstraint : public BaseConstraint +{ +public: + EqualityPositionConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs); + + /** \brief Parse bounds on position parameters from MoveIt's constraint message. + * + * This can be non-trivial given the often complex structure of these messages. + * */ + void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; + + void function(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + + void jacobian(const Eigen::Ref& joint_values, Eigen::Ref out) const override; + +private: + /** \brief Position bounds under this threshold are interpreted as equality constraints, the others as unbounded. + * + * WARNING: Below follows the explanation of an ugly hack. Ideally, the user could specify equality constraints by + * setting the constraint dimension to zero. However, this would result in a constraint region primitive with a zero + * dimension in MoveIt, which the planner can (almost) never satisfy. Therefore we use a threshold value, below which + * the constraint is interpreted as an equality constraint. This threshold value is not used in the planning algorithm itself! + * + * This threshold value should be larger than the tolerance of the constraints specified in OMPL + * (ompl::magic::CONSTRAINT_PROJECTION_TOLERANCE = 1e-4). + * + * This is necessary because the constraints are also checked by MoveIt in the StateValidity checker. If this check + * would use a stricter tolerance than was used to satisfy the constraints in OMPL, all states would be invalid. + * Therefore the dimension of an equality constraint specified in the constraint message should be at least equal the + * the tolerance used by OMPL to project onto the constraints. To avoid checking for exact equality on floats, the + * threshold is made a bit larger. + * + * EQUALITY_CONSTRAINT_THRESHOLD > tolerance in constraint message > OMPL projection tolerance + * + * That's why the value is 1e-3 > 1e-4. + * Now the user can specify any value between 1e-3 and 1e-4 to specify an equality constraint. + * **/ + static constexpr double EQUALITY_CONSTRAINT_THRESHOLD{ 0.001 }; + + /** \brief Bool vector indicating which dimensions are constrained. **/ + std::vector is_dim_constrained_; +}; + +/****************************************** + * Orientation constraints + * ****************************************/ +/** \brief Orientation constraints parameterized using exponential coordinates. + * + * An orientation constraint is modeled as a deviation from a target orientation. + * The deviation is represented using exponential coordinates. A three element vector represents the rotation axis + * multiplied with the angle in radians around this axis. + * + * R_error = R_end_effector ^ (-1) * R_target + * R_error -> rotation angle and axis (using Eigen3) + * error = angle * axis (vector with three elements) + * + * And then the constraints can be written as + * + * - absolute_x_axis_tolerance < error[0] < absolute_x_axis_tolerance + * - absolute_y_axis_tolerance < error[1] < absolute_y_axis_tolerance + * - absolute_z_axis_tolerance < error[2] < absolute_z_axis_tolerance + * + * **IMPORTANT** It is NOT how orientation error is handled in the default MoveIt constraint samplers, where XYZ + * intrinsic euler angles are used. Using exponential coordinates is analog to how orientation error is calculated in + * the TrajOpt motion planner. + * + * */ +class OrientationConstraint : public BaseConstraint +{ +public: + OrientationConstraint(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group, + const unsigned int num_dofs) + : BaseConstraint(robot_model, group, num_dofs) + { + } + + /** \brief Parse bounds on orientation parameters from MoveIt's constraint message. + * + * This can be non-trivial given the often complex structure of these messages. + * */ + void parseConstraintMsg(const moveit_msgs::msg::Constraints& constraints) override; + + /** \brief For inequality constraints: calculate the value of the parameter that is being constrained by the bounds. + * + * In this orientation constraints case, it calculates the orientation + * of the end-effector. This error is then converted in generic equality constraints in the implementation of + * `ompl_interface::BaseConstraint::function`. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::function directly and ignore + * the bounds calculation. + * */ + Eigen::VectorXd calcError(const Eigen::Ref& x) const override; + + /** \brief For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. + * * + * This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. + * It does not take into account the derivative of the penalty functions defined in the Bounds class. + * This correction is added in the implementation of of BaseConstraint::jacobian. + * + * This method can be bypassed if you want to override `ompl_interface::BaseConstraint::jacobian directly and ignore + * the bounds calculation. + * + * TODO(jeroendm), Maybe also use an output argument as in `ompl::base::Constraint::jacobian(x, out)` for better + * performance? + * */ + Eigen::MatrixXd calcErrorJacobian(const Eigen::Ref& x) const override; +}; + +/** \brief Extract position constraints from the MoveIt message. + * + * Assumes there is a single primitive of type `shape_msgs/SolidPrimitive.BOX`. + * The dimensions of the box are the bounds on the deviation of the link origin from + * the target pose, given in constraint_regions.primitive_poses[0]. + * */ +Bounds positionConstraintMsgToBoundVector(const moveit_msgs::msg::PositionConstraint& pos_con); + +/** \brief Extract orientation constraints from the MoveIt message + * + * These bounds are assumed to be centered around the target orientation / desired orientation + * given in the "orientation" field in the message. + * These bounds represent orientation error between the desired orientation and the current orientation of the + * end-effector. + * + * The "absolute_x_axis_tolerance", "absolute_y_axis_tolerance" and "absolute_z_axis_tolerance" are interpreted as + * the width of the tolerance regions around the target orientation, represented using exponential coordinates. + * + * */ +Bounds orientationConstraintMsgToBoundVector(const moveit_msgs::msg::OrientationConstraint& ori_con); + +/** \brief Factory to create constraints based on what is in the MoveIt constraint message. **/ +ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group, + const moveit_msgs::msg::Constraints& constraints); + +/** \brief Return a matrix to convert angular velocity to angle-axis velocity + * Based on: + * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf + * Eq. 2.107 + * */ +inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref& axis) +{ + double t{ std::abs(angle) }; + Eigen::Matrix3d r_skew; + r_skew << 0, -axis[2], axis[1], axis[2], 0, -axis[0], -axis[1], axis[0], 0; + r_skew *= angle; + + double c; + c = (1 - 0.5 * t * std::sin(t) / (1 - std::cos(t))); + + return Eigen::Matrix3d::Identity() - 0.5 * r_skew + (r_skew * r_skew / (t * t)) * c; +} + +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h index bf2c987f43..b53cb107c8 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,46 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -typedef Eigen::Ref OMPLProjection; - -namespace ompl_interface -{ -class ModelBasedPlanningContext; - -/** @class ProjectionEvaluatorLinkPose - @brief */ -class ProjectionEvaluatorLinkPose : public ompl::base::ProjectionEvaluator -{ -public: - ProjectionEvaluatorLinkPose(const ModelBasedPlanningContext* pc, const std::string& link); - - unsigned int getDimension() const override; - void defaultCellSizes() override; - void project(const ompl::base::State* state, OMPLProjection projection) const override; - -private: - const ModelBasedPlanningContext* planning_context_; - const moveit::core::LinkModel* link_; - TSStateStorage tss_; -}; - -/** @class ProjectionEvaluatorJointValue - @brief */ -class ProjectionEvaluatorJointValue : public ompl::base::ProjectionEvaluator -{ -public: - ProjectionEvaluatorJointValue(const ModelBasedPlanningContext* pc, std::vector variables); - - unsigned int getDimension() const override; - void defaultCellSizes() override; - void project(const ompl::base::State* state, OMPLProjection projection) const override; - -private: - std::vector variables_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.hpp new file mode 100644 index 0000000000..2a0c6fa6a2 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/projection_evaluators.hpp @@ -0,0 +1,80 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include + +typedef Eigen::Ref OMPLProjection; + +namespace ompl_interface +{ +class ModelBasedPlanningContext; + +/** @class ProjectionEvaluatorLinkPose + @brief */ +class ProjectionEvaluatorLinkPose : public ompl::base::ProjectionEvaluator +{ +public: + ProjectionEvaluatorLinkPose(const ModelBasedPlanningContext* pc, const std::string& link); + + unsigned int getDimension() const override; + void defaultCellSizes() override; + void project(const ompl::base::State* state, OMPLProjection projection) const override; + +private: + const ModelBasedPlanningContext* planning_context_; + const moveit::core::LinkModel* link_; + TSStateStorage tss_; +}; + +/** @class ProjectionEvaluatorJointValue + @brief */ +class ProjectionEvaluatorJointValue : public ompl::base::ProjectionEvaluator +{ +public: + ProjectionEvaluatorJointValue(const ModelBasedPlanningContext* pc, std::vector variables); + + unsigned int getDimension() const override; + void defaultCellSizes() override; + void project(const ompl::base::State* state, OMPLProjection projection) const override; + +private: + std::vector variables_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index a3fead6927..83383b065a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -46,112 +58,5 @@ * **/ #pragma once - -#include -#include -#include - -namespace ompl_interface -{ -class ModelBasedPlanningContext; - -/** @class StateValidityChecker - @brief An interface for a OMPL state validity checker*/ -class StateValidityChecker : public ompl::base::StateValidityChecker -{ -public: - StateValidityChecker(const ModelBasedPlanningContext* planning_context); - - bool isValid(const ompl::base::State* state) const override - { - return isValid(state, verbose_); - } - - bool isValid(const ompl::base::State* state, double& dist) const override - { - return isValid(state, dist, verbose_); - } - - bool isValid(const ompl::base::State* state, double& dist, ompl::base::State* /*validState*/, - bool& /*validStateAvailable*/) const override - { - return isValid(state, dist, verbose_); - } - - virtual bool isValid(const ompl::base::State* state, bool verbose) const; - virtual bool isValid(const ompl::base::State* state, double& dist, bool verbose) const; - - virtual double cost(const ompl::base::State* state) const; - double clearance(const ompl::base::State* state) const override; - - void setVerbose(bool flag); - -protected: - const ModelBasedPlanningContext* planning_context_; - std::string group_name_; - TSStateStorage tss_; - collision_detection::CollisionRequest collision_request_simple_; - collision_detection::CollisionRequest collision_request_with_distance_; - collision_detection::CollisionRequest collision_request_simple_verbose_; - collision_detection::CollisionRequest collision_request_with_distance_verbose_; - - collision_detection::CollisionRequest collision_request_with_cost_; - bool verbose_; -}; - -/** \brief A StateValidityChecker that can handle states of type `ompl::base::ConstraintStateSpace::StateType`. - * - * We cannot not just cast the states and pass them to the isValid version of the parent class, because inside the - * state-validity checker, the line: - * - * planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); - * - * requires the state type to be of the constrained state space, while: - * - * state->as()->isValidityKnown() - * - * requires accessing the underlying state by calling `getState()` on the constrained state space state. - * Therefore this class implements specific versions of the isValid methods. - * - * We still check the path constraints, because not all states sampled by the constrained state space - * satisfy the constraints unfortunately. This is a complicated issue. For more details see: - * https://github.com/moveit/moveit/issues/2092#issuecomment-669911722. - **/ -class ConstrainedPlanningStateValidityChecker : public StateValidityChecker -{ -public: - using StateValidityChecker::isValid; - -public: - ConstrainedPlanningStateValidityChecker(const ModelBasedPlanningContext* planning_context) - : StateValidityChecker(planning_context) - { - } - - /** \brief Check validity for states of type ompl::base::ConstrainedStateSpace - * - * This state type is special in that it "wraps" around a normal state, - * which can be accessed by the getState() method. In this class we assume that this state, - * is of type `ompl_interface::ConstrainedPlanningStateSpace`, which inherits from - * `ompl_interface::ModelBasedStateSpace`. - * - * (For the actual implementation of this, look at the ompl::base::WrapperStateSpace.) - * - * Code sample that can be used to check all the assumptions: - * - * #include - * #include - * - * // the code below should be pasted at the top of the isValid method - * auto css = dynamic_cast(wrapped_state); - * assert(css != nullptr); - * auto cpss = dynamic_cast(planning_context_->getOMPLStateSpace().get()); - * assert(cpss != nullptr); - * auto cssi = dynamic_cast(si_); - * assert(cssi != nullptr); - * - **/ - bool isValid(const ompl::base::State* wrapped_state, bool verbose) const override; - bool isValid(const ompl::base::State* wrapped_state, double& dist, bool verbose) const override; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.hpp new file mode 100644 index 0000000000..4124c92665 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.hpp @@ -0,0 +1,157 @@ +/********************************************************************* + * 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, Jeroen De Maeyer */ + +/** A state validity checker checks: + * + * - Bounds (joint limits). + * - Collision. + * - Kinematic path constraints. + * - Generic user-specified feasibility using the `isStateFeasible` of the planning scene. + * + * IMPORTANT: Although the isValid method takes the state as `const ompl::base::State* state`, + * it uses const_cast to modify the validity of the state with `markInvalid` and `markValid` for caching. + * **/ + +#pragma once + +#include +#include +#include + +namespace ompl_interface +{ +class ModelBasedPlanningContext; + +/** @class StateValidityChecker + @brief An interface for a OMPL state validity checker*/ +class StateValidityChecker : public ompl::base::StateValidityChecker +{ +public: + StateValidityChecker(const ModelBasedPlanningContext* planning_context); + + bool isValid(const ompl::base::State* state) const override + { + return isValid(state, verbose_); + } + + bool isValid(const ompl::base::State* state, double& dist) const override + { + return isValid(state, dist, verbose_); + } + + bool isValid(const ompl::base::State* state, double& dist, ompl::base::State* /*validState*/, + bool& /*validStateAvailable*/) const override + { + return isValid(state, dist, verbose_); + } + + virtual bool isValid(const ompl::base::State* state, bool verbose) const; + virtual bool isValid(const ompl::base::State* state, double& dist, bool verbose) const; + + virtual double cost(const ompl::base::State* state) const; + double clearance(const ompl::base::State* state) const override; + + void setVerbose(bool flag); + +protected: + const ModelBasedPlanningContext* planning_context_; + std::string group_name_; + TSStateStorage tss_; + collision_detection::CollisionRequest collision_request_simple_; + collision_detection::CollisionRequest collision_request_with_distance_; + collision_detection::CollisionRequest collision_request_simple_verbose_; + collision_detection::CollisionRequest collision_request_with_distance_verbose_; + + collision_detection::CollisionRequest collision_request_with_cost_; + bool verbose_; +}; + +/** \brief A StateValidityChecker that can handle states of type `ompl::base::ConstraintStateSpace::StateType`. + * + * We cannot not just cast the states and pass them to the isValid version of the parent class, because inside the + * state-validity checker, the line: + * + * planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state); + * + * requires the state type to be of the constrained state space, while: + * + * state->as()->isValidityKnown() + * + * requires accessing the underlying state by calling `getState()` on the constrained state space state. + * Therefore this class implements specific versions of the isValid methods. + * + * We still check the path constraints, because not all states sampled by the constrained state space + * satisfy the constraints unfortunately. This is a complicated issue. For more details see: + * https://github.com/moveit/moveit/issues/2092#issuecomment-669911722. + **/ +class ConstrainedPlanningStateValidityChecker : public StateValidityChecker +{ +public: + using StateValidityChecker::isValid; + +public: + ConstrainedPlanningStateValidityChecker(const ModelBasedPlanningContext* planning_context) + : StateValidityChecker(planning_context) + { + } + + /** \brief Check validity for states of type ompl::base::ConstrainedStateSpace + * + * This state type is special in that it "wraps" around a normal state, + * which can be accessed by the getState() method. In this class we assume that this state, + * is of type `ompl_interface::ConstrainedPlanningStateSpace`, which inherits from + * `ompl_interface::ModelBasedStateSpace`. + * + * (For the actual implementation of this, look at the ompl::base::WrapperStateSpace.) + * + * Code sample that can be used to check all the assumptions: + * + * #include + * #include + * + * // the code below should be pasted at the top of the isValid method + * auto css = dynamic_cast(wrapped_state); + * assert(css != nullptr); + * auto cpss = dynamic_cast(planning_context_->getOMPLStateSpace().get()); + * assert(cpss != nullptr); + * auto cssi = dynamic_cast(si_); + * assert(cssi != nullptr); + * + **/ + bool isValid(const ompl::base::State* wrapped_state, bool verbose) const override; + bool isValid(const ompl::base::State* wrapped_state, double& dist, bool verbose) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h index ab6d38fc66..31621f821d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,25 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace ompl_interface -{ -class TSStateStorage -{ -public: - TSStateStorage(const moveit::core::RobotModelPtr& robot_model); - TSStateStorage(const moveit::core::RobotState& start_state); - ~TSStateStorage(); - - moveit::core::RobotState* getStateStorage() const; - -private: - moveit::core::RobotState start_state_; - mutable std::map thread_states_; - mutable std::mutex lock_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.hpp new file mode 100644 index 0000000000..6e5a5e8b81 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/threadsafe_state_storage.hpp @@ -0,0 +1,59 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include + +namespace ompl_interface +{ +class TSStateStorage +{ +public: + TSStateStorage(const moveit::core::RobotModelPtr& robot_model); + TSStateStorage(const moveit::core::RobotState& start_state); + ~TSStateStorage(); + + moveit::core::RobotState* getStateStorage() const; + +private: + moveit::core::RobotState start_state_; + mutable std::map thread_states_; + mutable std::mutex lock_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h index 8790fdadcf..43770b293a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,422 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace ompl_interface -{ -namespace ob = ompl::base; -namespace og = ompl::geometric; -namespace ot = ompl::tools; - -MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext); // Defines ModelBasedPlanningContextPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc - -struct ModelBasedPlanningContextSpecification; -typedef std::function - ConfiguredPlannerAllocator; -typedef std::function ConfiguredPlannerSelector; - -struct ModelBasedPlanningContextSpecification -{ - std::map config_; - ConfiguredPlannerSelector planner_selector_; - constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; - - ModelBasedStateSpacePtr state_space_; - og::SimpleSetupPtr ompl_simple_setup_; // pass in the correct simple setup type - - /** \brief OMPL constrained state space to handle path constraints. - * - * When the parameter "use_ompl_constrained_planning" is set to true in ompl_planning.yaml, - * the path constraints are handled by this state space. - * - * **Important**: because code often depends on the attribute `state_space_` to copy states from MoveIt to OMPL, we - * must set `state_space_` to have type `ompl_interface::ConstrainedPlanningStateSpace`. The actual planning does - * not happen with this `state_space_`, but it is used to create the `constrained_state_space_` of type - * `ompl::base::ConstrainedStateSpace`. The latter is the one passed to OMPL simple setup (after creating a - * ConstrainedSpaceInformation object from it). - * */ - ob::ConstrainedStateSpacePtr constrained_state_space_; -}; - -class ModelBasedPlanningContext : public planning_interface::PlanningContext -{ -public: - ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec); - - ~ModelBasedPlanningContext() override - { - } - - void solve(planning_interface::MotionPlanResponse& res) override; - void solve(planning_interface::MotionPlanDetailedResponse& res) override; - - void clear() override; - bool terminate() override; - - const ModelBasedPlanningContextSpecification& getSpecification() const - { - return spec_; - } - - const std::map& getSpecificationConfig() const - { - return spec_.config_; - } - - void setSpecificationConfig(const std::map& config) - { - spec_.config_ = config; - } - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return spec_.state_space_->getRobotModel(); - } - - const moveit::core::JointModelGroup* getJointModelGroup() const - { - return spec_.state_space_->getJointModelGroup(); - } - - const moveit::core::RobotState& getCompleteInitialRobotState() const - { - return complete_initial_robot_state_; - } - - const ModelBasedStateSpacePtr& getOMPLStateSpace() const - { - return spec_.state_space_; - } - - const og::SimpleSetupPtr& getOMPLSimpleSetup() const - { - return ompl_simple_setup_; - } - - og::SimpleSetupPtr& getOMPLSimpleSetup() - { - return ompl_simple_setup_; - } - - const ot::Benchmark& getOMPLBenchmark() const - { - return ompl_benchmark_; - } - - ot::Benchmark& getOMPLBenchmark() - { - return ompl_benchmark_; - } - - const kinematic_constraints::KinematicConstraintSetPtr& getPathConstraints() const - { - return path_constraints_; - } - - /* \brief Get the maximum number of sampling attempts allowed when sampling states is needed */ - unsigned int getMaximumStateSamplingAttempts() const - { - return max_state_sampling_attempts_; - } - - /* \brief Set the maximum number of sampling attempts allowed when sampling states is needed */ - void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts) - { - max_state_sampling_attempts_ = max_state_sampling_attempts; - } - - /* \brief Get the maximum number of sampling attempts allowed when sampling goals is needed */ - unsigned int getMaximumGoalSamplingAttempts() const - { - return max_goal_sampling_attempts_; - } - - /* \brief Set the maximum number of sampling attempts allowed when sampling goals is needed */ - void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts) - { - max_goal_sampling_attempts_ = max_goal_sampling_attempts; - } - - /* \brief Get the maximum number of valid goal samples to store */ - unsigned int getMaximumGoalSamples() const - { - return max_goal_samples_; - } - - /* \brief Set the maximum number of valid goal samples to store */ - void setMaximumGoalSamples(unsigned int max_goal_samples) - { - max_goal_samples_ = max_goal_samples; - } - - /* \brief Get the maximum number of planning threads allowed */ - unsigned int getMaximumPlanningThreads() const - { - return max_planning_threads_; - } - - /* \brief Set the maximum number of planning threads */ - void setMaximumPlanningThreads(unsigned int max_planning_threads) - { - max_planning_threads_ = max_planning_threads; - } - - /* \brief Get the maximum solution segment length */ - double getMaximumSolutionSegmentLength() const - { - return max_solution_segment_length_; - } - - /* \brief Set the maximum solution segment length */ - void setMaximumSolutionSegmentLength(double mssl) - { - max_solution_segment_length_ = mssl; - } - - unsigned int getMinimumWaypointCount() const - { - return minimum_waypoint_count_; - } - - /** \brief Get the minimum number of waypoints along the solution path */ - void setMinimumWaypointCount(unsigned int mwc) - { - minimum_waypoint_count_ = mwc; - } - - const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager() - { - return spec_.constraint_sampler_manager_; - } - - void setConstraintSamplerManager(const constraint_samplers::ConstraintSamplerManagerPtr& csm) - { - spec_.constraint_sampler_manager_ = csm; - } - - void setVerboseStateValidityChecks(bool flag); - - void setProjectionEvaluator(const std::string& peval); - - void setPlanningVolume(const moveit_msgs::msg::WorkspaceParameters& wparams); - - void setCompleteInitialState(const moveit::core::RobotState& complete_initial_robot_state); - - bool setGoalConstraints(const std::vector& goal_constraints, - const moveit_msgs::msg::Constraints& path_constraints, - moveit_msgs::msg::MoveItErrorCodes* error); - bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints, - moveit_msgs::msg::MoveItErrorCodes* error); - - void setConstraintsApproximations(const ConstraintsLibraryPtr& constraints_library) - { - constraints_library_ = constraints_library; - } - - ConstraintsLibraryPtr getConstraintsLibraryNonConst() - { - return constraints_library_; - } - - const ConstraintsLibraryPtr& getConstraintsLibrary() const - { - return constraints_library_; - } - - bool simplifySolutions() const - { - return simplify_solutions_; - } - - void simplifySolutions(bool flag) - { - simplify_solutions_ = flag; - } - - void setInterpolation(bool flag) - { - interpolate_ = flag; - } - - void setHybridize(bool flag) - { - hybridize_ = flag; - } - - /* @brief Solve the planning problem. Return true if the problem is solved - @param timeout The time to spend on solving - @param count The number of runs to combine the paths of, in an attempt to generate better quality paths - */ - const moveit_msgs::msg::MoveItErrorCodes solve(double timeout, unsigned int count); - - /* @brief Benchmark the planning problem. Return true on successful saving of benchmark results - @param timeout The time to spend on solving - @param count The number of runs to average in the computation of the benchmark - @param filename The name of the file to which the benchmark results are to be saved (automatic names can be - provided if a name is not specified) - */ - bool benchmark(double timeout, unsigned int count, const std::string& filename = ""); - - /* @brief Get the amount of time spent computing the last plan */ - double getLastPlanTime() const - { - return last_plan_time_; - } - - /* @brief Get the amount of time spent simplifying the last plan */ - double getLastSimplifyTime() const - { - return last_simplify_time_; - } - - /* @brief Apply smoothing and try to simplify the plan - @param timeout The amount of time allowed to be spent on simplifying the plan*/ - void simplifySolution(double timeout); - - /* @brief Interpolate the solution*/ - void interpolateSolution(); - - /* @brief Get the solution as a RobotTrajectory object*/ - bool getSolutionPath(robot_trajectory::RobotTrajectory& traj) const; - - void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const; - - /** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint - * approximations to */ - bool loadConstraintApproximations(const rclcpp::Node::SharedPtr& node); - - /** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint - * approximations to */ - bool saveConstraintApproximations(const rclcpp::Node::SharedPtr& node); - - /** \brief Configure ompl_simple_setup_ and optionally the constraints_library_. - * - * ompl_simple_setup_ gets a start state, state sampler, and state validity checker. - * - * \param node ROS node used to load the constraint approximations. - * \param use_constraints_approximations Set to true if we want to load the constraint approximation. - * */ - virtual void configure(const rclcpp::Node::SharedPtr& node, bool use_constraints_approximations); - -protected: - void preSolve(); - void postSolve(); - - void startSampling(); - void stopSampling(); - - virtual ob::ProjectionEvaluatorPtr getProjectionEvaluator(const std::string& peval) const; - virtual ob::StateSamplerPtr allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const; - virtual void useConfig(); - virtual ob::GoalPtr constructGoal(); - - /* @brief Construct a planner termination condition, by default a simple time limit - @param timeout The maximum time (in seconds) that can be used for planning - @param start The point in time from which planning is considered to have started - - An additional planner termination condition can be specified per planner - configuration in ompl_planning.yaml via the `termination_condition` parameter. - Possible values are: - - * `Iteration[num]`: Terminate after `num` iterations. Here, `num` should be replaced - with a positive integer. - * `CostConvergence[solutions_window,epsilon]`: Terminate after the cost (as specified - by an optimization objective) has converged. The parameter `solutions_window` - specifies the minimum number of solutions to use in deciding whether a planner has - converged. The parameter `epsilon` is the threshold to consider for convergence. - This should be a positive number close to 0. If the cumulative moving average does - not change by a relative fraction of epsilon after a new better solution is found, - convergence has been reached. - * `ExactSolution`: Terminate as soon as an exact solution is found or a timeout - occurs. This modifies the behavior of anytime/optimizing planners to terminate - upon discovering the first feasible solution. - - In all cases, the planner will terminate when either the user-specified termination - condition is satisfied or the time limit specified by `timeout` has been reached, - whichever occurs first. - */ - virtual ob::PlannerTerminationCondition constructPlannerTerminationCondition(double timeout, - const ompl::time::point& start); - - void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc); - void unregisterTerminationCondition(); - - /** \brief Convert OMPL PlannerStatus to moveit_msgs::msg::MoveItErrorCode */ - int32_t logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup); - - ModelBasedPlanningContextSpecification spec_; - - moveit::core::RobotState complete_initial_robot_state_; - - /// the OMPL planning context; this contains the problem definition and the planner used - og::SimpleSetupPtr ompl_simple_setup_; - - /// the OMPL tool for benchmarking planners - ot::Benchmark ompl_benchmark_; - - /// tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_simple_setup_ - ot::ParallelPlan ompl_parallel_plan_; - - std::vector space_signature_; - - kinematic_constraints::KinematicConstraintSetPtr path_constraints_; - moveit_msgs::msg::Constraints path_constraints_msg_; - std::vector goal_constraints_; - - const ob::PlannerTerminationCondition* ptc_; - std::mutex ptc_lock_; - - /// the time spent computing the last plan - double last_plan_time_; - - /// the time spent simplifying the last plan - double last_simplify_time_; - - /// maximum number of valid states to store in the goal region for any planning request (when such sampling is - /// possible) - unsigned int max_goal_samples_; - - /// maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some - /// set of constraints - unsigned int max_state_sampling_attempts_; - - /// maximum number of attempts to be made at sampling a goal states - unsigned int max_goal_sampling_attempts_; - - /// when planning in parallel, this is the maximum number of threads to use at one time - unsigned int max_planning_threads_; - - /// the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the - /// extent of the space - double max_solution_segment_length_; - - /// the minimum number of points to include on the solution path (interpolation is used to reach this number, if - /// needed) - unsigned int minimum_waypoint_count_; - - /// when false, clears planners before running solve() - bool multi_query_planning_enabled_; - - ConstraintsLibraryPtr constraints_library_; - - bool simplify_solutions_; - - // if false the final solution is not interpolated - bool interpolate_; - - // if false parallel plan returns the first solution found - bool hybridize_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.hpp new file mode 100644 index 0000000000..ee0444ec4a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.hpp @@ -0,0 +1,456 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ompl_interface +{ +namespace ob = ompl::base; +namespace og = ompl::geometric; +namespace ot = ompl::tools; + +MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext); // Defines ModelBasedPlanningContextPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc + +struct ModelBasedPlanningContextSpecification; +typedef std::function + ConfiguredPlannerAllocator; +typedef std::function ConfiguredPlannerSelector; + +struct ModelBasedPlanningContextSpecification +{ + std::map config_; + ConfiguredPlannerSelector planner_selector_; + constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; + + ModelBasedStateSpacePtr state_space_; + og::SimpleSetupPtr ompl_simple_setup_; // pass in the correct simple setup type + + /** \brief OMPL constrained state space to handle path constraints. + * + * When the parameter "use_ompl_constrained_planning" is set to true in ompl_planning.yaml, + * the path constraints are handled by this state space. + * + * **Important**: because code often depends on the attribute `state_space_` to copy states from MoveIt to OMPL, we + * must set `state_space_` to have type `ompl_interface::ConstrainedPlanningStateSpace`. The actual planning does + * not happen with this `state_space_`, but it is used to create the `constrained_state_space_` of type + * `ompl::base::ConstrainedStateSpace`. The latter is the one passed to OMPL simple setup (after creating a + * ConstrainedSpaceInformation object from it). + * */ + ob::ConstrainedStateSpacePtr constrained_state_space_; +}; + +class ModelBasedPlanningContext : public planning_interface::PlanningContext +{ +public: + ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec); + + ~ModelBasedPlanningContext() override + { + } + + void solve(planning_interface::MotionPlanResponse& res) override; + void solve(planning_interface::MotionPlanDetailedResponse& res) override; + + void clear() override; + bool terminate() override; + + const ModelBasedPlanningContextSpecification& getSpecification() const + { + return spec_; + } + + const std::map& getSpecificationConfig() const + { + return spec_.config_; + } + + void setSpecificationConfig(const std::map& config) + { + spec_.config_ = config; + } + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return spec_.state_space_->getRobotModel(); + } + + const moveit::core::JointModelGroup* getJointModelGroup() const + { + return spec_.state_space_->getJointModelGroup(); + } + + const moveit::core::RobotState& getCompleteInitialRobotState() const + { + return complete_initial_robot_state_; + } + + const ModelBasedStateSpacePtr& getOMPLStateSpace() const + { + return spec_.state_space_; + } + + const og::SimpleSetupPtr& getOMPLSimpleSetup() const + { + return ompl_simple_setup_; + } + + og::SimpleSetupPtr& getOMPLSimpleSetup() + { + return ompl_simple_setup_; + } + + const ot::Benchmark& getOMPLBenchmark() const + { + return ompl_benchmark_; + } + + ot::Benchmark& getOMPLBenchmark() + { + return ompl_benchmark_; + } + + const kinematic_constraints::KinematicConstraintSetPtr& getPathConstraints() const + { + return path_constraints_; + } + + /* \brief Get the maximum number of sampling attempts allowed when sampling states is needed */ + unsigned int getMaximumStateSamplingAttempts() const + { + return max_state_sampling_attempts_; + } + + /* \brief Set the maximum number of sampling attempts allowed when sampling states is needed */ + void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts) + { + max_state_sampling_attempts_ = max_state_sampling_attempts; + } + + /* \brief Get the maximum number of sampling attempts allowed when sampling goals is needed */ + unsigned int getMaximumGoalSamplingAttempts() const + { + return max_goal_sampling_attempts_; + } + + /* \brief Set the maximum number of sampling attempts allowed when sampling goals is needed */ + void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts) + { + max_goal_sampling_attempts_ = max_goal_sampling_attempts; + } + + /* \brief Get the maximum number of valid goal samples to store */ + unsigned int getMaximumGoalSamples() const + { + return max_goal_samples_; + } + + /* \brief Set the maximum number of valid goal samples to store */ + void setMaximumGoalSamples(unsigned int max_goal_samples) + { + max_goal_samples_ = max_goal_samples; + } + + /* \brief Get the maximum number of planning threads allowed */ + unsigned int getMaximumPlanningThreads() const + { + return max_planning_threads_; + } + + /* \brief Set the maximum number of planning threads */ + void setMaximumPlanningThreads(unsigned int max_planning_threads) + { + max_planning_threads_ = max_planning_threads; + } + + /* \brief Get the maximum solution segment length */ + double getMaximumSolutionSegmentLength() const + { + return max_solution_segment_length_; + } + + /* \brief Set the maximum solution segment length */ + void setMaximumSolutionSegmentLength(double mssl) + { + max_solution_segment_length_ = mssl; + } + + unsigned int getMinimumWaypointCount() const + { + return minimum_waypoint_count_; + } + + /** \brief Get the minimum number of waypoints along the solution path */ + void setMinimumWaypointCount(unsigned int mwc) + { + minimum_waypoint_count_ = mwc; + } + + const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager() + { + return spec_.constraint_sampler_manager_; + } + + void setConstraintSamplerManager(const constraint_samplers::ConstraintSamplerManagerPtr& csm) + { + spec_.constraint_sampler_manager_ = csm; + } + + void setVerboseStateValidityChecks(bool flag); + + void setProjectionEvaluator(const std::string& peval); + + void setPlanningVolume(const moveit_msgs::msg::WorkspaceParameters& wparams); + + void setCompleteInitialState(const moveit::core::RobotState& complete_initial_robot_state); + + bool setGoalConstraints(const std::vector& goal_constraints, + const moveit_msgs::msg::Constraints& path_constraints, + moveit_msgs::msg::MoveItErrorCodes* error); + bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints, + moveit_msgs::msg::MoveItErrorCodes* error); + + void setConstraintsApproximations(const ConstraintsLibraryPtr& constraints_library) + { + constraints_library_ = constraints_library; + } + + ConstraintsLibraryPtr getConstraintsLibraryNonConst() + { + return constraints_library_; + } + + const ConstraintsLibraryPtr& getConstraintsLibrary() const + { + return constraints_library_; + } + + bool simplifySolutions() const + { + return simplify_solutions_; + } + + void simplifySolutions(bool flag) + { + simplify_solutions_ = flag; + } + + void setInterpolation(bool flag) + { + interpolate_ = flag; + } + + void setHybridize(bool flag) + { + hybridize_ = flag; + } + + /* @brief Solve the planning problem. Return true if the problem is solved + @param timeout The time to spend on solving + @param count The number of runs to combine the paths of, in an attempt to generate better quality paths + */ + const moveit_msgs::msg::MoveItErrorCodes solve(double timeout, unsigned int count); + + /* @brief Benchmark the planning problem. Return true on successful saving of benchmark results + @param timeout The time to spend on solving + @param count The number of runs to average in the computation of the benchmark + @param filename The name of the file to which the benchmark results are to be saved (automatic names can be + provided if a name is not specified) + */ + bool benchmark(double timeout, unsigned int count, const std::string& filename = ""); + + /* @brief Get the amount of time spent computing the last plan */ + double getLastPlanTime() const + { + return last_plan_time_; + } + + /* @brief Get the amount of time spent simplifying the last plan */ + double getLastSimplifyTime() const + { + return last_simplify_time_; + } + + /* @brief Apply smoothing and try to simplify the plan + @param timeout The amount of time allowed to be spent on simplifying the plan*/ + void simplifySolution(double timeout); + + /* @brief Interpolate the solution*/ + void interpolateSolution(); + + /* @brief Get the solution as a RobotTrajectory object*/ + bool getSolutionPath(robot_trajectory::RobotTrajectory& traj) const; + + void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const; + + /** @brief Look up param server 'constraint_approximations' and use its value as the path to load constraint + * approximations to */ + bool loadConstraintApproximations(const rclcpp::Node::SharedPtr& node); + + /** @brief Look up param server 'constraint_approximations' and use its value as the path to save constraint + * approximations to */ + bool saveConstraintApproximations(const rclcpp::Node::SharedPtr& node); + + /** \brief Configure ompl_simple_setup_ and optionally the constraints_library_. + * + * ompl_simple_setup_ gets a start state, state sampler, and state validity checker. + * + * \param node ROS node used to load the constraint approximations. + * \param use_constraints_approximations Set to true if we want to load the constraint approximation. + * */ + virtual void configure(const rclcpp::Node::SharedPtr& node, bool use_constraints_approximations); + +protected: + void preSolve(); + void postSolve(); + + void startSampling(); + void stopSampling(); + + virtual ob::ProjectionEvaluatorPtr getProjectionEvaluator(const std::string& peval) const; + virtual ob::StateSamplerPtr allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const; + virtual void useConfig(); + virtual ob::GoalPtr constructGoal(); + + /* @brief Construct a planner termination condition, by default a simple time limit + @param timeout The maximum time (in seconds) that can be used for planning + @param start The point in time from which planning is considered to have started + + An additional planner termination condition can be specified per planner + configuration in ompl_planning.yaml via the `termination_condition` parameter. + Possible values are: + + * `Iteration[num]`: Terminate after `num` iterations. Here, `num` should be replaced + with a positive integer. + * `CostConvergence[solutions_window,epsilon]`: Terminate after the cost (as specified + by an optimization objective) has converged. The parameter `solutions_window` + specifies the minimum number of solutions to use in deciding whether a planner has + converged. The parameter `epsilon` is the threshold to consider for convergence. + This should be a positive number close to 0. If the cumulative moving average does + not change by a relative fraction of epsilon after a new better solution is found, + convergence has been reached. + * `ExactSolution`: Terminate as soon as an exact solution is found or a timeout + occurs. This modifies the behavior of anytime/optimizing planners to terminate + upon discovering the first feasible solution. + + In all cases, the planner will terminate when either the user-specified termination + condition is satisfied or the time limit specified by `timeout` has been reached, + whichever occurs first. + */ + virtual ob::PlannerTerminationCondition constructPlannerTerminationCondition(double timeout, + const ompl::time::point& start); + + void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc); + void unregisterTerminationCondition(); + + /** \brief Convert OMPL PlannerStatus to moveit_msgs::msg::MoveItErrorCode */ + int32_t logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup); + + ModelBasedPlanningContextSpecification spec_; + + moveit::core::RobotState complete_initial_robot_state_; + + /// the OMPL planning context; this contains the problem definition and the planner used + og::SimpleSetupPtr ompl_simple_setup_; + + /// the OMPL tool for benchmarking planners + ot::Benchmark ompl_benchmark_; + + /// tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_simple_setup_ + ot::ParallelPlan ompl_parallel_plan_; + + std::vector space_signature_; + + kinematic_constraints::KinematicConstraintSetPtr path_constraints_; + moveit_msgs::msg::Constraints path_constraints_msg_; + std::vector goal_constraints_; + + const ob::PlannerTerminationCondition* ptc_; + std::mutex ptc_lock_; + + /// the time spent computing the last plan + double last_plan_time_; + + /// the time spent simplifying the last plan + double last_simplify_time_; + + /// maximum number of valid states to store in the goal region for any planning request (when such sampling is + /// possible) + unsigned int max_goal_samples_; + + /// maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some + /// set of constraints + unsigned int max_state_sampling_attempts_; + + /// maximum number of attempts to be made at sampling a goal states + unsigned int max_goal_sampling_attempts_; + + /// when planning in parallel, this is the maximum number of threads to use at one time + unsigned int max_planning_threads_; + + /// the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the + /// extent of the space + double max_solution_segment_length_; + + /// the minimum number of points to include on the solution path (interpolation is used to reach this number, if + /// needed) + unsigned int minimum_waypoint_count_; + + /// when false, clears planners before running solve() + bool multi_query_planning_enabled_; + + ConstraintsLibraryPtr constraints_library_; + + bool simplify_solutions_; + + // if false the final solution is not interpolated + bool interpolate_; + + // if false parallel plan returns the first solution found + bool hybridize_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h index 9ae1ee16fa..c8e023d5c5 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,121 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** \brief The MoveIt interface to OMPL */ -namespace ompl_interface -{ -/** @class OMPLInterface - * This class defines the interface to the motion planners in OMPL*/ -class OMPLInterface -{ -public: - /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified - * Node */ - OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace); - - /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified - Node. However, - planner configurations are used as specified in \e pconfig instead of reading them from ROS parameters - */ - OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, - const planning_interface::PlannerConfigurationMap& pconfig, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace); - - virtual ~OMPLInterface(); - - /** @brief Specify configurations for the planners. - @param pconfig Configurations for the different planners */ - void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig); - - /** @brief Get the configurations for the planners that are already loaded - @param pconfig Configurations for the different planners */ - const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const - { - return context_manager_.getPlannerConfigurations(); - } - - ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req) const; - ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::msg::MoveItErrorCodes& error_code) const; - - const PlanningContextManager& getPlanningContextManager() const - { - return context_manager_; - } - - PlanningContextManager& getPlanningContextManager() - { - return context_manager_; - } - - constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() - { - return *constraint_sampler_manager_; - } - - const constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() const - { - return *constraint_sampler_manager_; - } - - void useConstraintsApproximations(bool flag) - { - use_constraints_approximations_ = flag; - } - - bool isUsingConstraintsApproximations() const - { - return use_constraints_approximations_; - } - - /** @brief Print the status of this node*/ - void printStatus(); - -protected: - /** @brief Load planner configurations for specified group into planner_config */ - bool loadPlannerConfiguration(const std::string& group_name, const std::string& planner_id, - const std::map& group_params, - planning_interface::PlannerConfigurationSettings& planner_config); - - /** @brief Configure the planners*/ - void loadPlannerConfigurations(); - - /** @brief Load the additional plugins for sampling constraints */ - void loadConstraintSamplers(); - - /** \brief Configure the OMPL planning context for a new planning request */ - ModelBasedPlanningContextPtr prepareForSolve(const planning_interface::MotionPlanRequest& req, - const planning_scene::PlanningSceneConstPtr& planning_scene, - moveit_msgs::msg::MoveItErrorCodes* error_code, unsigned int* attempts, - double* timeout) const; - - rclcpp::Node::SharedPtr node_; /// The ROS node - const std::string parameter_namespace_; - - /** \brief The kinematic model for which motion plans are computed */ - moveit::core::RobotModelConstPtr robot_model_; - - constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; - - PlanningContextManager context_manager_; - - bool use_constraints_approximations_; - -private: - constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.hpp new file mode 100644 index 0000000000..8276db84c7 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.hpp @@ -0,0 +1,155 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** \brief The MoveIt interface to OMPL */ +namespace ompl_interface +{ +/** @class OMPLInterface + * This class defines the interface to the motion planners in OMPL*/ +class OMPLInterface +{ +public: + /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified + * Node */ + OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace); + + /** \brief Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified + Node. However, + planner configurations are used as specified in \e pconfig instead of reading them from ROS parameters + */ + OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, + const planning_interface::PlannerConfigurationMap& pconfig, const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace); + + virtual ~OMPLInterface(); + + /** @brief Specify configurations for the planners. + @param pconfig Configurations for the different planners */ + void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig); + + /** @brief Get the configurations for the planners that are already loaded + @param pconfig Configurations for the different planners */ + const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const + { + return context_manager_.getPlannerConfigurations(); + } + + ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req) const; + ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code) const; + + const PlanningContextManager& getPlanningContextManager() const + { + return context_manager_; + } + + PlanningContextManager& getPlanningContextManager() + { + return context_manager_; + } + + constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() + { + return *constraint_sampler_manager_; + } + + const constraint_samplers::ConstraintSamplerManager& getConstraintSamplerManager() const + { + return *constraint_sampler_manager_; + } + + void useConstraintsApproximations(bool flag) + { + use_constraints_approximations_ = flag; + } + + bool isUsingConstraintsApproximations() const + { + return use_constraints_approximations_; + } + + /** @brief Print the status of this node*/ + void printStatus(); + +protected: + /** @brief Load planner configurations for specified group into planner_config */ + bool loadPlannerConfiguration(const std::string& group_name, const std::string& planner_id, + const std::map& group_params, + planning_interface::PlannerConfigurationSettings& planner_config); + + /** @brief Configure the planners*/ + void loadPlannerConfigurations(); + + /** @brief Load the additional plugins for sampling constraints */ + void loadConstraintSamplers(); + + /** \brief Configure the OMPL planning context for a new planning request */ + ModelBasedPlanningContextPtr prepareForSolve(const planning_interface::MotionPlanRequest& req, + const planning_scene::PlanningSceneConstPtr& planning_scene, + moveit_msgs::msg::MoveItErrorCodes* error_code, unsigned int* attempts, + double* timeout) const; + + rclcpp::Node::SharedPtr node_; /// The ROS node + const std::string parameter_namespace_; + + /** \brief The kinematic model for which motion plans are computed */ + moveit::core::RobotModelConstPtr robot_model_; + + constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; + + PlanningContextManager context_manager_; + + bool use_constraints_approximations_; + +private: + constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h index 2423bef0df..59803884f7 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,47 +47,5 @@ /* Author: Jeroen De Maeyer */ #pragma once - -#include -#include - -namespace ompl_interface -{ -MOVEIT_CLASS_FORWARD(ConstrainedPlanningStateSpace); - -/** \brief State space to be used in combination with OMPL's constrained planning functionality. - * - * The state space is called ConstrainedPlanningStateSpace, as in "use this state space for constrained planning". - * Not to be confused with the `ompl::base::ConstrainedStateSpace`, which is constrained in the sense that it generates - * states that satisfy the constraints. This state space does not! OMPL's `ompl::base::ConstrainedStateSpace` will wrap - *around ConstrainedPlanningStateSpace. - * - * This class overrides the virtual methods to copy states from OMPL to MoveIt and vice-versa. It is used as state space - * in the `ModelBasedPlanningContextSpecification` and can handle general `ompl::base::State*` pointers that need to be - * casted to a `ompl::Base::ConstrainedStateSpace::StateType`, instead of the 'normal' - * `ompl_interface::ModelBasedStateSpace::StateType`. This casting looks like this: - * - * ompl_state->as()->getState()->as() - * - * where `getState()` is used to access the underlying state space, which should be an instance of this class. - **/ -class ConstrainedPlanningStateSpace : public ModelBasedStateSpace -{ -public: - static const std::string PARAMETERIZATION_TYPE; - - ConstrainedPlanningStateSpace(const ModelBasedStateSpaceSpecification& spec); - - const std::string& getParameterizationType() const override - { - return PARAMETERIZATION_TYPE; - } - - // override copy operations between OMPL and ROS, because a constrained state has a different internal structure - double* getValueAddressAtIndex(ompl::base::State* ompl_state, const unsigned int index) const override; - void copyToRobotState(moveit::core::RobotState& robot_state, const ompl::base::State* ompl_state) const override; - void copyToOMPLState(ompl::base::State* ompl_state, const moveit::core::RobotState& robot_state) const override; - void copyJointToOMPLState(ompl::base::State* ompl_state, const moveit::core::RobotState& robot_state, - const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const override; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.hpp new file mode 100644 index 0000000000..57d1c63df2 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.hpp @@ -0,0 +1,81 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * 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 KU Leuven 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: Jeroen De Maeyer */ + +#pragma once + +#include +#include + +namespace ompl_interface +{ +MOVEIT_CLASS_FORWARD(ConstrainedPlanningStateSpace); + +/** \brief State space to be used in combination with OMPL's constrained planning functionality. + * + * The state space is called ConstrainedPlanningStateSpace, as in "use this state space for constrained planning". + * Not to be confused with the `ompl::base::ConstrainedStateSpace`, which is constrained in the sense that it generates + * states that satisfy the constraints. This state space does not! OMPL's `ompl::base::ConstrainedStateSpace` will wrap + *around ConstrainedPlanningStateSpace. + * + * This class overrides the virtual methods to copy states from OMPL to MoveIt and vice-versa. It is used as state space + * in the `ModelBasedPlanningContextSpecification` and can handle general `ompl::base::State*` pointers that need to be + * casted to a `ompl::Base::ConstrainedStateSpace::StateType`, instead of the 'normal' + * `ompl_interface::ModelBasedStateSpace::StateType`. This casting looks like this: + * + * ompl_state->as()->getState()->as() + * + * where `getState()` is used to access the underlying state space, which should be an instance of this class. + **/ +class ConstrainedPlanningStateSpace : public ModelBasedStateSpace +{ +public: + static const std::string PARAMETERIZATION_TYPE; + + ConstrainedPlanningStateSpace(const ModelBasedStateSpaceSpecification& spec); + + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } + + // override copy operations between OMPL and ROS, because a constrained state has a different internal structure + double* getValueAddressAtIndex(ompl::base::State* ompl_state, const unsigned int index) const override; + void copyToRobotState(moveit::core::RobotState& robot_state, const ompl::base::State* ompl_state) const override; + void copyToOMPLState(ompl::base::State* ompl_state, const moveit::core::RobotState& robot_state) const override; + void copyJointToOMPLState(ompl::base::State* ompl_state, const moveit::core::RobotState& robot_state, + const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h index e7a8067266..e8761e8092 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,32 +48,5 @@ /* Mostly copied from Ioan Sucan's code */ #pragma once - -#include - -namespace ompl_interface -{ -class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory -{ -public: - ConstrainedPlanningStateSpaceFactory(); - - /** \brief Return a priority that this planner should be used for this specific planning problem. - * - * This state space factory is currently only used if there is exactly one position or orientation constraint, - * or if `enforce_constrained_state_space` was set to `true` in ompl_planning.yaml. - * In the first case, we prefer planning in the constrained state space and return a priority of 200. - * In the second case, it is the only factory to choose from, so the priority does not matter, - * and it returns a low priority so it will never be chosen when others are available. - * (The second lowest priority is -1 in the PoseModelStateSpaceFactory.) - * - * For more details on this state space selection process, see: - * https://github.com/JeroenDM/moveit/pull/2 - * **/ - int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const override; - -protected: - ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.hpp new file mode 100644 index 0000000000..2a6392bd10 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, KU Leuven + * 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 KU Leuven 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: Jeroen De Maeyer */ +/* Mostly copied from Ioan Sucan's code */ + +#pragma once + +#include + +namespace ompl_interface +{ +class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory +{ +public: + ConstrainedPlanningStateSpaceFactory(); + + /** \brief Return a priority that this planner should be used for this specific planning problem. + * + * This state space factory is currently only used if there is exactly one position or orientation constraint, + * or if `enforce_constrained_state_space` was set to `true` in ompl_planning.yaml. + * In the first case, we prefer planning in the constrained state space and return a priority of 200. + * In the second case, it is the only factory to choose from, so the priority does not matter, + * and it returns a low priority so it will never be chosen when others are available. + * (The second lowest priority is -1 in the PoseModelStateSpaceFactory.) + * + * For more details on this state space selection process, see: + * https://github.com/JeroenDM/moveit/pull/2 + * **/ + int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + const moveit::core::RobotModelConstPtr& robot_model) const override; + +protected: + ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h index 91d8867a10..cfe7a5cb74 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,21 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace ompl_interface -{ -class JointModelStateSpace : public ModelBasedStateSpace -{ -public: - static const std::string PARAMETERIZATION_TYPE; - - JointModelStateSpace(const ModelBasedStateSpaceSpecification& spec); - - const std::string& getParameterizationType() const override - { - return PARAMETERIZATION_TYPE; - } -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.hpp new file mode 100644 index 0000000000..26ec40263b --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.hpp @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +namespace ompl_interface +{ +class JointModelStateSpace : public ModelBasedStateSpace +{ +public: + static const std::string PARAMETERIZATION_TYPE; + + JointModelStateSpace(const ModelBasedStateSpaceSpecification& spec); + + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index d45f8b737a..06919287f1 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,20 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace ompl_interface -{ -class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory -{ -public: - JointModelStateSpaceFactory(); - - int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const override; - -protected: - ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.hpp new file mode 100644 index 0000000000..c8d21154c7 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.hpp @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +namespace ompl_interface +{ +class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory +{ +public: + JointModelStateSpaceFactory(); + + int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + const moveit::core::RobotModelConstPtr& robot_model) const override; + +protected: + ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index e334322a98..37dcc757dc 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,239 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -namespace ompl_interface -{ -typedef std::function - InterpolationFunction; -typedef std::function DistanceFunction; - -struct ModelBasedStateSpaceSpecification -{ - ModelBasedStateSpaceSpecification(const moveit::core::RobotModelConstPtr& robot_model, - const moveit::core::JointModelGroup* jmg) - : robot_model_(robot_model), joint_model_group_(jmg) - { - } - - ModelBasedStateSpaceSpecification(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name) - : robot_model_(robot_model), joint_model_group_(robot_model_->getJointModelGroup(group_name)) - { - if (!joint_model_group_) - throw std::runtime_error("Group '" + group_name + "' was not found"); - } - - moveit::core::RobotModelConstPtr robot_model_; - const moveit::core::JointModelGroup* joint_model_group_; - moveit::core::JointBoundsVector joint_bounds_; -}; - -OMPL_CLASS_FORWARD(ModelBasedStateSpace); - -class ModelBasedStateSpace : public ompl::base::StateSpace -{ -public: - class StateType : public ompl::base::State - { - public: - enum - { - VALIDITY_KNOWN = 1, - GOAL_DISTANCE_KNOWN = 2, - VALIDITY_TRUE = 4, - IS_START_STATE = 8, - IS_GOAL_STATE = 16 - }; - - StateType() : ompl::base::State(), values(nullptr), tag(-1), flags(0), distance(0.0) - { - } - - void markValid(double d) - { - distance = d; - flags |= GOAL_DISTANCE_KNOWN; - markValid(); - } - - void markValid() - { - flags |= (VALIDITY_KNOWN | VALIDITY_TRUE); - } - - void markInvalid(double d) - { - distance = d; - flags |= GOAL_DISTANCE_KNOWN; - markInvalid(); - } - - void markInvalid() - { - flags &= ~VALIDITY_TRUE; - flags |= VALIDITY_KNOWN; - } - - bool isValidityKnown() const - { - return flags & VALIDITY_KNOWN; - } - - void clearKnownInformation() - { - flags = 0; - } - - bool isMarkedValid() const - { - return flags & VALIDITY_TRUE; - } - - bool isGoalDistanceKnown() const - { - return flags & GOAL_DISTANCE_KNOWN; - } - - bool isStartState() const - { - return flags & IS_START_STATE; - } - - bool isGoalState() const - { - return flags & IS_GOAL_STATE; - } - - bool isInputState() const - { - return flags & (IS_START_STATE | IS_GOAL_STATE); - } - - void markStartState() - { - flags |= IS_START_STATE; - } - - void markGoalState() - { - flags |= IS_GOAL_STATE; - } - - double* values; - int tag; - int flags; - double distance; - }; - - ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec); - ~ModelBasedStateSpace() override; - - void setInterpolationFunction(const InterpolationFunction& fun) - { - interpolation_function_ = fun; - } - - void setDistanceFunction(const DistanceFunction& fun) - { - distance_function_ = fun; - } - - ompl::base::State* allocState() const override; - void freeState(ompl::base::State* state) const override; - unsigned int getDimension() const override; - void enforceBounds(ompl::base::State* state) const override; - bool satisfiesBounds(const ompl::base::State* state) const override; - - void copyState(ompl::base::State* destination, const ompl::base::State* source) const override; - void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, - ompl::base::State* state) const override; - double distance(const ompl::base::State* state1, const ompl::base::State* state2) const override; - bool equalStates(const ompl::base::State* state1, const ompl::base::State* state2) const override; - double getMaximumExtent() const override; - double getMeasure() const override; - - unsigned int getSerializationLength() const override; - void serialize(void* serialization, const ompl::base::State* state) const override; - void deserialize(ompl::base::State* state, const void* serialization) const override; - double* getValueAddressAtIndex(ompl::base::State* state, const unsigned int index) const override; - - ompl::base::StateSamplerPtr allocDefaultStateSampler() const override; - - virtual const std::string& getParameterizationType() const = 0; - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return spec_.robot_model_; - } - - const moveit::core::JointModelGroup* getJointModelGroup() const - { - return spec_.joint_model_group_; - } - - const std::string& getJointModelGroupName() const - { - return getJointModelGroup()->getName(); - } - - const ModelBasedStateSpaceSpecification& getSpecification() const - { - return spec_; - } - - void printState(const ompl::base::State* state, std::ostream& out) const override; - void printSettings(std::ostream& out) const override; - - /// Set the planning volume for the possible SE2 and/or SE3 components of the state space - virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ); - - const moveit::core::JointBoundsVector& getJointsBounds() const - { - return spec_.joint_bounds_; - } - - /// Copy the data from an OMPL state to a set of joint states. - // The joint states \b must be specified in the same order as the joint models in the constructor - virtual void copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const; - - /// Copy the data from a set of joint states to an OMPL state. - // The joint states \b must be specified in the same order as the joint models in the constructor - virtual void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const; - - /** - * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt robot_state to an OMPL - * state. - * \param state - output OMPL state with single joint modified - * \param robot_state - input MoveIt state to get the joint value from - * \param joint_model - the joint to copy values of - * \param ompl_state_joint_index - the index of the joint in the ompl state (passed in for efficiency, you should - * cache this index) - * e.g. ompl_state_joint_index = joint_model_group_->getVariableGroupIndex("virtual_joint"); - */ - virtual void copyJointToOMPLState(ompl::base::State* state, const moveit::core::RobotState& robot_state, - const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const; - - double getTagSnapToSegment() const; - void setTagSnapToSegment(double snap); - -protected: - ModelBasedStateSpaceSpecification spec_; - std::vector joint_bounds_storage_; - std::vector joint_model_vector_; - unsigned int variable_count_; - size_t state_values_size_; - - InterpolationFunction interpolation_function_; - DistanceFunction distance_function_; - - double tag_snap_to_segment_; - double tag_snap_to_segment_complement_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.hpp new file mode 100644 index 0000000000..c203be9dd0 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.hpp @@ -0,0 +1,273 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace ompl_interface +{ +typedef std::function + InterpolationFunction; +typedef std::function DistanceFunction; + +struct ModelBasedStateSpaceSpecification +{ + ModelBasedStateSpaceSpecification(const moveit::core::RobotModelConstPtr& robot_model, + const moveit::core::JointModelGroup* jmg) + : robot_model_(robot_model), joint_model_group_(jmg) + { + } + + ModelBasedStateSpaceSpecification(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name) + : robot_model_(robot_model), joint_model_group_(robot_model_->getJointModelGroup(group_name)) + { + if (!joint_model_group_) + throw std::runtime_error("Group '" + group_name + "' was not found"); + } + + moveit::core::RobotModelConstPtr robot_model_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::JointBoundsVector joint_bounds_; +}; + +OMPL_CLASS_FORWARD(ModelBasedStateSpace); + +class ModelBasedStateSpace : public ompl::base::StateSpace +{ +public: + class StateType : public ompl::base::State + { + public: + enum + { + VALIDITY_KNOWN = 1, + GOAL_DISTANCE_KNOWN = 2, + VALIDITY_TRUE = 4, + IS_START_STATE = 8, + IS_GOAL_STATE = 16 + }; + + StateType() : ompl::base::State(), values(nullptr), tag(-1), flags(0), distance(0.0) + { + } + + void markValid(double d) + { + distance = d; + flags |= GOAL_DISTANCE_KNOWN; + markValid(); + } + + void markValid() + { + flags |= (VALIDITY_KNOWN | VALIDITY_TRUE); + } + + void markInvalid(double d) + { + distance = d; + flags |= GOAL_DISTANCE_KNOWN; + markInvalid(); + } + + void markInvalid() + { + flags &= ~VALIDITY_TRUE; + flags |= VALIDITY_KNOWN; + } + + bool isValidityKnown() const + { + return flags & VALIDITY_KNOWN; + } + + void clearKnownInformation() + { + flags = 0; + } + + bool isMarkedValid() const + { + return flags & VALIDITY_TRUE; + } + + bool isGoalDistanceKnown() const + { + return flags & GOAL_DISTANCE_KNOWN; + } + + bool isStartState() const + { + return flags & IS_START_STATE; + } + + bool isGoalState() const + { + return flags & IS_GOAL_STATE; + } + + bool isInputState() const + { + return flags & (IS_START_STATE | IS_GOAL_STATE); + } + + void markStartState() + { + flags |= IS_START_STATE; + } + + void markGoalState() + { + flags |= IS_GOAL_STATE; + } + + double* values; + int tag; + int flags; + double distance; + }; + + ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec); + ~ModelBasedStateSpace() override; + + void setInterpolationFunction(const InterpolationFunction& fun) + { + interpolation_function_ = fun; + } + + void setDistanceFunction(const DistanceFunction& fun) + { + distance_function_ = fun; + } + + ompl::base::State* allocState() const override; + void freeState(ompl::base::State* state) const override; + unsigned int getDimension() const override; + void enforceBounds(ompl::base::State* state) const override; + bool satisfiesBounds(const ompl::base::State* state) const override; + + void copyState(ompl::base::State* destination, const ompl::base::State* source) const override; + void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, + ompl::base::State* state) const override; + double distance(const ompl::base::State* state1, const ompl::base::State* state2) const override; + bool equalStates(const ompl::base::State* state1, const ompl::base::State* state2) const override; + double getMaximumExtent() const override; + double getMeasure() const override; + + unsigned int getSerializationLength() const override; + void serialize(void* serialization, const ompl::base::State* state) const override; + void deserialize(ompl::base::State* state, const void* serialization) const override; + double* getValueAddressAtIndex(ompl::base::State* state, const unsigned int index) const override; + + ompl::base::StateSamplerPtr allocDefaultStateSampler() const override; + + virtual const std::string& getParameterizationType() const = 0; + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return spec_.robot_model_; + } + + const moveit::core::JointModelGroup* getJointModelGroup() const + { + return spec_.joint_model_group_; + } + + const std::string& getJointModelGroupName() const + { + return getJointModelGroup()->getName(); + } + + const ModelBasedStateSpaceSpecification& getSpecification() const + { + return spec_; + } + + void printState(const ompl::base::State* state, std::ostream& out) const override; + void printSettings(std::ostream& out) const override; + + /// Set the planning volume for the possible SE2 and/or SE3 components of the state space + virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ); + + const moveit::core::JointBoundsVector& getJointsBounds() const + { + return spec_.joint_bounds_; + } + + /// Copy the data from an OMPL state to a set of joint states. + // The joint states \b must be specified in the same order as the joint models in the constructor + virtual void copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const; + + /// Copy the data from a set of joint states to an OMPL state. + // The joint states \b must be specified in the same order as the joint models in the constructor + virtual void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const; + + /** + * \brief Copy a single joint's values (which might have multiple variables) from a MoveIt robot_state to an OMPL + * state. + * \param state - output OMPL state with single joint modified + * \param robot_state - input MoveIt state to get the joint value from + * \param joint_model - the joint to copy values of + * \param ompl_state_joint_index - the index of the joint in the ompl state (passed in for efficiency, you should + * cache this index) + * e.g. ompl_state_joint_index = joint_model_group_->getVariableGroupIndex("virtual_joint"); + */ + virtual void copyJointToOMPLState(ompl::base::State* state, const moveit::core::RobotState& robot_state, + const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const; + + double getTagSnapToSegment() const; + void setTagSnapToSegment(double snap); + +protected: + ModelBasedStateSpaceSpecification spec_; + std::vector joint_bounds_storage_; + std::vector joint_model_vector_; + unsigned int variable_count_; + size_t state_values_size_; + + InterpolationFunction interpolation_function_; + DistanceFunction distance_function_; + + double tag_snap_to_segment_; + double tag_snap_to_segment_complement_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h index bcef1410ac..bc63210279 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,42 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace ompl_interface -{ -MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory); // Defines ModelBasedStateSpaceFactoryPtr, ConstPtr, WeakPtr... etc - -class ModelBasedStateSpaceFactory -{ -public: - ModelBasedStateSpaceFactory() - { - } - - virtual ~ModelBasedStateSpaceFactory() - { - } - - ModelBasedStateSpacePtr getNewStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const; - - const std::string& getType() const - { - return type_; - } - - /** \brief Decide whether the type of state space constructed by this factory could represent problems specified by - the user - request \e req for group \e group. The group \e group must always be specified and takes precedence over \e - req.group_name, which may be different */ - virtual int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const = 0; - -protected: - virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const = 0; - std::string type_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.hpp new file mode 100644 index 0000000000..763d7c3da3 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +namespace ompl_interface +{ +MOVEIT_CLASS_FORWARD(ModelBasedStateSpaceFactory); // Defines ModelBasedStateSpaceFactoryPtr, ConstPtr, WeakPtr... etc + +class ModelBasedStateSpaceFactory +{ +public: + ModelBasedStateSpaceFactory() + { + } + + virtual ~ModelBasedStateSpaceFactory() + { + } + + ModelBasedStateSpacePtr getNewStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const; + + const std::string& getType() const + { + return type_; + } + + /** \brief Decide whether the type of state space constructed by this factory could represent problems specified by + the user + request \e req for group \e group. The group \e group must always be specified and takes precedence over \e + req.group_name, which may be different */ + virtual int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + const moveit::core::RobotModelConstPtr& robot_model) const = 0; + +protected: + virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const = 0; + std::string type_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 4ebe272e8d..86d15a254b 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,116 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include - -namespace ompl_interface -{ -class PoseModelStateSpace : public ModelBasedStateSpace -{ -public: - static const std::string PARAMETERIZATION_TYPE; - - class StateType : public ModelBasedStateSpace::StateType - { - public: - enum - { - JOINTS_COMPUTED = 256, - POSE_COMPUTED = 512 - }; - - StateType() : ModelBasedStateSpace::StateType(), poses(nullptr) - { - flags |= JOINTS_COMPUTED; - } - - bool jointsComputed() const - { - return flags & JOINTS_COMPUTED; - } - - bool poseComputed() const - { - return flags & POSE_COMPUTED; - } - - void setJointsComputed(bool value) - { - if (value) - { - flags |= JOINTS_COMPUTED; - } - else - { - flags &= ~JOINTS_COMPUTED; - } - } - - void setPoseComputed(bool value) - { - if (value) - { - flags |= POSE_COMPUTED; - } - else - { - flags &= ~POSE_COMPUTED; - } - } - - ompl::base::SE3StateSpace::StateType** poses; - }; - - PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec); - ~PoseModelStateSpace() override; - - ompl::base::State* allocState() const override; - void freeState(ompl::base::State* state) const override; - void copyState(ompl::base::State* destination, const ompl::base::State* source) const override; - void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, - ompl::base::State* state) const override; - double distance(const ompl::base::State* state1, const ompl::base::State* state2) const override; - double getMaximumExtent() const override; - - ompl::base::StateSamplerPtr allocDefaultStateSampler() const override; - - bool computeStateFK(ompl::base::State* state) const; - bool computeStateIK(ompl::base::State* state) const; - bool computeStateK(ompl::base::State* state) const; - - void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) override; - void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const override; - void sanityChecks() const override; - - const std::string& getParameterizationType() const override - { - return PARAMETERIZATION_TYPE; - } - -private: - struct PoseComponent - { - PoseComponent(const moveit::core::JointModelGroup* subgroup, - const moveit::core::JointModelGroup::KinematicsSolver& k); - - bool computeStateFK(StateType* full_state, unsigned int idx) const; - bool computeStateIK(StateType* full_state, unsigned int idx) const; - - bool operator<(const PoseComponent& o) const - { - return subgroup_->getName() < o.subgroup_->getName(); - } - - const moveit::core::JointModelGroup* subgroup_; - kinematics::KinematicsBasePtr kinematics_solver_; - std::vector bijection_; - ompl::base::StateSpacePtr state_space_; - std::vector fk_link_; - }; - - std::vector poses_; - double jump_factor_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.hpp new file mode 100644 index 0000000000..3bdd08a108 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.hpp @@ -0,0 +1,150 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include + +namespace ompl_interface +{ +class PoseModelStateSpace : public ModelBasedStateSpace +{ +public: + static const std::string PARAMETERIZATION_TYPE; + + class StateType : public ModelBasedStateSpace::StateType + { + public: + enum + { + JOINTS_COMPUTED = 256, + POSE_COMPUTED = 512 + }; + + StateType() : ModelBasedStateSpace::StateType(), poses(nullptr) + { + flags |= JOINTS_COMPUTED; + } + + bool jointsComputed() const + { + return flags & JOINTS_COMPUTED; + } + + bool poseComputed() const + { + return flags & POSE_COMPUTED; + } + + void setJointsComputed(bool value) + { + if (value) + { + flags |= JOINTS_COMPUTED; + } + else + { + flags &= ~JOINTS_COMPUTED; + } + } + + void setPoseComputed(bool value) + { + if (value) + { + flags |= POSE_COMPUTED; + } + else + { + flags &= ~POSE_COMPUTED; + } + } + + ompl::base::SE3StateSpace::StateType** poses; + }; + + PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec); + ~PoseModelStateSpace() override; + + ompl::base::State* allocState() const override; + void freeState(ompl::base::State* state) const override; + void copyState(ompl::base::State* destination, const ompl::base::State* source) const override; + void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, + ompl::base::State* state) const override; + double distance(const ompl::base::State* state1, const ompl::base::State* state2) const override; + double getMaximumExtent() const override; + + ompl::base::StateSamplerPtr allocDefaultStateSampler() const override; + + bool computeStateFK(ompl::base::State* state) const; + bool computeStateIK(ompl::base::State* state) const; + bool computeStateK(ompl::base::State* state) const; + + void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) override; + void copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const override; + void sanityChecks() const override; + + const std::string& getParameterizationType() const override + { + return PARAMETERIZATION_TYPE; + } + +private: + struct PoseComponent + { + PoseComponent(const moveit::core::JointModelGroup* subgroup, + const moveit::core::JointModelGroup::KinematicsSolver& k); + + bool computeStateFK(StateType* full_state, unsigned int idx) const; + bool computeStateIK(StateType* full_state, unsigned int idx) const; + + bool operator<(const PoseComponent& o) const + { + return subgroup_->getName() < o.subgroup_->getName(); + } + + const moveit::core::JointModelGroup* subgroup_; + kinematics::KinematicsBasePtr kinematics_solver_; + std::vector bijection_; + ompl::base::StateSpacePtr state_space_; + std::vector fk_link_; + }; + + std::vector poses_; + double jump_factor_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index 12a6daee9e..ff67a81f36 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,20 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace ompl_interface -{ -class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory -{ -public: - PoseModelStateSpaceFactory(); - - int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, - const moveit::core::RobotModelConstPtr& robot_model) const override; - -protected: - ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.hpp new file mode 100644 index 0000000000..d0340f99bc --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.hpp @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +namespace ompl_interface +{ +class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory +{ +public: + PoseModelStateSpaceFactory(); + + int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + const moveit::core::RobotModelConstPtr& robot_model) const override; + +protected: + ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const override; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index f14aad9d5b..4bcf8dc192 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,236 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -#include - -#include -#include - -namespace ompl_interface -{ -class MultiQueryPlannerAllocator -{ -public: - MultiQueryPlannerAllocator() = default; - ~MultiQueryPlannerAllocator(); - - template - ob::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name, - const ModelBasedPlanningContextSpecification& spec); - -private: - template - ob::PlannerPtr allocatePlannerImpl(const ob::SpaceInformationPtr& si, const std::string& new_name, - const ModelBasedPlanningContextSpecification& spec, bool load_planner_data = false, - bool store_planner_data = false, const std::string& file_path = ""); - - template - inline ob::Planner* allocatePersistentPlanner(const ob::PlannerData& data); - - // Storing multi-query planners - std::map planners_; - - std::map planner_data_storage_paths_; - - // Store and load planner data - ob::PlannerDataStorage storage_; -}; - -class PlanningContextManager -{ -public: - PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, - constraint_samplers::ConstraintSamplerManagerPtr csm); - ~PlanningContextManager(); - - /** @brief Specify configurations for the planners. - @param pconfig Configurations for the different planners */ - void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig); - - /** @brief Return the previously set planner configurations */ - const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const - { - return planner_configs_; - } - - /* \brief Get the maximum number of sampling attempts allowed when sampling states is needed */ - unsigned int getMaximumStateSamplingAttempts() const - { - return max_state_sampling_attempts_; - } - - /* \brief Set the maximum number of sampling attempts allowed when sampling states is needed */ - void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts) - { - max_state_sampling_attempts_ = max_state_sampling_attempts; - } - - /* \brief Get the maximum number of sampling attempts allowed when sampling goals is needed */ - unsigned int getMaximumGoalSamplingAttempts() const - { - return max_goal_sampling_attempts_; - } - - /* \brief Set the maximum number of sampling attempts allowed when sampling goals is needed */ - void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts) - { - max_goal_sampling_attempts_ = max_goal_sampling_attempts; - } - - /* \brief Get the maximum number of goal samples */ - unsigned int getMaximumGoalSamples() const - { - return max_goal_samples_; - } - - /* \brief Set the maximum number of goal samples */ - void setMaximumGoalSamples(unsigned int max_goal_samples) - { - max_goal_samples_ = max_goal_samples; - } - - /* \brief Get the maximum number of planning threads allowed */ - unsigned int getMaximumPlanningThreads() const - { - return max_planning_threads_; - } - - /* \brief Set the maximum number of planning threads */ - void setMaximumPlanningThreads(unsigned int max_planning_threads) - { - max_planning_threads_ = max_planning_threads; - } - - /* \brief Get the maximum solution segment length */ - double getMaximumSolutionSegmentLength() const - { - return max_solution_segment_length_; - } - - /* \brief Set the maximum solution segment length */ - void setMaximumSolutionSegmentLength(double mssl) - { - max_solution_segment_length_ = mssl; - } - - unsigned int getMinimumWaypointCount() const - { - return minimum_waypoint_count_; - } - - /** \brief Get the minimum number of waypoints along the solution path */ - void setMinimumWaypointCount(unsigned int mwc) - { - minimum_waypoint_count_ = mwc; - } - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - /** \brief Returns a planning context to OMPLInterface, which in turn passes it to OMPLPlannerManager. - * - * This function checks the input and reads planner specific configurations. - * Then it creates the planning context with PlanningContextManager::createPlanningContext. - * Finally, it puts the context into a state appropriate for planning. - * This last step involves setting the start, goal, and state validity checker using the method - * ModelBasedPlanningContext::configure. - * - * */ - ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const rclcpp::Node::SharedPtr& node, - bool use_constraints_approximations) const; - - void registerPlannerAllocator(const std::string& planner_id, const ConfiguredPlannerAllocator& pa) - { - known_planners_[planner_id] = pa; - } - - void registerStateSpaceFactory(const ModelBasedStateSpaceFactoryPtr& factory) - { - state_space_factories_[factory->getType()] = factory; - } - - const std::map& getRegisteredPlannerAllocators() const - { - return known_planners_; - } - - const std::map& getRegisteredStateSpaceFactories() const - { - return state_space_factories_; - } - - ConfiguredPlannerSelector getPlannerSelector() const; - -protected: - ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const; - - void registerDefaultPlanners(); - void registerDefaultStateSpaces(); - - template - void registerPlannerAllocatorHelper(const std::string& planner_id); - - /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */ - ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, - const ModelBasedStateSpaceFactoryPtr& factory, - const moveit_msgs::msg::MotionPlanRequest& req) const; - - const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& factory_type) const; - const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& group_name, - const moveit_msgs::msg::MotionPlanRequest& req) const; - - /** \brief The kinematic model for which motion plans are computed */ - moveit::core::RobotModelConstPtr robot_model_; - - constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; - - std::map known_planners_; - std::map state_space_factories_; - - /** \brief All the existing planning configurations. The name - of the configuration is the key of the map. This name can - be of the form "group_name[config_name]" if there are - particular configurations specified for a group, or of the - form "group_name" if default settings are to be used. */ - planning_interface::PlannerConfigurationMap planner_configs_; - - /// maximum number of states to sample in the goal region for any planning request (when such sampling is possible) - unsigned int max_goal_samples_; - - /// maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some - /// set of constraints - unsigned int max_state_sampling_attempts_; - - /// maximum number of attempts to be made at sampling goals - unsigned int max_goal_sampling_attempts_; - - /// when planning in parallel, this is the maximum number of threads to use at one time - unsigned int max_planning_threads_; - - /// the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the - /// extent of the space - double max_solution_segment_length_; - - /// the minimum number of points to include on the solution path (interpolation is used to reach this number, if - /// needed) - unsigned int minimum_waypoint_count_; - - /// Multi-query planner allocator - MultiQueryPlannerAllocator planner_allocator_; - -private: - MOVEIT_STRUCT_FORWARD(CachedContexts); - CachedContextsPtr cached_contexts_; -}; -} // namespace ompl_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.hpp new file mode 100644 index 0000000000..b9469f9d91 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.hpp @@ -0,0 +1,270 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include + +namespace ompl_interface +{ +class MultiQueryPlannerAllocator +{ +public: + MultiQueryPlannerAllocator() = default; + ~MultiQueryPlannerAllocator(); + + template + ob::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec); + +private: + template + ob::PlannerPtr allocatePlannerImpl(const ob::SpaceInformationPtr& si, const std::string& new_name, + const ModelBasedPlanningContextSpecification& spec, bool load_planner_data = false, + bool store_planner_data = false, const std::string& file_path = ""); + + template + inline ob::Planner* allocatePersistentPlanner(const ob::PlannerData& data); + + // Storing multi-query planners + std::map planners_; + + std::map planner_data_storage_paths_; + + // Store and load planner data + ob::PlannerDataStorage storage_; +}; + +class PlanningContextManager +{ +public: + PlanningContextManager(moveit::core::RobotModelConstPtr robot_model, + constraint_samplers::ConstraintSamplerManagerPtr csm); + ~PlanningContextManager(); + + /** @brief Specify configurations for the planners. + @param pconfig Configurations for the different planners */ + void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig); + + /** @brief Return the previously set planner configurations */ + const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const + { + return planner_configs_; + } + + /* \brief Get the maximum number of sampling attempts allowed when sampling states is needed */ + unsigned int getMaximumStateSamplingAttempts() const + { + return max_state_sampling_attempts_; + } + + /* \brief Set the maximum number of sampling attempts allowed when sampling states is needed */ + void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts) + { + max_state_sampling_attempts_ = max_state_sampling_attempts; + } + + /* \brief Get the maximum number of sampling attempts allowed when sampling goals is needed */ + unsigned int getMaximumGoalSamplingAttempts() const + { + return max_goal_sampling_attempts_; + } + + /* \brief Set the maximum number of sampling attempts allowed when sampling goals is needed */ + void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts) + { + max_goal_sampling_attempts_ = max_goal_sampling_attempts; + } + + /* \brief Get the maximum number of goal samples */ + unsigned int getMaximumGoalSamples() const + { + return max_goal_samples_; + } + + /* \brief Set the maximum number of goal samples */ + void setMaximumGoalSamples(unsigned int max_goal_samples) + { + max_goal_samples_ = max_goal_samples; + } + + /* \brief Get the maximum number of planning threads allowed */ + unsigned int getMaximumPlanningThreads() const + { + return max_planning_threads_; + } + + /* \brief Set the maximum number of planning threads */ + void setMaximumPlanningThreads(unsigned int max_planning_threads) + { + max_planning_threads_ = max_planning_threads; + } + + /* \brief Get the maximum solution segment length */ + double getMaximumSolutionSegmentLength() const + { + return max_solution_segment_length_; + } + + /* \brief Set the maximum solution segment length */ + void setMaximumSolutionSegmentLength(double mssl) + { + max_solution_segment_length_ = mssl; + } + + unsigned int getMinimumWaypointCount() const + { + return minimum_waypoint_count_; + } + + /** \brief Get the minimum number of waypoints along the solution path */ + void setMinimumWaypointCount(unsigned int mwc) + { + minimum_waypoint_count_ = mwc; + } + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + /** \brief Returns a planning context to OMPLInterface, which in turn passes it to OMPLPlannerManager. + * + * This function checks the input and reads planner specific configurations. + * Then it creates the planning context with PlanningContextManager::createPlanningContext. + * Finally, it puts the context into a state appropriate for planning. + * This last step involves setting the start, goal, and state validity checker using the method + * ModelBasedPlanningContext::configure. + * + * */ + ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const rclcpp::Node::SharedPtr& node, + bool use_constraints_approximations) const; + + void registerPlannerAllocator(const std::string& planner_id, const ConfiguredPlannerAllocator& pa) + { + known_planners_[planner_id] = pa; + } + + void registerStateSpaceFactory(const ModelBasedStateSpaceFactoryPtr& factory) + { + state_space_factories_[factory->getType()] = factory; + } + + const std::map& getRegisteredPlannerAllocators() const + { + return known_planners_; + } + + const std::map& getRegisteredStateSpaceFactories() const + { + return state_space_factories_; + } + + ConfiguredPlannerSelector getPlannerSelector() const; + +protected: + ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const; + + void registerDefaultPlanners(); + void registerDefaultStateSpaces(); + + template + void registerPlannerAllocatorHelper(const std::string& planner_id); + + /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */ + ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, + const ModelBasedStateSpaceFactoryPtr& factory, + const moveit_msgs::msg::MotionPlanRequest& req) const; + + const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& factory_type) const; + const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& group_name, + const moveit_msgs::msg::MotionPlanRequest& req) const; + + /** \brief The kinematic model for which motion plans are computed */ + moveit::core::RobotModelConstPtr robot_model_; + + constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; + + std::map known_planners_; + std::map state_space_factories_; + + /** \brief All the existing planning configurations. The name + of the configuration is the key of the map. This name can + be of the form "group_name[config_name]" if there are + particular configurations specified for a group, or of the + form "group_name" if default settings are to be used. */ + planning_interface::PlannerConfigurationMap planner_configs_; + + /// maximum number of states to sample in the goal region for any planning request (when such sampling is possible) + unsigned int max_goal_samples_; + + /// maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some + /// set of constraints + unsigned int max_state_sampling_attempts_; + + /// maximum number of attempts to be made at sampling goals + unsigned int max_goal_sampling_attempts_; + + /// when planning in parallel, this is the maximum number of threads to use at one time + unsigned int max_planning_threads_; + + /// the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the + /// extent of the space + double max_solution_segment_length_; + + /// the minimum number of points to include on the solution path (interpolation is used to reach this number, if + /// needed) + unsigned int minimum_waypoint_count_; + + /// Multi-query planner allocator + MultiQueryPlannerAllocator planner_allocator_; + +private: + MOVEIT_STRUCT_FORWARD(CachedContexts); + CachedContextsPtr cached_contexts_; +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp index ca1b90114c..fe30f37753 100644 --- a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp @@ -35,15 +35,15 @@ /* Authors: Ioan Sucan, Michael Goerner */ -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include -#include -#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index 84814ac7c0..34afabd9ae 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp index 38a020c57b..cd06ec5c95 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_sampler.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 35a94544a7..d066d9f9b4 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -37,8 +37,8 @@ #include #include #include -#include -#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/goal_union.cpp b/moveit_planners/ompl/ompl_interface/src/detail/goal_union.cpp index 3827c73be5..ff831846c5 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/goal_union.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/goal_union.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include namespace diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp index a6bb82bde0..155531c44f 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp b/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp index cb938df902..e664d60ba9 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/projection_evaluators.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index ae626d0cb5..fbd941308c 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Jeroen De Maeyer */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp index 7d88e8bdb8..bbe598a618 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/threadsafe_state_storage.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include ompl_interface::TSStateStorage::TSStateStorage(const moveit::core::RobotModelPtr& robot_model) : start_state_(robot_model) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index dca38f7da8..ebe1f0dc31 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -39,17 +39,17 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include -#include +#include -#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index cc34c91f85..a52e381080 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 7daef874b5..e4675820fc 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan, Dave Coleman */ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp index f00efdc5f5..ecb54eee3c 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space.cpp @@ -34,7 +34,7 @@ /* Author: Jeroen De Maeyer */ -#include +#include #include diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp index 7830f81c37..a762dd6b2a 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp @@ -35,8 +35,8 @@ /* Author: Jeroen De Maeyer */ /* Mostly copied from Ioan Sucan's code */ -#include -#include +#include +#include namespace ompl_interface { diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp index 8182da68f5..9ba095d420 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include const std::string ompl_interface::JointModelStateSpace::PARAMETERIZATION_TYPE = "JointModel"; diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index 85f54e8c3f..608f915283 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include ompl_interface::JointModelStateSpaceFactory::JointModelStateSpaceFactory() : ModelBasedStateSpaceFactory() { diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 4482e781d4..f0110fcc22 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp index 1a3158654b..5b9bcc678e 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include ompl_interface::ModelBasedStateSpacePtr ompl_interface::ModelBasedStateSpaceFactory::getNewStateSpace(const ModelBasedStateSpaceSpecification& space_spec) const diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 600e5281de..ff569ab591 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp index 953b9382f3..b581c6d732 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include ompl_interface::PoseModelStateSpaceFactory::PoseModelStateSpaceFactory() : ModelBasedStateSpaceFactory() { diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index bedd813c48..8da1bb4c7f 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include #include @@ -71,12 +71,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include using namespace std::placeholders; diff --git a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h b/moveit_planners/ompl/ompl_interface/test/load_test_robot.hpp similarity index 96% rename from moveit_planners/ompl/ompl_interface/test/load_test_robot.h rename to moveit_planners/ompl/ompl_interface/test/load_test_robot.hpp index 6ae825d265..96d1aebb51 100644 --- a/moveit_planners/ompl/ompl_interface/test/load_test_robot.h +++ b/moveit_planners/ompl/ompl_interface/test/load_test_robot.hpp @@ -35,9 +35,9 @@ /* Author: Jeroen De Maeyer */ #pragma once -#include -#include -#include +#include +#include +#include #include namespace ompl_interface_testing @@ -61,7 +61,7 @@ namespace ompl_interface_testing * --- example.cpp --- * * #include - * #include "load_test_robot.h" + * #include "load_test_robot.hpp" * * class GenericTests : public ompl_interface_testing::LoadTestRobot, public testing::Test * { diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp index b411d7b630..558f03aba6 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp @@ -45,11 +45,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -57,7 +57,7 @@ #include #include -#include "load_test_robot.h" +#include "load_test_robot.hpp" rclcpp::Logger getLogger() { diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp index bdb5676551..7a243fce0f 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp @@ -35,17 +35,17 @@ /* Author: Jeroen De Maeyer */ -#include "load_test_robot.h" +#include "load_test_robot.hpp" #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp index aa06ac75d1..5ea926c3e5 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp @@ -41,7 +41,7 @@ * NOTE q = joint positions **/ -#include "load_test_robot.h" +#include "load_test_robot.hpp" #include #include @@ -50,11 +50,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index 16ecc3fc63..dfafe33f6f 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -50,20 +50,20 @@ * **/ -#include "load_test_robot.h" +#include "load_test_robot.hpp" #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include /** \brief Generic implementation of the tests that can be executed on different robots. **/ diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index 72568adc20..3089a31fbc 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -36,14 +36,14 @@ #include -#include -#include +#include +#include #include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp index dbf5a69749..b62f59bd05 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp @@ -48,17 +48,17 @@ * The test do show what is minimally required to create a working StateValidityChecker. **/ -#include "load_test_robot.h" +#include "load_test_robot.hpp" #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp index fd6711682d..6c642610d0 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp @@ -38,8 +38,8 @@ This test checks if a state can be set in TSSafeStateStorage and correctly retri The skeleton of this test was taken from test_state_validity_checker.cpp by Jeroen De Maeyer. */ -#include "load_test_robot.h" -#include +#include "load_test_robot.hpp" +#include #include /** \brief Generic implementation of the tests that can be executed on different robots. **/ diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits.h b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits.h new file mode 100644 index 0000000000..301e25fa8b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits.h @@ -0,0 +1,31 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.h b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.h new file mode 100644 index 0000000000..7918de9fbe --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits_copy/joint_limits_rosparam.h @@ -0,0 +1,31 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h index 61737ba275..2217fab1c2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,10 +45,5 @@ *********************************************************************/ #pragma once - -#include - -namespace pilz_industrial_motion_planner -{ -static const std::string SEQUENCE_SERVICE_NAME = "plan_sequence_path"; -} +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.hpp new file mode 100644 index 0000000000..61737ba275 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/capability_names.hpp @@ -0,0 +1,42 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +namespace pilz_industrial_motion_planner +{ +static const std::string SEQUENCE_SERVICE_NAME = "plan_sequence_path"; +} diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h index 9d7b70431c..7aab35f9d7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,19 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include "cartesian_trajectory_point.h" - -namespace pilz_industrial_motion_planner -{ -struct CartesianTrajectory -{ - std::string group_name; - std::string link_name; - std::vector points; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.hpp new file mode 100644 index 0000000000..34345d4b35 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory.hpp @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include "cartesian_trajectory_point.hpp" + +namespace pilz_industrial_motion_planner +{ +struct CartesianTrajectory +{ + std::string group_name; + std::string link_name; + std::vector points; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h index e810ae4726..0c9cb3bc0d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,20 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -struct CartesianTrajectoryPoint -{ - geometry_msgs::msg::Pose pose; - geometry_msgs::msg::Twist velocity; - geometry_msgs::msg::Twist acceleration; - rclcpp::Duration time_from_start{ 0, 0 }; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.hpp new file mode 100644 index 0000000000..e810ae4726 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.hpp @@ -0,0 +1,52 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +struct CartesianTrajectoryPoint +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist velocity; + geometry_msgs::msg::Twist acceleration; + rclcpp::Duration time_from_start{ 0, 0 }; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h index 9c19a0be04..a5beedcb6b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,204 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -using RobotTrajCont = std::vector; - -// List of exceptions which can be thrown by the CommandListManager class. -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, - moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, - moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, - moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); - -/** - * @brief This class orchestrates the planning of single commands and - * command lists. - */ -class CommandListManager -{ -public: - CommandListManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model); - - /** - * @brief Generates trajectories for the specified list of motion commands. - * - * The following rules apply: - * - If two consecutive trajectories are from the same group, they are - * simply attached to each other, given that the blend_radius is zero. - * - If two consecutive trajectories are from the same group, they are - * blended together, given that the blend_radius is GREATER than zero. - * - If two consecutive trajectories are from different groups, then - * the second trajectory is added as new element to the result container. - * All following trajectories are then attached to the new trajectory - * element (until all requests are processed or until the next group change). - * - * @param planning_scene The planning scene to be used for trajectory - * generation. - * @param req_list List of motion requests containing: PTP, LIN, CIRC - * and/or gripper commands. - * Please note: A request is only valid if: - * - All blending radii are non negative. - * - The blending radius of the last request is 0. - * - Only the first request of each group has a start state. - * - None of the blending radii overlap with each other. - * - * Please note: - * Starts states do not need to state the joints of all groups. - * It is sufficient if a start state states only the joints of the group - * which it belongs to. Starts states can even be incomplete. In this case - * default values are set for the unset joints. - * - * @return Contains the calculated/generated trajectories. - */ - RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_pipeline::PlanningPipelinePtr& planning_pipeline, - const moveit_msgs::msg::MotionSequenceRequest& req_list); - -private: - using MotionResponseCont = std::vector; - using RobotState_OptRef = const std::optional>; - using RadiiCont = std::vector; - using GroupNamesCont = std::vector; - -private: - /** - * @brief Validates that two consecutive blending radii do not overlap. - * - * @param motion_plan_responses Container of calculated/generated - * trajectories. - * @param radii Container stating the blend radii. - */ - void checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const; - - /** - * @brief Solve each sequence item individually. - * - * @param planning_scene The planning_scene to be used for trajectory - * generation. - * @param req_list Container of requests for calculation/generation. - * - * @return Container of generated trajectories. - */ - MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_pipeline::PlanningPipelinePtr& planning_pipeline, - const moveit_msgs::msg::MotionSequenceRequest& req_list) const; - - /** - * @return TRUE if the blending radii of specified trajectories overlap, - * otherwise FALSE. The functions returns FALSE if both trajectories are from - * different groups or if both trajectories are end-effector groups. - */ - bool checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, - const robot_trajectory::RobotTrajectory& traj_B, const double radii_B) const; - -private: - /** - * @return The last RobotState of the specified group which can - * be found in the specified vector. - */ - static RobotState_OptRef getPreviousEndState(const MotionResponseCont& motion_plan_responses, - const std::string& group_name); - - /** - * @brief Set start state to end state of previous calculated trajectory - * from group. - */ - static void setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, - moveit_msgs::msg::RobotState& start_state); - - /** - * @return Container of radii extracted from the specified request list. - * - * Please note: - * This functions sets invalid blend radii to zero. Invalid blend radii are: - * - blend radii between end-effectors and - * - blend raddi between different groups. - */ - static RadiiCont extractBlendRadii(const moveit::core::RobotModel& model, - const moveit_msgs::msg::MotionSequenceRequest& req_list); - - /** - * @return True in case of an invalid blend radii between specified - * command A and B, otherwise False. Invalid blend radii are: - * - blend radii between end-effectors and - * - blend raddi between different groups. - */ - static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, - const moveit_msgs::msg::MotionSequenceItem& item_A, - const moveit_msgs::msg::MotionSequenceItem& item_B); - - /** - * @brief Checks that all blend radii are greater or equal to zero. - */ - static void checkForNegativeRadii(const moveit_msgs::msg::MotionSequenceRequest& req_list); - - /** - * @brief Checks that last blend radius is zero. - */ - static void checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list); - - /** - * @brief Checks that only the first request of the specified group has - * a start state in the specified request list. - */ - static void checkStartStatesOfGroup(const moveit_msgs::msg::MotionSequenceRequest& req_list, - const std::string& group_name); - - /** - * @brief Checks that each group in the specified request list has only - * one start state. - */ - static void checkStartStates(const moveit_msgs::msg::MotionSequenceRequest& req_list); - - /** - * @return Returns all group names which are present in the specified - * request. - */ - static GroupNamesCont getGroupNames(const moveit_msgs::msg::MotionSequenceRequest& req_list); - -private: - //! Node - const rclcpp::Node::SharedPtr node_; - - //! Robot model - moveit::core::RobotModelConstPtr model_; - - //! @brief Builder to construct the container containing the final - //! trajectories. - PlanComponentsBuilder plan_comp_builder_; - - std::shared_ptr param_listener_; - cartesian_limits::Params params_; -}; - -inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list) -{ - if (req_list.items.back().blend_radius != 0.0) - { - throw LastBlendRadiusNotZeroException("The last blending radius must be zero"); - } -} - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.hpp new file mode 100644 index 0000000000..a3a64b6dfe --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.hpp @@ -0,0 +1,236 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +using RobotTrajCont = std::vector; + +// List of exceptions which can be thrown by the CommandListManager class. +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); + +/** + * @brief This class orchestrates the planning of single commands and + * command lists. + */ +class CommandListManager +{ +public: + CommandListManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Generates trajectories for the specified list of motion commands. + * + * The following rules apply: + * - If two consecutive trajectories are from the same group, they are + * simply attached to each other, given that the blend_radius is zero. + * - If two consecutive trajectories are from the same group, they are + * blended together, given that the blend_radius is GREATER than zero. + * - If two consecutive trajectories are from different groups, then + * the second trajectory is added as new element to the result container. + * All following trajectories are then attached to the new trajectory + * element (until all requests are processed or until the next group change). + * + * @param planning_scene The planning scene to be used for trajectory + * generation. + * @param req_list List of motion requests containing: PTP, LIN, CIRC + * and/or gripper commands. + * Please note: A request is only valid if: + * - All blending radii are non negative. + * - The blending radius of the last request is 0. + * - Only the first request of each group has a start state. + * - None of the blending radii overlap with each other. + * + * Please note: + * Starts states do not need to state the joints of all groups. + * It is sufficient if a start state states only the joints of the group + * which it belongs to. Starts states can even be incomplete. In this case + * default values are set for the unset joints. + * + * @return Contains the calculated/generated trajectories. + */ + RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::msg::MotionSequenceRequest& req_list); + +private: + using MotionResponseCont = std::vector; + using RobotState_OptRef = const std::optional>; + using RadiiCont = std::vector; + using GroupNamesCont = std::vector; + +private: + /** + * @brief Validates that two consecutive blending radii do not overlap. + * + * @param motion_plan_responses Container of calculated/generated + * trajectories. + * @param radii Container stating the blend radii. + */ + void checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const; + + /** + * @brief Solve each sequence item individually. + * + * @param planning_scene The planning_scene to be used for trajectory + * generation. + * @param req_list Container of requests for calculation/generation. + * + * @return Container of generated trajectories. + */ + MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_pipeline::PlanningPipelinePtr& planning_pipeline, + const moveit_msgs::msg::MotionSequenceRequest& req_list) const; + + /** + * @return TRUE if the blending radii of specified trajectories overlap, + * otherwise FALSE. The functions returns FALSE if both trajectories are from + * different groups or if both trajectories are end-effector groups. + */ + bool checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, + const robot_trajectory::RobotTrajectory& traj_B, const double radii_B) const; + +private: + /** + * @return The last RobotState of the specified group which can + * be found in the specified vector. + */ + static RobotState_OptRef getPreviousEndState(const MotionResponseCont& motion_plan_responses, + const std::string& group_name); + + /** + * @brief Set start state to end state of previous calculated trajectory + * from group. + */ + static void setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, + moveit_msgs::msg::RobotState& start_state); + + /** + * @return Container of radii extracted from the specified request list. + * + * Please note: + * This functions sets invalid blend radii to zero. Invalid blend radii are: + * - blend radii between end-effectors and + * - blend raddi between different groups. + */ + static RadiiCont extractBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::msg::MotionSequenceRequest& req_list); + + /** + * @return True in case of an invalid blend radii between specified + * command A and B, otherwise False. Invalid blend radii are: + * - blend radii between end-effectors and + * - blend raddi between different groups. + */ + static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, + const moveit_msgs::msg::MotionSequenceItem& item_A, + const moveit_msgs::msg::MotionSequenceItem& item_B); + + /** + * @brief Checks that all blend radii are greater or equal to zero. + */ + static void checkForNegativeRadii(const moveit_msgs::msg::MotionSequenceRequest& req_list); + + /** + * @brief Checks that last blend radius is zero. + */ + static void checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list); + + /** + * @brief Checks that only the first request of the specified group has + * a start state in the specified request list. + */ + static void checkStartStatesOfGroup(const moveit_msgs::msg::MotionSequenceRequest& req_list, + const std::string& group_name); + + /** + * @brief Checks that each group in the specified request list has only + * one start state. + */ + static void checkStartStates(const moveit_msgs::msg::MotionSequenceRequest& req_list); + + /** + * @return Returns all group names which are present in the specified + * request. + */ + static GroupNamesCont getGroupNames(const moveit_msgs::msg::MotionSequenceRequest& req_list); + +private: + //! Node + const rclcpp::Node::SharedPtr node_; + + //! Robot model + moveit::core::RobotModelConstPtr model_; + + //! @brief Builder to construct the container containing the final + //! trajectories. + PlanComponentsBuilder plan_comp_builder_; + + std::shared_ptr param_listener_; + cartesian_limits::Params params_; +}; + +inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list) +{ + if (req_list.items.back().blend_radius != 0.0) + { + throw LastBlendRadiusNotZeroException("The last blending radius must be zero"); + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h index 87b6ac8223..30be1fcef5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,119 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include - -#include -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Unifies the joint limits from the given joint models with joint - * limits from the node parameters. - * - * Does not support MultiDOF joints. - */ -class JointLimitsAggregator -{ -public: - /** - * @brief Aggregates(combines) the joint limits from joint model and - * node parameters. - * The rules for the combination are: - * 1. Position and velocity limits in the node parameters must be stricter - * or equal if they are defined. - * 2. Limits in the node parameters where the corresponding - * has__limits are „false“ - * are considered undefined(see point 1). - * 3. Not all joints have to be limited by the node parameters. Selective - * limitation is possible. - * 4. If max_deceleration is unset, it will be set to: max_deceleration = - - * max_acceleration. - * @note The acceleration/deceleration can only be set via the node parameters parameter - * since they are not supported - * in the urdf so far. - * @param node Node to use for accessing joint limit parameters - * @param param_namespace Namespace to use for looking up node parameters - * @param joint_models The joint models - * @return Container containing the limits - */ - static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr& node, - const std::string& param_namespace, - const std::vector& joint_models); - -protected: - /** - * @brief Update the position limits with the ones from the joint_model. - * - * If the joint model has no position limit, the value is unchanged. - * - * @param joint_model The joint model - * @param joint_limit The joint_limit to be filled with new values. - */ - static void updatePositionLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); - - /** - * @brief Update the velocity limit with the one from the joint_model. - * - * If the joint model has no velocity limit, the value is unchanged. - * - * @param joint_model The joint model - * @param joint_limit The joint_limit to be filled with new values. - */ - static void updateVelocityLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); - - /** - * @brief Checks if the position limits from the given joint_limit are - * stricter than the limits of the joint_model. - * Throws AggregationBoundsViolationException on violation - * @param joint_model The joint_model - * @param joint_limit The joint_limit - */ - static void checkPositionBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); - - /** - * @brief Checks if the velocity limit from the given joint_limit are stricter - * than the limit of the joint_model. - * Throws AggregationBoundsViolationException on violation - * @param joint_model The joint_model - * @param joint_limit The joint_limit - */ - static void checkVelocityBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); -}; - -/** - * @class AggregationException - * @brief A base class for all aggregation exceptions inheriting from - * std::runtime_exception - */ -class AggregationException : public std::runtime_error -{ -public: - AggregationException(const std::string& error_desc) : std::runtime_error(error_desc) - { - } -}; - -/** - * @class AggregationJointMissingException - * @brief Thrown the limits from the node parameter are weaker(forbidden) than - * the ones defined in the urdf - * - */ -class AggregationBoundsViolationException : public AggregationException -{ -public: - AggregationBoundsViolationException(const std::string& error_desc) : AggregationException(error_desc) - { - } -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.hpp new file mode 100644 index 0000000000..da245238cd --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.hpp @@ -0,0 +1,151 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Unifies the joint limits from the given joint models with joint + * limits from the node parameters. + * + * Does not support MultiDOF joints. + */ +class JointLimitsAggregator +{ +public: + /** + * @brief Aggregates(combines) the joint limits from joint model and + * node parameters. + * The rules for the combination are: + * 1. Position and velocity limits in the node parameters must be stricter + * or equal if they are defined. + * 2. Limits in the node parameters where the corresponding + * has__limits are „false“ + * are considered undefined(see point 1). + * 3. Not all joints have to be limited by the node parameters. Selective + * limitation is possible. + * 4. If max_deceleration is unset, it will be set to: max_deceleration = - + * max_acceleration. + * @note The acceleration/deceleration can only be set via the node parameters parameter + * since they are not supported + * in the urdf so far. + * @param node Node to use for accessing joint limit parameters + * @param param_namespace Namespace to use for looking up node parameters + * @param joint_models The joint models + * @return Container containing the limits + */ + static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr& node, + const std::string& param_namespace, + const std::vector& joint_models); + +protected: + /** + * @brief Update the position limits with the ones from the joint_model. + * + * If the joint model has no position limit, the value is unchanged. + * + * @param joint_model The joint model + * @param joint_limit The joint_limit to be filled with new values. + */ + static void updatePositionLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); + + /** + * @brief Update the velocity limit with the one from the joint_model. + * + * If the joint model has no velocity limit, the value is unchanged. + * + * @param joint_model The joint model + * @param joint_limit The joint_limit to be filled with new values. + */ + static void updateVelocityLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit); + + /** + * @brief Checks if the position limits from the given joint_limit are + * stricter than the limits of the joint_model. + * Throws AggregationBoundsViolationException on violation + * @param joint_model The joint_model + * @param joint_limit The joint_limit + */ + static void checkPositionBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); + + /** + * @brief Checks if the velocity limit from the given joint_limit are stricter + * than the limit of the joint_model. + * Throws AggregationBoundsViolationException on violation + * @param joint_model The joint_model + * @param joint_limit The joint_limit + */ + static void checkVelocityBoundsThrowing(const moveit::core::JointModel* joint_model, const JointLimit& joint_limit); +}; + +/** + * @class AggregationException + * @brief A base class for all aggregation exceptions inheriting from + * std::runtime_exception + */ +class AggregationException : public std::runtime_error +{ +public: + AggregationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class AggregationJointMissingException + * @brief Thrown the limits from the node parameter are weaker(forbidden) than + * the ones defined in the urdf + * + */ +class AggregationBoundsViolationException : public AggregationException +{ +public: + AggregationBoundsViolationException(const std::string& error_desc) : AggregationException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h index fb5e47bdab..03dd00ab22 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,139 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Container for JointLimits, essentially a map with convenience - * functions. - * Adds the ability to as for limits and get a common limit that unifies all - * given limits - */ -class JointLimitsContainer -{ -public: - /** - * @brief Add a limit - * @param joint_name Name if the joint this limit belongs to - * @param joint_limit Limit of the joint - * @return true if the limit was added, false - * if joint_limit.has_deceleration_limit && - * joint_limit.max_deceleration >= 0 - */ - bool addLimit(const std::string& joint_name, JointLimit joint_limit); - - /** - * @brief Check if there is a limit for a joint with the given name in this - * container - * @param joint_name Name of the joint - */ - bool hasLimit(const std::string& joint_name) const; - - /** - * @brief Get Number of limits in the container - * @return Number of limits in the container - */ - size_t getCount() const; - - /** - * @brief Returns whether the container is empty - * @return true if empty, false otherwise - */ - bool empty() const; - - /** - * @brief Returns joint limit fusion of all(position, velocity, acceleration, - * deceleration) limits for all joint. - * There are cases where the most strict limit of all limits is needed. - * If there are no matching limits, the flag - * has_[position|velocity|...]_limits is set to false. - * - * @return joint limit - */ - JointLimit getCommonLimit() const; - - /** - * @brief Returns joint limit fusion of all(position, velocity, acceleration, - * deceleration) limits for given joints. - * There are cases where the most strict limit of all limits is needed. - * If there are no matching limits, the flag - * has_[position|velocity|...]_limits is set to false. - * - * @param joint_names - * @return joint limit - * @throws std::out_of_range if a joint limit with this name does not exist - */ - JointLimit getCommonLimit(const std::vector& joint_names) const; - - /** - * @brief getLimit get the limit for the given joint name - * @param joint_name - * @return joint limit - * @throws std::out_of_range if a joint limit with this name does not exist - */ - JointLimit getLimit(const std::string& joint_name) const; - - /** - * @brief ConstIterator to the underlying data structure - * @return - */ - std::map::const_iterator begin() const; - - /** - * @brief ConstIterator to the underlying data structure - * @return - */ - std::map::const_iterator end() const; - - /** - * @brief verify position limit of single joint - * @param joint_name - * @param joint_position - * @return true if within limits, false otherwise - */ - bool verifyPositionLimit(const std::string& joint_name, double joint_position) const; - - /** - * @brief verify velocity limit of single joint - * @param joint_name - * @param joint_velocity - * @return true if within limits, false otherwise - */ - bool verifyVelocityLimit(const std::string& joint_name, double joint_velocity) const; - - /** - * @brief verify acceleration limit of single joint - * @param joint_name - * @param joint_acceleration - * @return true if within limits, false otherwise - */ - bool verifyAccelerationLimit(const std::string& joint_name, double joint_acceleration) const; - - /** - * @brief verify deceleration limit of single joint - * @param joint_name - * @param joint_acceleration - * @return true if within limits, false otherwise - */ - bool verifyDecelerationLimit(const std::string& joint_name, double joint_acceleration) const; - -private: - /** - * @brief update the most strict limit with given joint limit - * @param joint_limit - * @param common_limit the current most strict limit - */ - static void updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit); - -protected: - /// Actual container object containing the data - std::map container_; -}; -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.hpp new file mode 100644 index 0000000000..912015c832 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.hpp @@ -0,0 +1,171 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Container for JointLimits, essentially a map with convenience + * functions. + * Adds the ability to as for limits and get a common limit that unifies all + * given limits + */ +class JointLimitsContainer +{ +public: + /** + * @brief Add a limit + * @param joint_name Name if the joint this limit belongs to + * @param joint_limit Limit of the joint + * @return true if the limit was added, false + * if joint_limit.has_deceleration_limit && + * joint_limit.max_deceleration >= 0 + */ + bool addLimit(const std::string& joint_name, JointLimit joint_limit); + + /** + * @brief Check if there is a limit for a joint with the given name in this + * container + * @param joint_name Name of the joint + */ + bool hasLimit(const std::string& joint_name) const; + + /** + * @brief Get Number of limits in the container + * @return Number of limits in the container + */ + size_t getCount() const; + + /** + * @brief Returns whether the container is empty + * @return true if empty, false otherwise + */ + bool empty() const; + + /** + * @brief Returns joint limit fusion of all(position, velocity, acceleration, + * deceleration) limits for all joint. + * There are cases where the most strict limit of all limits is needed. + * If there are no matching limits, the flag + * has_[position|velocity|...]_limits is set to false. + * + * @return joint limit + */ + JointLimit getCommonLimit() const; + + /** + * @brief Returns joint limit fusion of all(position, velocity, acceleration, + * deceleration) limits for given joints. + * There are cases where the most strict limit of all limits is needed. + * If there are no matching limits, the flag + * has_[position|velocity|...]_limits is set to false. + * + * @param joint_names + * @return joint limit + * @throws std::out_of_range if a joint limit with this name does not exist + */ + JointLimit getCommonLimit(const std::vector& joint_names) const; + + /** + * @brief getLimit get the limit for the given joint name + * @param joint_name + * @return joint limit + * @throws std::out_of_range if a joint limit with this name does not exist + */ + JointLimit getLimit(const std::string& joint_name) const; + + /** + * @brief ConstIterator to the underlying data structure + * @return + */ + std::map::const_iterator begin() const; + + /** + * @brief ConstIterator to the underlying data structure + * @return + */ + std::map::const_iterator end() const; + + /** + * @brief verify position limit of single joint + * @param joint_name + * @param joint_position + * @return true if within limits, false otherwise + */ + bool verifyPositionLimit(const std::string& joint_name, double joint_position) const; + + /** + * @brief verify velocity limit of single joint + * @param joint_name + * @param joint_velocity + * @return true if within limits, false otherwise + */ + bool verifyVelocityLimit(const std::string& joint_name, double joint_velocity) const; + + /** + * @brief verify acceleration limit of single joint + * @param joint_name + * @param joint_acceleration + * @return true if within limits, false otherwise + */ + bool verifyAccelerationLimit(const std::string& joint_name, double joint_acceleration) const; + + /** + * @brief verify deceleration limit of single joint + * @param joint_name + * @param joint_acceleration + * @return true if within limits, false otherwise + */ + bool verifyDecelerationLimit(const std::string& joint_name, double joint_acceleration) const; + +private: + /** + * @brief update the most strict limit with given joint limit + * @param joint_limit + * @param common_limit the current most strict limit + */ + static void updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit); + +protected: + /// Actual container object containing the data + std::map container_; +}; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h index 0f21fb437b..8e46127033 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,35 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -namespace joint_limits_interface -{ -/** - * @brief Extends joint_limits_interface::JointLimits with a deceleration - * parameter - */ -struct JointLimits : joint_limits::JointLimits -{ - JointLimits() : max_deceleration(0.0), has_deceleration_limits(false) - { - } - - /// maximum deceleration MUST(!) be negative - double max_deceleration; - - bool has_deceleration_limits; -}; -} // namespace joint_limits_interface - -typedef joint_limits_interface::JointLimits JointLimit; -typedef std::map JointLimitsMap; -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.hpp new file mode 100644 index 0000000000..0f21fb437b --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @brief Extends joint_limits_interface::JointLimits with a deceleration + * parameter + */ +struct JointLimits : joint_limits::JointLimits +{ + JointLimits() : max_deceleration(0.0), has_deceleration_limits(false) + { + } + + /// maximum deceleration MUST(!) be negative + double max_deceleration; + + bool has_deceleration_limits; +}; +} // namespace joint_limits_interface + +typedef joint_limits_interface::JointLimits JointLimit; +typedef std::map JointLimitsMap; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h index d73caaed60..5f8fd673af 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,68 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -namespace joint_limits_interface -{ -/** - * @see joint_limits::declareParameters(...) - */ -inline bool declareParameters(const std::string& joint_name, const std::string& param_ns, - const rclcpp::Node::SharedPtr& node) -{ - return joint_limits::declareParameters(joint_name, node, param_ns); -} -/** - * @see joint_limits::getJointLimits(...) - */ -inline bool getJointLimits(const std::string& joint_name, const std::string& param_ns, - const rclcpp::Node::SharedPtr& node, joint_limits_interface::JointLimits& limits) -{ - // Load the existing limits - if (!joint_limits::getJointLimits(joint_name, node, param_ns, limits)) - { - return false; // LCOV_EXCL_LINE // The case where getJointLimits returns - // false is covered above. - } - try - { - // Deceleration limits - const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; - if (node->has_parameter(param_base_name + ".has_deceleration_limits")) - { - limits.has_deceleration_limits = node->get_parameter(param_base_name + ".has_deceleration_limits").as_bool(); - } - else - { - limits.has_deceleration_limits = node->declare_parameter(param_base_name + ".has_deceleration_limits", false); - } - if (limits.has_deceleration_limits) - { - if (node->has_parameter(param_base_name + ".max_deceleration")) - { - limits.max_deceleration = node->get_parameter(param_base_name + ".max_deceleration").as_double(); - } - else - { - limits.max_deceleration = - node->declare_parameter(param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); - } - } - } - catch (const std::exception& ex) - { - RCLCPP_WARN_STREAM(node->get_logger(), "Failed loading deceleration limits"); - limits.has_deceleration_limits = false; - } - - return true; -} -} // namespace joint_limits_interface -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.hpp new file mode 100644 index 0000000000..ab3d1dea7d --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_interface_extension.hpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +namespace joint_limits_interface +{ +/** + * @see joint_limits::declareParameters(...) + */ +inline bool declareParameters(const std::string& joint_name, const std::string& param_ns, + const rclcpp::Node::SharedPtr& node) +{ + return joint_limits::declareParameters(joint_name, node, param_ns); +} +/** + * @see joint_limits::getJointLimits(...) + */ +inline bool getJointLimits(const std::string& joint_name, const std::string& param_ns, + const rclcpp::Node::SharedPtr& node, joint_limits_interface::JointLimits& limits) +{ + // Load the existing limits + if (!joint_limits::getJointLimits(joint_name, node, param_ns, limits)) + { + return false; // LCOV_EXCL_LINE // The case where getJointLimits returns + // false is covered above. + } + try + { + // Deceleration limits + const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name; + if (node->has_parameter(param_base_name + ".has_deceleration_limits")) + { + limits.has_deceleration_limits = node->get_parameter(param_base_name + ".has_deceleration_limits").as_bool(); + } + else + { + limits.has_deceleration_limits = node->declare_parameter(param_base_name + ".has_deceleration_limits", false); + } + if (limits.has_deceleration_limits) + { + if (node->has_parameter(param_base_name + ".max_deceleration")) + { + limits.max_deceleration = node->get_parameter(param_base_name + ".max_deceleration").as_double(); + } + else + { + limits.max_deceleration = + node->declare_parameter(param_base_name + ".max_deceleration", std::numeric_limits::quiet_NaN()); + } + } + } + catch (const std::exception& ex) + { + RCLCPP_WARN_STREAM(node->get_logger(), "Failed loading deceleration limits"); + limits.has_deceleration_limits = false; + } + + return true; +} +} // namespace joint_limits_interface +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h index 1911cf3943..5e63f6ddb9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,121 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Validates the equality of all limits inside a container - */ -class JointLimitsValidator -{ -public: - /** - * @brief Validates that the position limits of all limits are equal - * @param joint_limits the joint limits - * @return true if all are equal - * @note always returns true if has_position_limits=false for all limits, or - * if the size of joint_limits is 0 or 1 - */ - static bool validateAllPositionLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); - - /** - * @brief Validates that the velocity of all limits is equal - * @param joint_limits the joint limits - * @return true if all are equal - * @note always returns true if has_velocity_limits=false for all limits, or - * if the size of joint_limits is 0 or 1 - */ - static bool validateAllVelocityLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); - - /** - * @brief Validates that the acceleration of all limits is equal - * @param joint_limits the joint limits - * @return true if all are equal - * @note always returns true if has_acceleration_limits=false for all limits, - * or if size of joint_limits is 0 or 1 - */ - static bool - validateAllAccelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); - - /** - * @brief Validates that the deceleration of all limits is equal - * @param joint_limits the joint limits - * @return true if all are equal - * @note always returns true if has_acceleration_limits=false for all limits, - * or if size of joint_limits is 0 or 1 - */ - static bool - validateAllDecelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); - -private: - static bool validateWithEqualFunc(bool (*eq_func)(const JointLimit&, const JointLimit&), - const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); - - static bool positionEqual(const JointLimit& lhs, const JointLimit& rhs); - - static bool velocityEqual(const JointLimit& lhs, const JointLimit& rhs); - - static bool accelerationEqual(const JointLimit& lhs, const JointLimit& rhs); - - static bool decelerationEqual(const JointLimit& lhs, const JointLimit& rhs); -}; - -/** - * @class ValidationException - * @brief A base class for all validations exceptions inheriting from - * std::runtime_exception - */ -class ValidationException : public std::runtime_error -{ -public: - ValidationException(const std::string& error_desc) : std::runtime_error(error_desc) - { - } -}; - -/** - * @class ValidationJointMissingException - * @brief Thrown the limits for a joint are defined in the urdf but not in the - * node parameters (loaded from yaml) - * - */ -class ValidationJointMissingException : public ValidationException -{ -public: - ValidationJointMissingException(const std::string& error_desc) : ValidationException(error_desc) - { - } -}; - -/** - * @class ValidationDifferentLimitsException - * @brief Thrown when the limits differ - * - */ -class ValidationDifferentLimitsException : public ValidationException -{ -public: - ValidationDifferentLimitsException(const std::string& error_desc) : ValidationException(error_desc) - { - } -}; - -/** - * @class ValidationBoundsViolationException - * @brief Thrown when the limits from the param server are weaker than the ones - * obtained from the urdf - * - */ -class ValidationBoundsViolationException : public ValidationException -{ -public: - ValidationBoundsViolationException(const std::string& error_desc) : ValidationException(error_desc) - { - } -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.hpp new file mode 100644 index 0000000000..0a737a48e9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_validator.hpp @@ -0,0 +1,153 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Validates the equality of all limits inside a container + */ +class JointLimitsValidator +{ +public: + /** + * @brief Validates that the position limits of all limits are equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_position_limits=false for all limits, or + * if the size of joint_limits is 0 or 1 + */ + static bool validateAllPositionLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the velocity of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_velocity_limits=false for all limits, or + * if the size of joint_limits is 0 or 1 + */ + static bool validateAllVelocityLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the acceleration of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_acceleration_limits=false for all limits, + * or if size of joint_limits is 0 or 1 + */ + static bool + validateAllAccelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + /** + * @brief Validates that the deceleration of all limits is equal + * @param joint_limits the joint limits + * @return true if all are equal + * @note always returns true if has_acceleration_limits=false for all limits, + * or if size of joint_limits is 0 or 1 + */ + static bool + validateAllDecelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + +private: + static bool validateWithEqualFunc(bool (*eq_func)(const JointLimit&, const JointLimit&), + const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits); + + static bool positionEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool velocityEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool accelerationEqual(const JointLimit& lhs, const JointLimit& rhs); + + static bool decelerationEqual(const JointLimit& lhs, const JointLimit& rhs); +}; + +/** + * @class ValidationException + * @brief A base class for all validations exceptions inheriting from + * std::runtime_exception + */ +class ValidationException : public std::runtime_error +{ +public: + ValidationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class ValidationJointMissingException + * @brief Thrown the limits for a joint are defined in the urdf but not in the + * node parameters (loaded from yaml) + * + */ +class ValidationJointMissingException : public ValidationException +{ +public: + ValidationJointMissingException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +/** + * @class ValidationDifferentLimitsException + * @brief Thrown when the limits differ + * + */ +class ValidationDifferentLimitsException : public ValidationException +{ +public: + ValidationDifferentLimitsException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +/** + * @class ValidationBoundsViolationException + * @brief Thrown when the limits from the param server are weaker than the ones + * obtained from the urdf + * + */ +class ValidationBoundsViolationException : public ValidationException +{ +public: + ValidationBoundsViolationException(const std::string& error_desc) : ValidationException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h index 5cdf967a63..5a038a1d45 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,70 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include "cartesian_limits_parameters.hpp" - -namespace pilz_industrial_motion_planner -{ -/** - * @brief This class combines CartesianLimit and JointLimits into on single - * class. - */ -class LimitsContainer -{ -public: - LimitsContainer(); - - /** - * @brief Return if this LimitsContainer has defined joint limits - * @return True if container contains joint limits - */ - bool hasJointLimits() const; - - /** - * @brief Set joint limits - * @param joint_limits - */ - void setJointLimits(JointLimitsContainer& joint_limits); - - /** - * @brief Obtain the Joint Limits from the container - * @return the joint limits - */ - const JointLimitsContainer& getJointLimitContainer() const; - - /** - * @brief Prints the cartesian limits set by user. Default values for limits are 0.0 - */ - void printCartesianLimits() const; - - /** - * @brief Set cartesian limits - * @param cartesian_limit - */ - void setCartesianLimits(cartesian_limits::Params& cartesian_limit); - - /** - * @brief Return the cartesian limits - * @return the cartesian limits - */ - const cartesian_limits::Params& getCartesianLimits() const; - -private: - /// Flag if joint limits where set - bool has_joint_limits_; - - /// The joint limits - JointLimitsContainer joint_limits_; - - /// Flag if cartesian limits have been set - bool has_cartesian_limits_; - - /// The cartesian limits - cartesian_limits::Params cartesian_limits_; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.hpp new file mode 100644 index 0000000000..e26a52e276 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.hpp @@ -0,0 +1,102 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include "cartesian_limits_parameters.hpp" + +namespace pilz_industrial_motion_planner +{ +/** + * @brief This class combines CartesianLimit and JointLimits into on single + * class. + */ +class LimitsContainer +{ +public: + LimitsContainer(); + + /** + * @brief Return if this LimitsContainer has defined joint limits + * @return True if container contains joint limits + */ + bool hasJointLimits() const; + + /** + * @brief Set joint limits + * @param joint_limits + */ + void setJointLimits(JointLimitsContainer& joint_limits); + + /** + * @brief Obtain the Joint Limits from the container + * @return the joint limits + */ + const JointLimitsContainer& getJointLimitContainer() const; + + /** + * @brief Prints the cartesian limits set by user. Default values for limits are 0.0 + */ + void printCartesianLimits() const; + + /** + * @brief Set cartesian limits + * @param cartesian_limit + */ + void setCartesianLimits(cartesian_limits::Params& cartesian_limit); + + /** + * @brief Return the cartesian limits + * @return the cartesian limits + */ + const cartesian_limits::Params& getCartesianLimits() const; + +private: + /// Flag if joint limits where set + bool has_joint_limits_; + + /// The joint limits + JointLimitsContainer joint_limits_; + + /// Flag if cartesian limits have been set + bool has_cartesian_limits_; + + /// The cartesian limits + cartesian_limits::Params cartesian_limits_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h index c52af305a5..b016a59076 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,62 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -class CommandListManager; -using MoveGroupSequenceGoalHandle = rclcpp_action::ServerGoalHandle; - -/** - * @brief Provide action to handle multiple trajectories and execute the result - * in the form of a MoveGroup capability (plugin). - */ -class MoveGroupSequenceAction : public move_group::MoveGroupCapability -{ -public: - MoveGroupSequenceAction(); - - void initialize() override; - -private: - using ExecutableTrajs = std::vector; - - using StartStateMsg = moveit_msgs::msg::MotionSequenceResponse::_sequence_start_type; - using StartStatesMsg = std::vector; - using PlannedTrajMsgs = moveit_msgs::msg::MotionSequenceResponse::_planned_trajectories_type; - -private: - void executeSequenceCallback(const std::shared_ptr& goal_handle); - void - executeSequenceCallbackPlanAndExecute(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, - const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res); - void executeMoveCallbackPlanOnly(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, - const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& res); - void startMoveExecutionCallback(); - void startMoveLookCallback(); - void preemptMoveCallback(); - void setMoveState(move_group::MoveGroupState state); - bool planUsingSequenceManager(const moveit_msgs::msg::MotionSequenceRequest& req, - plan_execution::ExecutableMotionPlan& plan); - -private: - static void convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& startStatesMsg, - PlannedTrajMsgs& plannedTrajsMsgs); - -private: - rclcpp::CallbackGroup::SharedPtr action_callback_group_; - std::shared_ptr> move_action_server_; - std::shared_ptr goal_handle_; - moveit_msgs::action::MoveGroupSequence::Feedback::SharedPtr move_feedback_; - - move_group::MoveGroupState move_state_{ move_group::IDLE }; - std::unique_ptr command_list_manager_; -}; -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.hpp new file mode 100644 index 0000000000..f6a32d1719 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.hpp @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +class CommandListManager; +using MoveGroupSequenceGoalHandle = rclcpp_action::ServerGoalHandle; + +/** + * @brief Provide action to handle multiple trajectories and execute the result + * in the form of a MoveGroup capability (plugin). + */ +class MoveGroupSequenceAction : public move_group::MoveGroupCapability +{ +public: + MoveGroupSequenceAction(); + + void initialize() override; + +private: + using ExecutableTrajs = std::vector; + + using StartStateMsg = moveit_msgs::msg::MotionSequenceResponse::_sequence_start_type; + using StartStatesMsg = std::vector; + using PlannedTrajMsgs = moveit_msgs::msg::MotionSequenceResponse::_planned_trajectories_type; + +private: + void executeSequenceCallback(const std::shared_ptr& goal_handle); + void + executeSequenceCallbackPlanAndExecute(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, + const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res); + void executeMoveCallbackPlanOnly(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, + const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& res); + void startMoveExecutionCallback(); + void startMoveLookCallback(); + void preemptMoveCallback(); + void setMoveState(move_group::MoveGroupState state); + bool planUsingSequenceManager(const moveit_msgs::msg::MotionSequenceRequest& req, + plan_execution::ExecutableMotionPlan& plan); + +private: + static void convertToMsg(const ExecutableTrajs& trajs, StartStatesMsg& startStatesMsg, + PlannedTrajMsgs& plannedTrajsMsgs); + +private: + rclcpp::CallbackGroup::SharedPtr action_callback_group_; + std::shared_ptr> move_action_server_; + std::shared_ptr goal_handle_; + moveit_msgs::action::MoveGroupSequence::Feedback::SharedPtr move_feedback_; + + move_group::MoveGroupState move_state_{ move_group::IDLE }; + std::unique_ptr command_list_manager_; +}; +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h index 19e4d1ad09..285dd92105 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,37 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -// Forward declarations -class CommandListManager; - -/** - * @brief Provide service to blend multiple trajectories in the form of a - * MoveGroup capability (plugin). - */ -class MoveGroupSequenceService : public move_group::MoveGroupCapability -{ -public: - MoveGroupSequenceService(); - ~MoveGroupSequenceService() override; - - void initialize() override; - -private: - bool plan(const moveit_msgs::srv::GetMotionSequence::Request::SharedPtr& req, - const moveit_msgs::srv::GetMotionSequence::Response::SharedPtr& res); - -private: - rclcpp::Service::SharedPtr sequence_service_; - std::unique_ptr command_list_manager_; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.hpp new file mode 100644 index 0000000000..00d02b1333 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_service.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +// Forward declarations +class CommandListManager; + +/** + * @brief Provide service to blend multiple trajectories in the form of a + * MoveGroup capability (plugin). + */ +class MoveGroupSequenceService : public move_group::MoveGroupCapability +{ +public: + MoveGroupSequenceService(); + ~MoveGroupSequenceService() override; + + void initialize() override; + +private: + bool plan(const moveit_msgs::srv::GetMotionSequence::Request::SharedPtr& req, + const moveit_msgs::srv::GetMotionSequence::Response::SharedPtr& res); + +private: + rclcpp::Service::SharedPtr sequence_service_; + std::unique_ptr command_list_manager_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h index 25b2b018f9..6004eec016 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,71 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Generator class for KDL::Path_Circle from different circle - * representations - */ -class PathCircleGenerator -{ -public: - /** - * @brief set the path circle from start, goal and center point - * - * Note that a half circle should use interim point and cannot be defined - * by circle center since start/goal/center points are colinear. - * @throws KDL::Error_MotionPlanning in case start and goal have different - * radii to the center point. - */ - static std::unique_ptr circleFromCenter(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, - const KDL::Vector& center_point, double eqradius); - - /** - * @brief set circle from start, goal and interim point - - * @throws KDL::Error_MotionPlanning if the given points are colinear. - */ - static std::unique_ptr circleFromInterim(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, - const KDL::Vector& interim_point, double eqradius); - -private: - PathCircleGenerator(){}; // no instantiation of this helper class! - - /** - * @brief law of cosines: returns angle gamma in c² = a²+b²-2ab cos(gamma) - * - * @return angle in radians - */ - static double cosines(const double a, const double b, const double c); - - static constexpr double MAX_RADIUS_DIFF{ 1e-2 }; - static constexpr double MAX_COLINEAR_NORM{ 1e-5 }; -}; - -} // namespace pilz_industrial_motion_planner - -class ErrorMotionPlanningCenterPointDifferentRadius : public KDL::Error_MotionPlanning -{ -public: - const char* Description() const override - { - return "Distances between start-center and goal-center are different." - " A circle cannot be created."; - } - int GetType() const override - { - return ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS; - } // LCOV_EXCL_LINE - -private: - static constexpr int ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS{ 3006 }; -}; +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.hpp new file mode 100644 index 0000000000..25b2b018f9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_circle_generator.hpp @@ -0,0 +1,103 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Generator class for KDL::Path_Circle from different circle + * representations + */ +class PathCircleGenerator +{ +public: + /** + * @brief set the path circle from start, goal and center point + * + * Note that a half circle should use interim point and cannot be defined + * by circle center since start/goal/center points are colinear. + * @throws KDL::Error_MotionPlanning in case start and goal have different + * radii to the center point. + */ + static std::unique_ptr circleFromCenter(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, + const KDL::Vector& center_point, double eqradius); + + /** + * @brief set circle from start, goal and interim point + + * @throws KDL::Error_MotionPlanning if the given points are colinear. + */ + static std::unique_ptr circleFromInterim(const KDL::Frame& start_pose, const KDL::Frame& goal_pose, + const KDL::Vector& interim_point, double eqradius); + +private: + PathCircleGenerator(){}; // no instantiation of this helper class! + + /** + * @brief law of cosines: returns angle gamma in c² = a²+b²-2ab cos(gamma) + * + * @return angle in radians + */ + static double cosines(const double a, const double b, const double c); + + static constexpr double MAX_RADIUS_DIFF{ 1e-2 }; + static constexpr double MAX_COLINEAR_NORM{ 1e-5 }; +}; + +} // namespace pilz_industrial_motion_planner + +class ErrorMotionPlanningCenterPointDifferentRadius : public KDL::Error_MotionPlanning +{ +public: + const char* Description() const override + { + return "Distances between start-center and goal-center are different." + " A circle cannot be created."; + } + int GetType() const override + { + return ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS; + } // LCOV_EXCL_LINE + +private: + static constexpr int ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS{ 3006 }; +}; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h index 92a725d8cf..6e1ede489e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,111 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include - -#include -#include - -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief MoveIt Plugin for Planning with Standard Robot Commands - * This planner is dedicated to return a instance of PlanningContext that - * corresponds to the requested motion command - * set as planner_id in the MotionPlanRequest). - * It can be easily extended with additional commands by creating a class - * inheriting from PlanningContextLoader. - */ -class CommandPlanner : public planning_interface::PlannerManager -{ -public: - ~CommandPlanner() override - { - } - - /** - * @brief Initializes the planner - * Upon initialization this planner will look for plugins implementing - * pilz_industrial_motion_planner::PlanningContextLoader. - * @param model The robot model - * @param node The node - * @param ns The namespace - * @return true on success, false otherwise - */ - bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, - const std::string& ns) override; - - /// Description of the planner - std::string getDescription() const override; - - /** - * @brief Returns the available planning commands - * @param list with the planning algorithms - * @note behined each command is a - * pilz_industrial_motion_planner::PlanningContextLoader loaded as plugin - */ - void getPlanningAlgorithms(std::vector& algs) const override; - - /** - * @brief Returns a PlanningContext that can be used to solve(calculate) the - * trajectory that corresponds to command - * given in motion request as planner_id. - * @param planning_scene - * @param req - * @param error_code - * @return - */ - planning_interface::PlanningContextPtr - getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::msg::MoveItErrorCodes& error_code) const override; - - /** - * @brief Checks if the request can be handled - * @param motion request containing the planning_id that corresponds to the - * motion command - * @return true if the request can be handled - */ - bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override; - - /** - * @brief Register a PlanningContextLoader to be used by the CommandPlanner - * @param planning_context_loader - * @throw ContextLoaderRegistrationException if a loader with the same - * algorithm name is already registered - */ - void registerContextLoader(const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader); - -private: - /// Plugin loader - std::unique_ptr> planner_context_loader_; - - /// Mapping from command to loader - std::map context_loader_map_; - - /// Robot model obtained at initialize - moveit::core::RobotModelConstPtr model_; - - /// Namespace where the parameters are stored, obtained at initialize - std::string namespace_; - - /// aggregated limits of the active joints - pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints_; - - /// cartesian limit - std::shared_ptr param_listener_; - cartesian_limits::Params params_; -}; - -MOVEIT_CLASS_FORWARD(CommandPlanner); - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp new file mode 100644 index 0000000000..535fbd9644 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp @@ -0,0 +1,143 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +#include +#include + +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief MoveIt Plugin for Planning with Standard Robot Commands + * This planner is dedicated to return a instance of PlanningContext that + * corresponds to the requested motion command + * set as planner_id in the MotionPlanRequest). + * It can be easily extended with additional commands by creating a class + * inheriting from PlanningContextLoader. + */ +class CommandPlanner : public planning_interface::PlannerManager +{ +public: + ~CommandPlanner() override + { + } + + /** + * @brief Initializes the planner + * Upon initialization this planner will look for plugins implementing + * pilz_industrial_motion_planner::PlanningContextLoader. + * @param model The robot model + * @param node The node + * @param ns The namespace + * @return true on success, false otherwise + */ + bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, + const std::string& ns) override; + + /// Description of the planner + std::string getDescription() const override; + + /** + * @brief Returns the available planning commands + * @param list with the planning algorithms + * @note behined each command is a + * pilz_industrial_motion_planner::PlanningContextLoader loaded as plugin + */ + void getPlanningAlgorithms(std::vector& algs) const override; + + /** + * @brief Returns a PlanningContext that can be used to solve(calculate) the + * trajectory that corresponds to command + * given in motion request as planner_id. + * @param planning_scene + * @param req + * @param error_code + * @return + */ + planning_interface::PlanningContextPtr + getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::msg::MoveItErrorCodes& error_code) const override; + + /** + * @brief Checks if the request can be handled + * @param motion request containing the planning_id that corresponds to the + * motion command + * @return true if the request can be handled + */ + bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override; + + /** + * @brief Register a PlanningContextLoader to be used by the CommandPlanner + * @param planning_context_loader + * @throw ContextLoaderRegistrationException if a loader with the same + * algorithm name is already registered + */ + void registerContextLoader(const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader); + +private: + /// Plugin loader + std::unique_ptr> planner_context_loader_; + + /// Mapping from command to loader + std::map context_loader_map_; + + /// Robot model obtained at initialize + moveit::core::RobotModelConstPtr model_; + + /// Namespace where the parameters are stored, obtained at initialize + std::string namespace_; + + /// aggregated limits of the active joints + pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints_; + + /// cartesian limit + std::shared_ptr param_listener_; + cartesian_limits::Params params_; +}; + +MOVEIT_CLASS_FORWARD(CommandPlanner); + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h index ce59f2b202..c0c20faf2a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,131 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -namespace pilz_industrial_motion_planner -{ -using TipFrameFunc_t = std::function; - -// List of exceptions which can be thrown by the PlanComponentsBuilder class. -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoBlenderSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoTipFrameFunctionSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoRobotModelSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(BlendingFailedException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); - -/** - * @brief Helper class to encapsulate the merge and blend process of - * trajectories. - */ -class PlanComponentsBuilder -{ -public: - /** - * @brief Sets the blender used to blend two trajectories. - */ - void setBlender(std::unique_ptr blender); - - /** - * @brief Sets the robot model needed to create new trajectory elements. - */ - void setModel(const moveit::core::RobotModelConstPtr& model); - - /** - * @brief Appends the specified trajectory to the trajectory container - * under construction. - * - * The appending complies to the following rules: - * - A trajectory is simply added/attached to the previous trajectory: - * - if they are from the same group and - * - if the specified blend_radius is zero. - * - A trajectory is blended together with the previous trajectory: - * - if they are from the same group and - * - if the specified blend_radius is GREATER than zero. - * - A new trajectory element is created and the given trajectory is - * appended/attached to the newly created empty trajectory: - * - if the given and previous trajectory are from different groups. - * - * @param planning_scene The scene planning is occurring in. - * - * @param other Trajectories which has to be added to the trajectory container - * under construction. - * - * @param blend_radius The blending radius between the previous and the - * specified trajectory. - */ - void append(const planning_scene::PlanningSceneConstPtr& planning_scene, - const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); - - /** - * @brief Clears the trajectory container under construction. - */ - void reset(); - - /** - * @return The final trajectory container which results from the append calls. - */ - std::vector build() const; - -private: - void blend(const planning_scene::PlanningSceneConstPtr& planning_scene, - const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); - -private: - /** - * @brief Appends a trajectory to a result trajectory leaving out the - * first point, if it matches the last point of the result trajectory. - * - * @note Controllers, so far at least the - * ros_controllers::JointTrajectoryController require a timewise strictly - * increasing trajectory. If through appending the last point of the - * original trajectory gets repeated, it is removed here. - */ - static void appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, - const robot_trajectory::RobotTrajectory& source); - -private: - //! Blender used to blend two trajectories. - std::unique_ptr blender_; - - //! Robot model needed to create new trajectory container elements. - moveit::core::RobotModelConstPtr model_; - - //! The previously added trajectory. - robot_trajectory::RobotTrajectoryPtr traj_tail_; - - //! The trajectory container under construction. - std::vector traj_cont_; - -private: - //! Constant to check for equality of variables of two RobotState instances. - static constexpr double ROBOT_STATE_EQUALITY_EPSILON = 1e-4; -}; - -inline void PlanComponentsBuilder::setBlender(std::unique_ptr blender) -{ - blender_ = std::move(blender); -} - -inline void PlanComponentsBuilder::setModel(const moveit::core::RobotModelConstPtr& model) -{ - model_ = model; -} - -inline void PlanComponentsBuilder::reset() -{ - traj_tail_ = nullptr; - traj_cont_.clear(); -} - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.hpp new file mode 100644 index 0000000000..787226925a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.hpp @@ -0,0 +1,163 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +using TipFrameFunc_t = std::function; + +// List of exceptions which can be thrown by the PlanComponentsBuilder class. +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoBlenderSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoTipFrameFunctionSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoRobotModelSetException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(BlendingFailedException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); + +/** + * @brief Helper class to encapsulate the merge and blend process of + * trajectories. + */ +class PlanComponentsBuilder +{ +public: + /** + * @brief Sets the blender used to blend two trajectories. + */ + void setBlender(std::unique_ptr blender); + + /** + * @brief Sets the robot model needed to create new trajectory elements. + */ + void setModel(const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Appends the specified trajectory to the trajectory container + * under construction. + * + * The appending complies to the following rules: + * - A trajectory is simply added/attached to the previous trajectory: + * - if they are from the same group and + * - if the specified blend_radius is zero. + * - A trajectory is blended together with the previous trajectory: + * - if they are from the same group and + * - if the specified blend_radius is GREATER than zero. + * - A new trajectory element is created and the given trajectory is + * appended/attached to the newly created empty trajectory: + * - if the given and previous trajectory are from different groups. + * + * @param planning_scene The scene planning is occurring in. + * + * @param other Trajectories which has to be added to the trajectory container + * under construction. + * + * @param blend_radius The blending radius between the previous and the + * specified trajectory. + */ + void append(const planning_scene::PlanningSceneConstPtr& planning_scene, + const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + + /** + * @brief Clears the trajectory container under construction. + */ + void reset(); + + /** + * @return The final trajectory container which results from the append calls. + */ + std::vector build() const; + +private: + void blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius); + +private: + /** + * @brief Appends a trajectory to a result trajectory leaving out the + * first point, if it matches the last point of the result trajectory. + * + * @note Controllers, so far at least the + * ros_controllers::JointTrajectoryController require a timewise strictly + * increasing trajectory. If through appending the last point of the + * original trajectory gets repeated, it is removed here. + */ + static void appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, + const robot_trajectory::RobotTrajectory& source); + +private: + //! Blender used to blend two trajectories. + std::unique_ptr blender_; + + //! Robot model needed to create new trajectory container elements. + moveit::core::RobotModelConstPtr model_; + + //! The previously added trajectory. + robot_trajectory::RobotTrajectoryPtr traj_tail_; + + //! The trajectory container under construction. + std::vector traj_cont_; + +private: + //! Constant to check for equality of variables of two RobotState instances. + static constexpr double ROBOT_STATE_EQUALITY_EPSILON = 1e-4; +}; + +inline void PlanComponentsBuilder::setBlender(std::unique_ptr blender) +{ + blender_ = std::move(blender); +} + +inline void PlanComponentsBuilder::setModel(const moveit::core::RobotModelConstPtr& model) +{ + model_ = model; +} + +inline void PlanComponentsBuilder::reset() +{ + traj_tail_ = nullptr; + traj_cont_.clear(); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index 7597316717..449993e37f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,139 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief PlanningContext for obtaining trajectories - */ -template -class PlanningContextBase : public planning_interface::PlanningContext -{ -public: - PlanningContextBase(const std::string& name, const std::string& group, - const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) - : planning_interface::PlanningContext(name, group) - , terminated_(false) - , model_(model) - , limits_(limits) - , generator_(model, limits_, group) - { - } - - ~PlanningContextBase() override - { - } - - /** - * @brief Calculates a trajectory for the request this context is currently - * set for - * @param res The result containing the respective trajectory, or error_code - * on failure - * @return true on success, false otherwise - */ - void solve(planning_interface::MotionPlanResponse& res) override; - - /** - * @brief Will return the same trajectory as - * solve(planning_interface::MotionPlanResponse& res) - * This function just delegates to the common response however here the same - * trajectory is stored with the - * descriptions "plan", "simplify", "interpolate" - * @param res The detailed response - * @return true on success, false otherwise - */ - void solve(planning_interface::MotionPlanDetailedResponse& res) override; - - /** - * @brief Will terminate solve() - * @return - * @note Currently will not stop a running solve but not start future solves. - */ - bool terminate() override; - - /** - * @copydoc planning_interface::PlanningContext::clear() - */ - void clear() override; - - /// Flag if terminated - std::atomic_bool terminated_; - - /// The robot model - moveit::core::RobotModelConstPtr model_; - - /// Joint limits to be used during planning - pilz_industrial_motion_planner::LimitsContainer limits_; - -protected: - GeneratorT generator_; -}; - -template -void pilz_industrial_motion_planner::PlanningContextBase::solve(planning_interface::MotionPlanResponse& res) -{ - if (terminated_) - { - RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), - "Using solve on a terminated planning context!"); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - return; - } - generator_.generate(getPlanningScene(), request_, res); -} - -template -void pilz_industrial_motion_planner::PlanningContextBase::solve( - planning_interface::MotionPlanDetailedResponse& res) -{ - // delegate to regular response - planning_interface::MotionPlanResponse undetailed_response; - solve(undetailed_response); - - res.description.push_back("plan"); - res.trajectory.push_back(undetailed_response.trajectory); - res.processing_time.push_back(undetailed_response.planning_time); - - res.description.push_back("simplify"); - res.trajectory.push_back(undetailed_response.trajectory); - res.processing_time.push_back(0); - - res.description.push_back("interpolate"); - res.trajectory.push_back(undetailed_response.trajectory); - res.processing_time.push_back(0); - - res.error_code = undetailed_response.error_code; -} - -template -bool pilz_industrial_motion_planner::PlanningContextBase::terminate() -{ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), - "Terminate called"); - terminated_ = true; - return true; -} - -template -void pilz_industrial_motion_planner::PlanningContextBase::clear() -{ - // No structures that need cleaning - return; -} - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp new file mode 100644 index 0000000000..0abba27e76 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp @@ -0,0 +1,171 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief PlanningContext for obtaining trajectories + */ +template +class PlanningContextBase : public planning_interface::PlanningContext +{ +public: + PlanningContextBase(const std::string& name, const std::string& group, + const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : planning_interface::PlanningContext(name, group) + , terminated_(false) + , model_(model) + , limits_(limits) + , generator_(model, limits_, group) + { + } + + ~PlanningContextBase() override + { + } + + /** + * @brief Calculates a trajectory for the request this context is currently + * set for + * @param res The result containing the respective trajectory, or error_code + * on failure + * @return true on success, false otherwise + */ + void solve(planning_interface::MotionPlanResponse& res) override; + + /** + * @brief Will return the same trajectory as + * solve(planning_interface::MotionPlanResponse& res) + * This function just delegates to the common response however here the same + * trajectory is stored with the + * descriptions "plan", "simplify", "interpolate" + * @param res The detailed response + * @return true on success, false otherwise + */ + void solve(planning_interface::MotionPlanDetailedResponse& res) override; + + /** + * @brief Will terminate solve() + * @return + * @note Currently will not stop a running solve but not start future solves. + */ + bool terminate() override; + + /** + * @copydoc planning_interface::PlanningContext::clear() + */ + void clear() override; + + /// Flag if terminated + std::atomic_bool terminated_; + + /// The robot model + moveit::core::RobotModelConstPtr model_; + + /// Joint limits to be used during planning + pilz_industrial_motion_planner::LimitsContainer limits_; + +protected: + GeneratorT generator_; +}; + +template +void pilz_industrial_motion_planner::PlanningContextBase::solve(planning_interface::MotionPlanResponse& res) +{ + if (terminated_) + { + RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), + "Using solve on a terminated planning context!"); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + return; + } + generator_.generate(getPlanningScene(), request_, res); +} + +template +void pilz_industrial_motion_planner::PlanningContextBase::solve( + planning_interface::MotionPlanDetailedResponse& res) +{ + // delegate to regular response + planning_interface::MotionPlanResponse undetailed_response; + solve(undetailed_response); + + res.description.push_back("plan"); + res.trajectory.push_back(undetailed_response.trajectory); + res.processing_time.push_back(undetailed_response.planning_time); + + res.description.push_back("simplify"); + res.trajectory.push_back(undetailed_response.trajectory); + res.processing_time.push_back(0); + + res.description.push_back("interpolate"); + res.trajectory.push_back(undetailed_response.trajectory); + res.processing_time.push_back(0); + + res.error_code = undetailed_response.error_code; +} + +template +bool pilz_industrial_motion_planner::PlanningContextBase::terminate() +{ + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), + "Terminate called"); + terminated_ = true; + return true; +} + +template +void pilz_industrial_motion_planner::PlanningContextBase::clear() +{ + // No structures that need cleaning + return; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h index 85b21efc84..d89226c9ff 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,35 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -#include -#include - -#include -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -MOVEIT_CLASS_FORWARD(PlanningContext); - -/** - * @brief PlanningContext for obtaining CIRC trajectories - */ -class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningContextBase -{ -public: - PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) - : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) - { - } -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp new file mode 100644 index 0000000000..e0b975c6f0 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext); + +/** + * @brief PlanningContext for obtaining CIRC trajectories + */ +class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h index 54b1594399..5fde25977c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -32,34 +44,6 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include - -#include - -#include -#include - -#include -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -MOVEIT_CLASS_FORWARD(PlanningContext); - -/** - * @brief PlanningContext for obtaining LIN trajectories - */ -class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContextBase -{ -public: - PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) - : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) - { - } -}; - -} // namespace pilz_industrial_motion_planner +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp new file mode 100644 index 0000000000..865febf5c6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext); + +/** + * @brief PlanningContext for obtaining LIN trajectories + */ +class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h index becd62a9df..3812eefe6d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,112 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Base class for all PlanningContextLoaders. - * Since planning_interface::PlanningContext has a non empty ctor, - * classes derived from it can not be plugins. - * This class serves as base class for wrappers. - */ -class PlanningContextLoader -{ -public: - PlanningContextLoader(); - virtual ~PlanningContextLoader(); - - /// Return the algorithm the loader uses - virtual std::string getAlgorithm() const; - - /** - * @brief Sets the robot model that can be passed to the planning context - * @param model The robot model - * @return false if could not be set - */ - virtual bool setModel(const moveit::core::RobotModelConstPtr& model); - - /** - * @brief Sets limits the planner can pass to the contexts - * @param limits container of limits, no guarantee to contain the limits for - * all joints of the model - * @return true if limits could be set - */ - virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits); - - /** - * @brief Return the planning context - * @param planning_context - * @param name context name - * @param group name of the planning group - * @return true on success, false otherwise - */ - virtual bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, - const std::string& group) const = 0; - -protected: - /** - * @brief Return the planning context of type T - * @param planning_context - * @param name context name - * @param group name of the planning group - * @return true on success, false otherwise - */ - template - bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, - const std::string& group) const; - -protected: - /// Name of the algorithm - std::string alg_; - - /// True if limits are set - bool limits_set_; - - /// Limits to be used during planning - pilz_industrial_motion_planner::LimitsContainer limits_; - - /// True if model is set - bool model_set_; - - /// The robot model - moveit::core::RobotModelConstPtr model_; -}; - -typedef std::shared_ptr PlanningContextLoaderPtr; -typedef std::shared_ptr PlanningContextLoaderConstPtr; - -template -bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& planning_context, - const std::string& name, const std::string& group) const -{ - if (limits_set_ && model_set_) - { - planning_context = std::make_shared(name, group, model_, limits_); - return true; - } - else - { - if (!limits_set_) - { - RCLCPP_ERROR(rclcpp::get_logger("planning_context_loader"), - "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); - } - if (!model_set_) - { - RCLCPP_ERROR(rclcpp::get_logger("planning_context_loader"), "Robot model was not set"); - } - return false; - } -} - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp new file mode 100644 index 0000000000..5d93a34d14 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp @@ -0,0 +1,144 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Base class for all PlanningContextLoaders. + * Since planning_interface::PlanningContext has a non empty ctor, + * classes derived from it can not be plugins. + * This class serves as base class for wrappers. + */ +class PlanningContextLoader +{ +public: + PlanningContextLoader(); + virtual ~PlanningContextLoader(); + + /// Return the algorithm the loader uses + virtual std::string getAlgorithm() const; + + /** + * @brief Sets the robot model that can be passed to the planning context + * @param model The robot model + * @return false if could not be set + */ + virtual bool setModel(const moveit::core::RobotModelConstPtr& model); + + /** + * @brief Sets limits the planner can pass to the contexts + * @param limits container of limits, no guarantee to contain the limits for + * all joints of the model + * @return true if limits could be set + */ + virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits); + + /** + * @brief Return the planning context + * @param planning_context + * @param name context name + * @param group name of the planning group + * @return true on success, false otherwise + */ + virtual bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const = 0; + +protected: + /** + * @brief Return the planning context of type T + * @param planning_context + * @param name context name + * @param group name of the planning group + * @return true on success, false otherwise + */ + template + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const; + +protected: + /// Name of the algorithm + std::string alg_; + + /// True if limits are set + bool limits_set_; + + /// Limits to be used during planning + pilz_industrial_motion_planner::LimitsContainer limits_; + + /// True if model is set + bool model_set_; + + /// The robot model + moveit::core::RobotModelConstPtr model_; +}; + +typedef std::shared_ptr PlanningContextLoaderPtr; +typedef std::shared_ptr PlanningContextLoaderConstPtr; + +template +bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& planning_context, + const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context = std::make_shared(name, group, model_, limits_); + return true; + } + else + { + if (!limits_set_) + { + RCLCPP_ERROR(rclcpp::get_logger("planning_context_loader"), + "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); + } + if (!model_set_) + { + RCLCPP_ERROR(rclcpp::get_logger("planning_context_loader"), "Robot model was not set"); + } + return false; + } +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h index cf9a847243..6e365f4ce8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,36 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Plugin that can generate instances of PlanningContextCIRC. - * Generates instances of PlanningContextLIN. - */ -class PlanningContextLoaderCIRC : public PlanningContextLoader -{ -public: - PlanningContextLoaderCIRC(); - ~PlanningContextLoaderCIRC() override; - - /** - * @brief return a instance of - * pilz_industrial_motion_planner::PlanningContextCIRC - * @param planning_context returned context - * @param name - * @param group - * @return true on success, false otherwise - */ - bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, - const std::string& group) const override; -}; - -typedef std::shared_ptr PlanningContextLoaderCIRCPtr; -typedef std::shared_ptr PlanningContextLoaderCIRCConstPtr; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.hpp new file mode 100644 index 0000000000..ea3e171a04 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_circ.hpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextCIRC. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderCIRC : public PlanningContextLoader +{ +public: + PlanningContextLoaderCIRC(); + ~PlanningContextLoaderCIRC() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextCIRC + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef std::shared_ptr PlanningContextLoaderCIRCPtr; +typedef std::shared_ptr PlanningContextLoaderCIRCConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h index 970e04af52..99f97852e0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,36 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Plugin that can generate instances of PlanningContextLIN. - * Generates instances of PlanningContextLIN. - */ -class PlanningContextLoaderLIN : public PlanningContextLoader -{ -public: - PlanningContextLoaderLIN(); - ~PlanningContextLoaderLIN() override; - - /** - * @brief return a instance of - * pilz_industrial_motion_planner::PlanningContextLIN - * @param planning_context returned context - * @param name - * @param group - * @return true on success, false otherwise - */ - bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, - const std::string& group) const override; -}; - -typedef std::shared_ptr PlanningContextLoaderLINPtr; -typedef std::shared_ptr PlanningContextLoaderLINConstPtr; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.hpp new file mode 100644 index 0000000000..c0f7eecf7e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_lin.hpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextLIN. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderLIN : public PlanningContextLoader +{ +public: + PlanningContextLoaderLIN(); + ~PlanningContextLoaderLIN() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextLIN + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef std::shared_ptr PlanningContextLoaderLINPtr; +typedef std::shared_ptr PlanningContextLoaderLINConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h index dfdf2b6efb..877d301689 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,36 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Plugin that can generate instances of PlanningContextPTP. - * Generates instances of PlanningContextPTP. - */ -class PlanningContextLoaderPTP : public PlanningContextLoader -{ -public: - PlanningContextLoaderPTP(); - ~PlanningContextLoaderPTP() override; - - /** - * @brief return a instance of - * pilz_industrial_motion_planner::PlanningContextPTP - * @param planning_context returned context - * @param name - * @param group - * @return true on success, false otherwise - */ - bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, - const std::string& group) const override; -}; - -typedef std::shared_ptr PlanningContextLoaderPTPPtr; -typedef std::shared_ptr PlanningContextLoaderPTPConstPtr; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.hpp new file mode 100644 index 0000000000..42e23df4d6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_ptp.hpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextPTP. + * Generates instances of PlanningContextPTP. + */ +class PlanningContextLoaderPTP : public PlanningContextLoader +{ +public: + PlanningContextLoaderPTP(); + ~PlanningContextLoaderPTP() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextPTP + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef std::shared_ptr PlanningContextLoaderPTPPtr; +typedef std::shared_ptr PlanningContextLoaderPTPConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h index c62bda37bb..829a43b1c7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,35 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -#include -#include - -#include -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -MOVEIT_CLASS_FORWARD(PlanningContext); - -/** - * @brief PlanningContext for obtaining PTP trajectories - */ -class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContextBase -{ -public: - PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) - : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) - { - } -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp new file mode 100644 index 0000000000..994020aad7 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext); + +/** + * @brief PlanningContext for obtaining PTP trajectories + */ +class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h index 5dfc44da29..c1a46f0e51 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,39 +45,5 @@ *********************************************************************/ #pragma once - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @class PlanningException - * @brief A base class for all pilz_industrial_motion_planner exceptions - * inheriting from std::runtime_exception - */ -class PlanningException : public std::runtime_error -{ -public: - PlanningException(const std::string& error_desc) : std::runtime_error(error_desc) - { - } -}; - -/** - * @class PlanningContextFactoryRegistrationException - * @brief An exception class thrown when the planner manager is unable to load a - * factory - * - * Loading a PlanningContextFactory can fail if a factory is loaded that - * would provide a command which was already provided by another factory loaded - * before. - */ -class ContextLoaderRegistrationException : public PlanningException -{ -public: - ContextLoaderRegistrationException(const std::string& error_desc) : PlanningException(error_desc) - { - } -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.hpp new file mode 100644 index 0000000000..5dfc44da29 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_exceptions.hpp @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @class PlanningException + * @brief A base class for all pilz_industrial_motion_planner exceptions + * inheriting from std::runtime_exception + */ +class PlanningException : public std::runtime_error +{ +public: + PlanningException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +/** + * @class PlanningContextFactoryRegistrationException + * @brief An exception class thrown when the planner manager is unable to load a + * factory + * + * Loading a PlanningContextFactory can fail if a factory is loaded that + * would provide a command which was already provided by another factory loaded + * before. + */ +class ContextLoaderRegistrationException : public PlanningException +{ +public: + ContextLoaderRegistrationException(const std::string& error_desc) : PlanningException(error_desc) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h index ac5f58a548..94a493fe16 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,58 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoSolverException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(MoreThanOneTipFrameException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); - -/** - * @returns true if the JointModelGroup has a solver, false otherwise. - * - * @tparam JointModelGroup aims at moveit::core::JointModelGroup - * @throws exception in case group is null. - */ -template -static bool hasSolver(const JointModelGroup* group) -{ - if (group == nullptr) - { - throw std::invalid_argument("Group must not be null"); - } - return group->getSolverInstance() != nullptr; -} - -/** - * @return The name of the tip frame (link) of the specified group - * returned by the solver. - * - * @tparam JointModelGroup aims at moveit::core::JointModelGroup - * @throws exception in case the group has no solver. - * @throws exception in case the solver for the group has more than one tip - * frame. - */ -template -static const std::string& getSolverTipFrame(const JointModelGroup* group) -{ - if (!hasSolver(group)) - { - throw NoSolverException("No solver for group " + group->getName()); - } - - const std::vector& tip_frames{ group->getSolverInstance()->getTipFrames() }; - if (tip_frames.size() > 1) - { - throw MoreThanOneTipFrameException("Solver for group \"" + group->getName() + "\" has more than one tip frame"); - } - return tip_frames.front(); -} - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.hpp new file mode 100644 index 0000000000..aaf17fa7bd --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/tip_frame_getter.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoSolverException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(MoreThanOneTipFrameException, moveit_msgs::msg::MoveItErrorCodes::FAILURE); + +/** + * @returns true if the JointModelGroup has a solver, false otherwise. + * + * @tparam JointModelGroup aims at moveit::core::JointModelGroup + * @throws exception in case group is null. + */ +template +static bool hasSolver(const JointModelGroup* group) +{ + if (group == nullptr) + { + throw std::invalid_argument("Group must not be null"); + } + return group->getSolverInstance() != nullptr; +} + +/** + * @return The name of the tip frame (link) of the specified group + * returned by the solver. + * + * @tparam JointModelGroup aims at moveit::core::JointModelGroup + * @throws exception in case the group has no solver. + * @throws exception in case the solver for the group has more than one tip + * frame. + */ +template +static const std::string& getSolverTipFrame(const JointModelGroup* group) +{ + if (!hasSolver(group)) + { + throw NoSolverException("No solver for group " + group->getName()); + } + + const std::vector& tip_frames{ group->getSolverInstance()->getTipFrames() }; + if (tip_frames.size() > 1) + { + throw MoreThanOneTipFrameException("Solver for group \"" + group->getName() + "\" has more than one tip frame"); + } + return tip_frames.front(); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h index 450f89fea5..c0e8559256 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,27 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -struct TrajectoryBlendRequest -{ - // The name of the group of joints on which this blender is operating - std::string group_name; - - // The name of the target link on which this blender is operating - std::string link_name; - - // Robot trajectories to be blended - robot_trajectory::RobotTrajectoryPtr first_trajectory; - robot_trajectory::RobotTrajectoryPtr second_trajectory; - - // Blend radius in meter - double blend_radius; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.hpp new file mode 100644 index 0000000000..c24dda5e2e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_request.hpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +struct TrajectoryBlendRequest +{ + // The name of the group of joints on which this blender is operating + std::string group_name; + + // The name of the target link on which this blender is operating + std::string link_name; + + // Robot trajectories to be blended + robot_trajectory::RobotTrajectoryPtr first_trajectory; + robot_trajectory::RobotTrajectoryPtr second_trajectory; + + // Blend radius in meter + double blend_radius; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h index 3f518e5737..67e7fa32e2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,25 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -namespace pilz_industrial_motion_planner -{ -struct TrajectoryBlendResponse -{ - // The name of the group of joints on which this blender is operating - std::string group_name; - - // Resulted robot trajectories after blending - robot_trajectory::RobotTrajectoryPtr first_trajectory; - robot_trajectory::RobotTrajectoryPtr blend_trajectory; - robot_trajectory::RobotTrajectoryPtr second_trajectory; - - // Error code - moveit_msgs::msg::MoveItErrorCodes error_code; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.hpp new file mode 100644 index 0000000000..4f3ce4e597 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blend_response.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +struct TrajectoryBlendResponse +{ + // The name of the group of joints on which this blender is operating + std::string group_name; + + // Resulted robot trajectories after blending + robot_trajectory::RobotTrajectoryPtr first_trajectory; + robot_trajectory::RobotTrajectoryPtr blend_trajectory; + robot_trajectory::RobotTrajectoryPtr second_trajectory; + + // Error code + moveit_msgs::msg::MoveItErrorCodes error_code; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h index 702cdaccd5..921a823d43 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,45 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Base class of trajectory blenders - */ -class TrajectoryBlender -{ -public: - TrajectoryBlender(const pilz_industrial_motion_planner::LimitsContainer& planner_limits) : limits_(planner_limits) - { - } - - virtual ~TrajectoryBlender() - { - } - - /** - * @brief Blend two robot trajectories with the given blending radius - * @param planning_scene: planning scene - * @param req: trajectory blend request - * @param res: trajectroy blend response - * @return true if blend succeed - */ - virtual bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene, - const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, - pilz_industrial_motion_planner::TrajectoryBlendResponse& res) = 0; - -protected: - const pilz_industrial_motion_planner::LimitsContainer limits_; -}; - -typedef std::unique_ptr TrajectoryBlenderUniquePtr; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.hpp new file mode 100644 index 0000000000..914b214d24 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.hpp @@ -0,0 +1,77 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Base class of trajectory blenders + */ +class TrajectoryBlender +{ +public: + TrajectoryBlender(const pilz_industrial_motion_planner::LimitsContainer& planner_limits) : limits_(planner_limits) + { + } + + virtual ~TrajectoryBlender() + { + } + + /** + * @brief Blend two robot trajectories with the given blending radius + * @param planning_scene: planning scene + * @param req: trajectory blend request + * @param res: trajectroy blend response + * @return true if blend succeed + */ + virtual bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) = 0; + +protected: + const pilz_industrial_motion_planner::LimitsContainer limits_; +}; + +typedef std::unique_ptr TrajectoryBlenderUniquePtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h index 705e1703f0..61e598d162 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,149 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Trajectory blender implementing transition window algorithm - * - * See doc/MotionBlendAlgorithmDescription.pdf for a mathematical description of - * the algorithm. - */ -class TrajectoryBlenderTransitionWindow : public TrajectoryBlender -{ -public: - TrajectoryBlenderTransitionWindow(const LimitsContainer& planner_limits) - : TrajectoryBlender::TrajectoryBlender(planner_limits) - { - } - - ~TrajectoryBlenderTransitionWindow() override - { - } - - /** - * @brief Blend two trajectories using transition window. The trajectories - * have to be equally and uniformly - * discretized. - * @param planning_scene: The scene planning is occurring in. - * @param req: following fields need to be filled for a valid request: - * - group_name : name of the planning group - * - link_name : name of the target link - * - first_trajectory: Joint trajectory stops at end point. - * The last point must be the same as the first point - * of the second trajectory. - * - second trajectory: Joint trajectory stops at end point. - * The first point must be the same as the last point - * of the first trajectory. - * - blend_radius: The blend radius determines a sphere with the - * intersection point of the two trajectories - * as the center. Trajectory blending happens inside of - * this sphere. - * @param res: following fields are returned as response by the blend - * algorithm - * - group_name : name of the planning group - * - first_trajectory: Part of the first original trajectory which is - * outside of the blend sphere. - * - blend_trajectory: Joint trajectory connecting the first and second - * trajectories without stop. - * The first waypoint has non-zero time from start. - * - second trajectory: Part of the second original trajectory which is - * outside of the blend sphere. - * The first waypoint has non-zero time from start. - * error_code: information of failed blend - * @return true if succeed - */ - bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene, - const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, - pilz_industrial_motion_planner::TrajectoryBlendResponse& res) override; - -private: - /** - * @brief validate trajectory blend request - * @param req - * @param sampling_time: get the same sampling time of the two input - * trajectories - * @param error_code - * @return - */ - bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, - moveit_msgs::msg::MoveItErrorCodes& error_code) const; - /** - * @brief searchBlendPoint - * @param req: trajectory blend request - * @param first_interse_index: index of the first point of the first - * trajectory that is inside the blend sphere - * @param second_interse_index: index of the last point of the second - * trajectory that is still inside the blend sphere - */ - bool searchIntersectionPoints(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, - std::size_t& first_interse_index, std::size_t& second_interse_index) const; - - /** - * @brief Determine how the second trajectory should be aligned with the first - * trajectory for blend. - * Let tau_1 be the time of the first trajectory from the first_interse_index - * to the end and tau_2 the time of the - * second trajectory from the beginning to the second_interse_index: - * - if tau_1 > tau_2:
- * align the end of the first trajectory with second_interse_index - * 
-   *    first traj:  |-------------|--------!--------------|
-   *    second traj:                        |--------------|-------------------|
-   *    blend phase:               |-----------------------|
-   *
- * - else:
- * align the first_interse_index with the beginning of the second - * trajectory - * 
-   *    first traj:  |-------------|-----------------------|
-   *    second traj: |-----------------------!----------|-------------------|
-   *    blend phase:               |----------------------------------|
-   *
- * - * @param req: trajectory blend request - * @param first_interse_index: index of the intersection point between first - * trajectory and blend sphere - * @param second_interse_index: index of the intersection point between second - * trajectory and blend sphere - * @param blend_align_index: index on the first trajectory, to which the first - * point on the second trajectory should - * be aligned to for motion blend. It is now always same as - * first_interse_index - * @param blend_time: time of the motion blend period - */ - void determineTrajectoryAlignment(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, - std::size_t first_interse_index, std::size_t second_interse_index, - std::size_t& blend_align_index) const; - - /** - * @brief blend two trajectories in Cartesian space, result in a - * MultiDOFJointTrajectory which consists - * of a list of transforms for the blend phase. - * @param req - * @param first_interse_index - * @param second_interse_index - * @param blend_begin_index - * @param sampling_time - * @param trajectory: the resulting blend trajectory inside the blending - * sphere - */ - void blendTrajectoryCartesian(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, - const std::size_t first_interse_index, const std::size_t second_interse_index, - const std::size_t blend_align_index, double sampling_time, - pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const; - -private: // static members - // Constant to check for equality of values. - static constexpr double EPSILON = 1e-4; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp new file mode 100644 index 0000000000..79672d8b00 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp @@ -0,0 +1,181 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Trajectory blender implementing transition window algorithm + * + * See doc/MotionBlendAlgorithmDescription.pdf for a mathematical description of + * the algorithm. + */ +class TrajectoryBlenderTransitionWindow : public TrajectoryBlender +{ +public: + TrajectoryBlenderTransitionWindow(const LimitsContainer& planner_limits) + : TrajectoryBlender::TrajectoryBlender(planner_limits) + { + } + + ~TrajectoryBlenderTransitionWindow() override + { + } + + /** + * @brief Blend two trajectories using transition window. The trajectories + * have to be equally and uniformly + * discretized. + * @param planning_scene: The scene planning is occurring in. + * @param req: following fields need to be filled for a valid request: + * - group_name : name of the planning group + * - link_name : name of the target link + * - first_trajectory: Joint trajectory stops at end point. + * The last point must be the same as the first point + * of the second trajectory. + * - second trajectory: Joint trajectory stops at end point. + * The first point must be the same as the last point + * of the first trajectory. + * - blend_radius: The blend radius determines a sphere with the + * intersection point of the two trajectories + * as the center. Trajectory blending happens inside of + * this sphere. + * @param res: following fields are returned as response by the blend + * algorithm + * - group_name : name of the planning group + * - first_trajectory: Part of the first original trajectory which is + * outside of the blend sphere. + * - blend_trajectory: Joint trajectory connecting the first and second + * trajectories without stop. + * The first waypoint has non-zero time from start. + * - second trajectory: Part of the second original trajectory which is + * outside of the blend sphere. + * The first waypoint has non-zero time from start. + * error_code: information of failed blend + * @return true if succeed + */ + bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene, + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + pilz_industrial_motion_planner::TrajectoryBlendResponse& res) override; + +private: + /** + * @brief validate trajectory blend request + * @param req + * @param sampling_time: get the same sampling time of the two input + * trajectories + * @param error_code + * @return + */ + bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + moveit_msgs::msg::MoveItErrorCodes& error_code) const; + /** + * @brief searchBlendPoint + * @param req: trajectory blend request + * @param first_interse_index: index of the first point of the first + * trajectory that is inside the blend sphere + * @param second_interse_index: index of the last point of the second + * trajectory that is still inside the blend sphere + */ + bool searchIntersectionPoints(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + std::size_t& first_interse_index, std::size_t& second_interse_index) const; + + /** + * @brief Determine how the second trajectory should be aligned with the first + * trajectory for blend. + * Let tau_1 be the time of the first trajectory from the first_interse_index + * to the end and tau_2 the time of the + * second trajectory from the beginning to the second_interse_index: + * - if tau_1 > tau_2:
+ * align the end of the first trajectory with second_interse_index + * 
+   *    first traj:  |-------------|--------!--------------|
+   *    second traj:                        |--------------|-------------------|
+   *    blend phase:               |-----------------------|
+   *
+ * - else:
+ * align the first_interse_index with the beginning of the second + * trajectory + * 
+   *    first traj:  |-------------|-----------------------|
+   *    second traj: |-----------------------!----------|-------------------|
+   *    blend phase:               |----------------------------------|
+   *
+ * + * @param req: trajectory blend request + * @param first_interse_index: index of the intersection point between first + * trajectory and blend sphere + * @param second_interse_index: index of the intersection point between second + * trajectory and blend sphere + * @param blend_align_index: index on the first trajectory, to which the first + * point on the second trajectory should + * be aligned to for motion blend. It is now always same as + * first_interse_index + * @param blend_time: time of the motion blend period + */ + void determineTrajectoryAlignment(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + std::size_t first_interse_index, std::size_t second_interse_index, + std::size_t& blend_align_index) const; + + /** + * @brief blend two trajectories in Cartesian space, result in a + * MultiDOFJointTrajectory which consists + * of a list of transforms for the blend phase. + * @param req + * @param first_interse_index + * @param second_interse_index + * @param blend_begin_index + * @param sampling_time + * @param trajectory: the resulting blend trajectory inside the blending + * sphere + */ + void blendTrajectoryCartesian(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, + const std::size_t first_interse_index, const std::size_t second_interse_index, + const std::size_t blend_align_index, double sampling_time, + pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const; + +private: // static members + // Constant to check for equality of values. + static constexpr double EPSILON = 1e-4; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index 0adf6d1385..f66bf3ac50 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,206 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief compute the inverse kinematics of a given pose, also check robot self - * collision - * @param scene: planning scene - * @param group_name: name of planning group - * @param link_name: name of target link - * @param pose: target pose in IK solver Frame - * @param frame_id: reference frame of the target pose - * @param seed: seed state of IK solver - * @param solution: solution of IK - * @param check_self_collision: true to enable self collision checking after IK - * computation - * @param timeout: timeout for IK, if not set the default solver timeout is used - * @return true if succeed - */ -bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, - const std::map& seed, std::map& solution, - bool check_self_collision = true, const double timeout = 0.0); - -bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, - const std::string& link_name, const geometry_msgs::msg::Pose& pose, const std::string& frame_id, - const std::map& seed, std::map& solution, - bool check_self_collision = true, const double timeout = 0.0); - -/** - * @brief compute the pose of a link at a given robot state - * @param robot_state: an arbitrary robot state (with collision objects attached) - * @param link_name: target link name - * @param joint_state: joint positions of this group - * @param pose: pose of the link in base frame of robot model - * @return true if succeed - */ -bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, - const std::map& joint_state, Eigen::Isometry3d& pose); - -bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, - const std::vector& joint_names, const std::vector& joint_positions, - Eigen::Isometry3d& pose); - -/** - * @brief verify the velocity/acceleration limits of current sample (based on - * backward difference computation) - * v(k) = [x(k) - x(k-1)]/[t(k) - t(k-1)] - * a(k) = [v(k) - v(k-1)]/[t(k) - t(k-2)]*2 - * @param position_last: position of last sample - * @param velocity_last: velocity of last sample - * @param position_current: position of current sample - * @param duration_last: duration of last sample - * @param duration_current: duration of current sample - * @param joint_limits: joint limits - * @return - */ -bool verifySampleJointLimits(const std::map& position_last, - const std::map& velocity_last, - const std::map& position_current, double duration_last, - double duration_current, const JointLimitsContainer& joint_limits); - -/** - * @brief Generate joint trajectory from a KDL Cartesian trajectory - * @param scene: planning scene - * @param joint_limits: joint limits - * @param trajectory: KDL Cartesian trajectory - * @param group_name: name of the planning group - * @param link_name: name of the target robot link - * @param initial_joint_position: initial joint positions, needed for selecting - * the ik solution - * @param sampling_time: sampling time of the generated trajectory - * @param joint_trajectory: output as robot joint trajectory, first and last - * point will have zero velocity - * and acceleration - * @param error_code: detailed error information - * @param check_self_collision: check for self collision during creation - * @return true if succeed - */ -bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, - const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, - const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, double sampling_time, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, - moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); - -/** - * @brief Generate joint trajectory from a MultiDOFJointTrajectory - * @param scene: planning scene - * @param trajectory: Cartesian trajectory - * @param info: motion plan information - * @param sampling_time - * @param joint_trajectory - * @param error_code - * @return true if succeed - */ -bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, - const JointLimitsContainer& joint_limits, - const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, - const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, - const std::map& initial_joint_velocity, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, - moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); - -/** - * @brief Determines the sampling time and checks that both trajectroies use the - * same sampling time. - * @return TRUE if the sampling time is equal between all given points (except - * the last two points - * of each trajectory), otherwise FALSE. - */ -bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr& first_trajectory, - const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double EPSILON, - double& sampling_time); - -/** - * @brief Check if the two robot states have the same joint - * position/velocity/acceleration. - * - * @param joint_group_name The name of the joint group. - * @param epsilon Constants defining how close the joint - * position/velocity/acceleration have to be to be - * recognized as equal. - * - * @return True if joint positions, joint velocities and joint accelerations are - * equal, otherwise false. - */ -bool isRobotStateEqual(const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, - const std::string& joint_group_name, double epsilon); - -/** - * @brief check if the robot state have zero velocity/acceleration - * @param state - * @param group - * @param EPSILON - * @return - */ -bool isRobotStateStationary(const moveit::core::RobotState& state, const std::string& group, double EPSILON); - -/** - * @brief Performs a linear search for the intersection point of the trajectory - * with the blending radius. - * @param center_position Center of blending sphere. - * @param r Radius of blending sphere. - * @param traj The trajectory. - * @param inverseOrder TRUE: Farthest element from blending sphere center is - * located at the - * smallest index of trajectroy. - * @param index The intersection index which has to be determined. - */ -bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, const double r, - const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, - std::size_t& index); - -bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, - double r); - -/** - * @brief Checks if current robot state is in self collision. - * @param scene: planning scene. - * @param test_for_self_collision Flag to deactivate this check during IK. - * @param robot_model: robot kinematics model. - * @param state Robot state instance used for . - * @param group - * @param ik_solution - * @return - */ -bool isStateColliding(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState* state, - const moveit::core::JointModelGroup* const group, const double* const ik_solution); -} // namespace pilz_industrial_motion_planner - -void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat); - -/** - * @brief Adapt goal pose, defined by position+orientation, to consider offset - * @param constraint to apply offset to - * @param offset to apply to the constraint - * @param orientation to apply to the offset - * @return final goal pose - */ -Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point& position, - const geometry_msgs::msg::Quaternion& orientation, - const geometry_msgs::msg::Vector3& offset); - -/** - * @brief Conviencency method, passing args from a goal constraint - * @param goal goal constraint - * @return final goal pose - */ -Eigen::Isometry3d getConstraintPose(const moveit_msgs::msg::Constraints& goal); +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp new file mode 100644 index 0000000000..1f898b4ea0 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -0,0 +1,238 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief compute the inverse kinematics of a given pose, also check robot self + * collision + * @param scene: planning scene + * @param group_name: name of planning group + * @param link_name: name of target link + * @param pose: target pose in IK solver Frame + * @param frame_id: reference frame of the target pose + * @param seed: seed state of IK solver + * @param solution: solution of IK + * @param check_self_collision: true to enable self collision checking after IK + * computation + * @param timeout: timeout for IK, if not set the default solver timeout is used + * @return true if succeed + */ +bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id, + const std::map& seed, std::map& solution, + bool check_self_collision = true, const double timeout = 0.0); + +bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, + const std::string& link_name, const geometry_msgs::msg::Pose& pose, const std::string& frame_id, + const std::map& seed, std::map& solution, + bool check_self_collision = true, const double timeout = 0.0); + +/** + * @brief compute the pose of a link at a given robot state + * @param robot_state: an arbitrary robot state (with collision objects attached) + * @param link_name: target link name + * @param joint_state: joint positions of this group + * @param pose: pose of the link in base frame of robot model + * @return true if succeed + */ +bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, + const std::map& joint_state, Eigen::Isometry3d& pose); + +bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name, + const std::vector& joint_names, const std::vector& joint_positions, + Eigen::Isometry3d& pose); + +/** + * @brief verify the velocity/acceleration limits of current sample (based on + * backward difference computation) + * v(k) = [x(k) - x(k-1)]/[t(k) - t(k-1)] + * a(k) = [v(k) - v(k-1)]/[t(k) - t(k-2)]*2 + * @param position_last: position of last sample + * @param velocity_last: velocity of last sample + * @param position_current: position of current sample + * @param duration_last: duration of last sample + * @param duration_current: duration of current sample + * @param joint_limits: joint limits + * @return + */ +bool verifySampleJointLimits(const std::map& position_last, + const std::map& velocity_last, + const std::map& position_current, double duration_last, + double duration_current, const JointLimitsContainer& joint_limits); + +/** + * @brief Generate joint trajectory from a KDL Cartesian trajectory + * @param scene: planning scene + * @param joint_limits: joint limits + * @param trajectory: KDL Cartesian trajectory + * @param group_name: name of the planning group + * @param link_name: name of the target robot link + * @param initial_joint_position: initial joint positions, needed for selecting + * the ik solution + * @param sampling_time: sampling time of the generated trajectory + * @param joint_trajectory: output as robot joint trajectory, first and last + * point will have zero velocity + * and acceleration + * @param error_code: detailed error information + * @param check_self_collision: check for self collision during creation + * @return true if succeed + */ +bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, + const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, double sampling_time, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, + moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); + +/** + * @brief Generate joint trajectory from a MultiDOFJointTrajectory + * @param scene: planning scene + * @param trajectory: Cartesian trajectory + * @param info: motion plan information + * @param sampling_time + * @param joint_trajectory + * @param error_code + * @return true if succeed + */ +bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, + const JointLimitsContainer& joint_limits, + const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, + const std::string& group_name, const std::string& link_name, + const std::map& initial_joint_position, + const std::map& initial_joint_velocity, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, + moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); + +/** + * @brief Determines the sampling time and checks that both trajectroies use the + * same sampling time. + * @return TRUE if the sampling time is equal between all given points (except + * the last two points + * of each trajectory), otherwise FALSE. + */ +bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr& first_trajectory, + const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double EPSILON, + double& sampling_time); + +/** + * @brief Check if the two robot states have the same joint + * position/velocity/acceleration. + * + * @param joint_group_name The name of the joint group. + * @param epsilon Constants defining how close the joint + * position/velocity/acceleration have to be to be + * recognized as equal. + * + * @return True if joint positions, joint velocities and joint accelerations are + * equal, otherwise false. + */ +bool isRobotStateEqual(const moveit::core::RobotState& state1, const moveit::core::RobotState& state2, + const std::string& joint_group_name, double epsilon); + +/** + * @brief check if the robot state have zero velocity/acceleration + * @param state + * @param group + * @param EPSILON + * @return + */ +bool isRobotStateStationary(const moveit::core::RobotState& state, const std::string& group, double EPSILON); + +/** + * @brief Performs a linear search for the intersection point of the trajectory + * with the blending radius. + * @param center_position Center of blending sphere. + * @param r Radius of blending sphere. + * @param traj The trajectory. + * @param inverseOrder TRUE: Farthest element from blending sphere center is + * located at the + * smallest index of trajectroy. + * @param index The intersection index which has to be determined. + */ +bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, const double r, + const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, + std::size_t& index); + +bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, + double r); + +/** + * @brief Checks if current robot state is in self collision. + * @param scene: planning scene. + * @param test_for_self_collision Flag to deactivate this check during IK. + * @param robot_model: robot kinematics model. + * @param state Robot state instance used for . + * @param group + * @param ik_solution + * @return + */ +bool isStateColliding(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState* state, + const moveit::core::JointModelGroup* const group, const double* const ik_solution); +} // namespace pilz_industrial_motion_planner + +void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat); + +/** + * @brief Adapt goal pose, defined by position+orientation, to consider offset + * @param constraint to apply offset to + * @param offset to apply to the constraint + * @param orientation to apply to the offset + * @return final goal pose + */ +Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point& position, + const geometry_msgs::msg::Quaternion& orientation, + const geometry_msgs::msg::Vector3& offset); + +/** + * @brief Conviencency method, passing args from a goal constraint + * @param goal goal constraint + * @return final goal pose + */ +Eigen::Isometry3d getConstraintPose(const moveit_msgs::msg::Constraints& goal); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h index daad273bf9..3feaeac67d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,90 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief Exception storing an moveit_msgs::msg::MoveItErrorCodes value. - */ -class MoveItErrorCodeException : public std::runtime_error -{ -public: - MoveItErrorCodeException(const std::string& msg); - -protected: - MoveItErrorCodeException(const MoveItErrorCodeException&) = default; - MoveItErrorCodeException(MoveItErrorCodeException&&) = default; - ~MoveItErrorCodeException() override = default; - - MoveItErrorCodeException& operator=(const MoveItErrorCodeException&) = default; - MoveItErrorCodeException& operator=(MoveItErrorCodeException&&) = default; - -public: - virtual const moveit_msgs::msg::MoveItErrorCodes::_val_type& getErrorCode() const = 0; -}; - -template -class TemplatedMoveItErrorCodeException : public MoveItErrorCodeException -{ -public: - TemplatedMoveItErrorCodeException(const std::string& msg); - TemplatedMoveItErrorCodeException(const std::string& msg, - const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code); - -public: - const moveit_msgs::msg::MoveItErrorCodes::_val_type& getErrorCode() const override; - -private: - const moveit_msgs::msg::MoveItErrorCodes::_val_type error_code_{ ERROR_CODE }; -}; - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -inline MoveItErrorCodeException::MoveItErrorCodeException(const std::string& msg) : std::runtime_error(msg) -{ -} - -template -inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException(const std::string& msg) - : MoveItErrorCodeException(msg) -{ -} - -template -inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException( - const std::string& msg, const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code) - : MoveItErrorCodeException(msg), error_code_(error_code) -{ -} - -template -inline const moveit_msgs::msg::MoveItErrorCodes::_val_type& -TemplatedMoveItErrorCodeException::getErrorCode() const -{ - return error_code_; -} - -/* - * @brief Macro to automatically generate a derived class of - * the MoveItErrorCodeException class. - */ -#define CREATE_MOVEIT_ERROR_CODE_EXCEPTION(EXCEPTION_CLASS_NAME, ERROR_CODE) \ - class EXCEPTION_CLASS_NAME : public TemplatedMoveItErrorCodeException \ - { \ - public: \ - EXCEPTION_CLASS_NAME(const std::string& msg) : TemplatedMoveItErrorCodeException(msg) \ - { \ - } \ - \ - EXCEPTION_CLASS_NAME(const std::string& msg, const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code) \ - : TemplatedMoveItErrorCodeException(msg, error_code) \ - { \ - } \ - } - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp new file mode 100644 index 0000000000..daad273bf9 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp @@ -0,0 +1,122 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Exception storing an moveit_msgs::msg::MoveItErrorCodes value. + */ +class MoveItErrorCodeException : public std::runtime_error +{ +public: + MoveItErrorCodeException(const std::string& msg); + +protected: + MoveItErrorCodeException(const MoveItErrorCodeException&) = default; + MoveItErrorCodeException(MoveItErrorCodeException&&) = default; + ~MoveItErrorCodeException() override = default; + + MoveItErrorCodeException& operator=(const MoveItErrorCodeException&) = default; + MoveItErrorCodeException& operator=(MoveItErrorCodeException&&) = default; + +public: + virtual const moveit_msgs::msg::MoveItErrorCodes::_val_type& getErrorCode() const = 0; +}; + +template +class TemplatedMoveItErrorCodeException : public MoveItErrorCodeException +{ +public: + TemplatedMoveItErrorCodeException(const std::string& msg); + TemplatedMoveItErrorCodeException(const std::string& msg, + const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code); + +public: + const moveit_msgs::msg::MoveItErrorCodes::_val_type& getErrorCode() const override; + +private: + const moveit_msgs::msg::MoveItErrorCodes::_val_type error_code_{ ERROR_CODE }; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +inline MoveItErrorCodeException::MoveItErrorCodeException(const std::string& msg) : std::runtime_error(msg) +{ +} + +template +inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException(const std::string& msg) + : MoveItErrorCodeException(msg) +{ +} + +template +inline TemplatedMoveItErrorCodeException::TemplatedMoveItErrorCodeException( + const std::string& msg, const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code) + : MoveItErrorCodeException(msg), error_code_(error_code) +{ +} + +template +inline const moveit_msgs::msg::MoveItErrorCodes::_val_type& +TemplatedMoveItErrorCodeException::getErrorCode() const +{ + return error_code_; +} + +/* + * @brief Macro to automatically generate a derived class of + * the MoveItErrorCodeException class. + */ +#define CREATE_MOVEIT_ERROR_CODE_EXCEPTION(EXCEPTION_CLASS_NAME, ERROR_CODE) \ + class EXCEPTION_CLASS_NAME : public TemplatedMoveItErrorCodeException \ + { \ + public: \ + EXCEPTION_CLASS_NAME(const std::string& msg) : TemplatedMoveItErrorCodeException(msg) \ + { \ + } \ + \ + EXCEPTION_CLASS_NAME(const std::string& msg, const moveit_msgs::msg::MoveItErrorCodes::_val_type& error_code) \ + : TemplatedMoveItErrorCodeException(msg, error_code) \ + { \ + } \ + } + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 69debc2b98..fb7844fd82 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,268 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace pilz_industrial_motion_planner -{ -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, - moveit_msgs::msg::MoveItErrorCodes::FAILURE); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(VelocityScalingIncorrect, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(AccelerationScalingIncorrect, - moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPlanningGroup, moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoJointNamesInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(SizeMismatchInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfStartStateOutOfRange, - moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NonZeroVelocityInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NotExactlyOneGoalConstraintGiven, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OnlyOneGoalTypeAllowed, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateGoalStateMismatch, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointConstraintDoesNotBelongToGroup, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfGoalOutOfRange, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionConstraintNameMissing, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OrientationConstraintNameMissing, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionOrientationConstraintNameMismatch, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoIKSolverAvailable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); - -/** - * @brief Base class of trajectory generators - * - * Note: All derived classes cannot have a start velocity - */ -class TrajectoryGenerator -{ -public: - TrajectoryGenerator(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits) - : robot_model_(robot_model), planner_limits_(planner_limits), clock_(std::make_unique()) - { - } - - virtual ~TrajectoryGenerator() = default; - - /** - * @brief generate robot trajectory with given sampling time - * @param req: motion plan request - * @param res: motion plan response - * @param sampling_time: sampling time of the generate trajectory - */ - void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, double sampling_time = 0.1); - -protected: - /** - * @brief This class is used to extract needed information from motion plan request. - */ - class MotionPlanInfo - { - public: - MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req); - - std::string group_name; - std::string link_name; - Eigen::Isometry3d start_pose; - Eigen::Isometry3d goal_pose; - std::map start_joint_position; - std::map goal_joint_position; - std::pair circ_path_point; - planning_scene::PlanningSceneConstPtr start_scene; // scene with updated start state - }; - - /** - * @brief build cartesian velocity profile for the path - * - * Uses the path to get the cartesian length and the angular distance from - * start to goal. - * The trap profile returns uses the longer distance of translational and - * rotational motion. - */ - std::unique_ptr cartesianTrapVelocityProfile(double max_velocity_scaling_factor, - double max_acceleration_scaling_factor, - const std::unique_ptr& path) const; - -private: - virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const; - - /** - * @brief Extract needed information from a motion plan request in order to - * simplify - * further usages. - * @param scene: planning scene - * @param req: motion plan request - * @param info: information extracted from motion plan request which is - * necessary for the planning - */ - virtual void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0; - - virtual void plan(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; - -private: - /** - * @brief Validate the motion plan request based on the common requirements of - * trajectroy generator - * Checks that: - * - req.max_velocity_scaling_factor [0.0001, 1], - * moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN on failure - * - req.max_acceleration_scaling_factor [0.0001, 1] , - * moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN on - * failure - * - req.group_name is a JointModelGroup of the Robotmodel, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME on - * failure - * - req.start_state.joint_state is not empty, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on failure - * - req.start_state.joint_state is within the limits, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on - * failure - * - req.start_state.joint_state is all zero, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on failure - * - req.goal_constraints must have exactly 1 defined cartesian order joint - * constraint - * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure - * A joint goal is checked for: - * - StartState joint-names matching goal joint-names, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on - * failure - * - Being defined in the req.group_name JointModelGroup - * - Being with the defined limits - * A cartesian goal is checked for: - * - A defined link_name for the constraint, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure - * - Matching link_name for position and orientation constraints, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure - * - A IK solver exists for the given req.group_name and constraint - * link_name, - * moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION on failure - * - A goal pose define in - * position_constraints[0].constraint_region.primitive_poses, - * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure - * @param req: motion plan request - */ - void validateRequest(const planning_interface::MotionPlanRequest& req, const moveit::core::RobotState& rstate) const; - - /** - * @brief set MotionPlanResponse from joint trajectory - */ - void setSuccessResponse(const moveit::core::RobotState& start_rs, const std::string& group_name, - const trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const; - - void setFailureResponse(const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const; - - void checkForValidGroupName(const std::string& group_name) const; - - /** - * @brief Validate that the start state of the request matches the - * requirements of the trajectory generator. - * - * These requirements are: - * - Names of the joints and given joint position match in size and are - * non-zero - * - The start state is within the position limits - * - The start state velocity is below - * TrajectoryGenerator::VELOCITY_TOLERANCE - */ - void checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const; - - void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, - const std::string& group_name, const moveit::core::RobotState& rstate) const; - - void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::string& group_name) const; - - void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, - const moveit::core::RobotState& robot_state, - const moveit::core::JointModelGroup* const jmg) const; - -private: - /** - * @return joint state message including only active joints in group - */ - sensor_msgs::msg::JointState filterGroupValues(const sensor_msgs::msg::JointState& robot_state, - const std::string& group) const; - - /** - * @return True if scaling factor is valid, otherwise false. - */ - static bool isScalingFactorValid(double scaling_factor); - static void checkVelocityScaling(double scaling_factor); - static void checkAccelerationScaling(double scaling_factor); - - /** - * @return True if ONE position + ONE orientation constraint given, - * otherwise false. - */ - static bool isCartesianGoalGiven(const moveit_msgs::msg::Constraints& constraint); - - /** - * @return True if joint constraint given, otherwise false. - */ - static bool isJointGoalGiven(const moveit_msgs::msg::Constraints& constraint); - - /** - * @return True if ONLY joint constraint or - * ONLY cartesian constraint (position+orientation) given, otherwise false. - */ - static bool isOnlyOneGoalTypeGiven(const moveit_msgs::msg::Constraints& constraint); - -protected: - const moveit::core::RobotModelConstPtr robot_model_; - const pilz_industrial_motion_planner::LimitsContainer planner_limits_; - static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; - static constexpr double MAX_SCALING_FACTOR{ 1. }; - static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; - const std::unique_ptr clock_; -}; - -inline bool TrajectoryGenerator::isScalingFactorValid(double scaling_factor) -{ - return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); -} - -inline bool TrajectoryGenerator::isCartesianGoalGiven(const moveit_msgs::msg::Constraints& constraint) -{ - return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1; -} - -inline bool TrajectoryGenerator::isJointGoalGiven(const moveit_msgs::msg::Constraints& constraint) -{ - return !constraint.joint_constraints.empty(); -} - -inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(const moveit_msgs::msg::Constraints& constraint) -{ - return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint)) || - (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint)); -} - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp new file mode 100644 index 0000000000..c17bcc8be3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp @@ -0,0 +1,300 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(TrajectoryGeneratorInvalidLimitsException, + moveit_msgs::msg::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(VelocityScalingIncorrect, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(AccelerationScalingIncorrect, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPlanningGroup, moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoJointNamesInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(SizeMismatchInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfStartStateOutOfRange, + moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NonZeroVelocityInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NotExactlyOneGoalConstraintGiven, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OnlyOneGoalTypeAllowed, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateGoalStateMismatch, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointConstraintDoesNotBelongToGroup, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointsOfGoalOutOfRange, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionConstraintNameMissing, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OrientationConstraintNameMissing, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PositionOrientationConstraintNameMismatch, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoIKSolverAvailable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePoseGiven, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + +/** + * @brief Base class of trajectory generators + * + * Note: All derived classes cannot have a start velocity + */ +class TrajectoryGenerator +{ +public: + TrajectoryGenerator(const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits) + : robot_model_(robot_model), planner_limits_(planner_limits), clock_(std::make_unique()) + { + } + + virtual ~TrajectoryGenerator() = default; + + /** + * @brief generate robot trajectory with given sampling time + * @param req: motion plan request + * @param res: motion plan response + * @param sampling_time: sampling time of the generate trajectory + */ + void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, double sampling_time = 0.1); + +protected: + /** + * @brief This class is used to extract needed information from motion plan request. + */ + class MotionPlanInfo + { + public: + MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req); + + std::string group_name; + std::string link_name; + Eigen::Isometry3d start_pose; + Eigen::Isometry3d goal_pose; + std::map start_joint_position; + std::map goal_joint_position; + std::pair circ_path_point; + planning_scene::PlanningSceneConstPtr start_scene; // scene with updated start state + }; + + /** + * @brief build cartesian velocity profile for the path + * + * Uses the path to get the cartesian length and the angular distance from + * start to goal. + * The trap profile returns uses the longer distance of translational and + * rotational motion. + */ + std::unique_ptr cartesianTrapVelocityProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, + const std::unique_ptr& path) const; + +private: + virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const; + + /** + * @brief Extract needed information from a motion plan request in order to + * simplify + * further usages. + * @param scene: planning scene + * @param req: motion plan request + * @param info: information extracted from motion plan request which is + * necessary for the planning + */ + virtual void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0; + + virtual void plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; + +private: + /** + * @brief Validate the motion plan request based on the common requirements of + * trajectroy generator + * Checks that: + * - req.max_velocity_scaling_factor [0.0001, 1], + * moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN on failure + * - req.max_acceleration_scaling_factor [0.0001, 1] , + * moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN on + * failure + * - req.group_name is a JointModelGroup of the Robotmodel, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME on + * failure + * - req.start_state.joint_state is not empty, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * - req.start_state.joint_state is within the limits, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on + * failure + * - req.start_state.joint_state is all zero, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE on failure + * - req.goal_constraints must have exactly 1 defined cartesian order joint + * constraint + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * A joint goal is checked for: + * - StartState joint-names matching goal joint-names, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on + * failure + * - Being defined in the req.group_name JointModelGroup + * - Being with the defined limits + * A cartesian goal is checked for: + * - A defined link_name for the constraint, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * - Matching link_name for position and orientation constraints, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * - A IK solver exists for the given req.group_name and constraint + * link_name, + * moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION on failure + * - A goal pose define in + * position_constraints[0].constraint_region.primitive_poses, + * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure + * @param req: motion plan request + */ + void validateRequest(const planning_interface::MotionPlanRequest& req, const moveit::core::RobotState& rstate) const; + + /** + * @brief set MotionPlanResponse from joint trajectory + */ + void setSuccessResponse(const moveit::core::RobotState& start_rs, const std::string& group_name, + const trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const; + + void setFailureResponse(const rclcpp::Time& planning_start, planning_interface::MotionPlanResponse& res) const; + + void checkForValidGroupName(const std::string& group_name) const; + + /** + * @brief Validate that the start state of the request matches the + * requirements of the trajectory generator. + * + * These requirements are: + * - Names of the joints and given joint position match in size and are + * non-zero + * - The start state is within the position limits + * - The start state velocity is below + * TrajectoryGenerator::VELOCITY_TOLERANCE + */ + void checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const; + + void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, + const std::string& group_name, const moveit::core::RobotState& rstate) const; + + void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::string& group_name) const; + + void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint, + const moveit::core::RobotState& robot_state, + const moveit::core::JointModelGroup* const jmg) const; + +private: + /** + * @return joint state message including only active joints in group + */ + sensor_msgs::msg::JointState filterGroupValues(const sensor_msgs::msg::JointState& robot_state, + const std::string& group) const; + + /** + * @return True if scaling factor is valid, otherwise false. + */ + static bool isScalingFactorValid(double scaling_factor); + static void checkVelocityScaling(double scaling_factor); + static void checkAccelerationScaling(double scaling_factor); + + /** + * @return True if ONE position + ONE orientation constraint given, + * otherwise false. + */ + static bool isCartesianGoalGiven(const moveit_msgs::msg::Constraints& constraint); + + /** + * @return True if joint constraint given, otherwise false. + */ + static bool isJointGoalGiven(const moveit_msgs::msg::Constraints& constraint); + + /** + * @return True if ONLY joint constraint or + * ONLY cartesian constraint (position+orientation) given, otherwise false. + */ + static bool isOnlyOneGoalTypeGiven(const moveit_msgs::msg::Constraints& constraint); + +protected: + const moveit::core::RobotModelConstPtr robot_model_; + const pilz_industrial_motion_planner::LimitsContainer planner_limits_; + static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; + static constexpr double MAX_SCALING_FACTOR{ 1. }; + static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; + const std::unique_ptr clock_; +}; + +inline bool TrajectoryGenerator::isScalingFactorValid(double scaling_factor) +{ + return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); +} + +inline bool TrajectoryGenerator::isCartesianGoalGiven(const moveit_msgs::msg::Constraints& constraint) +{ + return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1; +} + +inline bool TrajectoryGenerator::isJointGoalGiven(const moveit_msgs::msg::Constraints& constraint) +{ + return !constraint.joint_constraints.empty(); +} + +inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(const moveit_msgs::msg::Constraints& constraint) +{ + return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint)) || + (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint)); +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index ef4ab6c814..3dd4e9828b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,79 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include - -#include - -namespace pilz_industrial_motion_planner -{ -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleToSmall, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CenterPointDifferentRadius, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircTrajectoryConversionFailure, - moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPathConstraintName, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPositionConstraints, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePose, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, - moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, - moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); - -/** - * @brief This class implements a trajectory generator of arcs in Cartesian - * space. - * The arc is specified by a start pose, a goal pose and a interim point on the - * arc, - * or a point as the center of the circle which forms the arc. Complete circle - * is not - * covered by this generator. - */ -class TrajectoryGeneratorCIRC : public TrajectoryGenerator -{ -public: - /** - * @brief Constructor of CIRC Trajectory Generator. - * - * @param planner_limits Limits in joint and Cartesian spaces - * - * @throw TrajectoryGeneratorInvalidLimitsException - * - */ - TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits, - const std::string& group_name); - -private: - void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; - - void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; - - void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, double sampling_time, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; - - /** - * @brief Construct a KDL::Path object for a Cartesian path of an arc. - * - * @return A unique pointer of the path object, null_ptr in case of an error. - * - * @throws CircleNoPlane if the plane in which the circle resides, - * could not be determined. - * - * @throws CircleToSmall if the specified circ radius is to small. - * - * @throws CenterPointDifferentRadius if the distances between start-center - * and goal-center are different. - */ - std::unique_ptr setPathCIRC(const MotionPlanInfo& info) const; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp new file mode 100644 index 0000000000..c73c89a5fa --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleNoPlane, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircleToSmall, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CenterPointDifferentRadius, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircTrajectoryConversionFailure, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownPathConstraintName, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPositionConstraints, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoPrimitivePose, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint, + moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch, + moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a trajectory generator of arcs in Cartesian + * space. + * The arc is specified by a start pose, a goal pose and a interim point on the + * arc, + * or a point as the center of the circle which forms the arc. Complete circle + * is not + * covered by this generator. + */ +class TrajectoryGeneratorCIRC : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of CIRC Trajectory Generator. + * + * @param planner_limits Limits in joint and Cartesian spaces + * + * @throw TrajectoryGeneratorInvalidLimitsException + * + */ + TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); + +private: + void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; + + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, double sampling_time, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; + + /** + * @brief Construct a KDL::Path object for a Cartesian path of an arc. + * + * @return A unique pointer of the path object, null_ptr in case of an error. + * + * @throws CircleNoPlane if the plane in which the circle resides, + * could not be determined. + * + * @throws CircleToSmall if the specified circ radius is to small. + * + * @throws CenterPointDifferentRadius if the distances between start-center + * and goal-center are different. + */ + std::unique_ptr setPathCIRC(const MotionPlanInfo& info) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index 61592e0794..fbf685c7f2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,54 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include - -#include -#include - -namespace pilz_industrial_motion_planner -{ -// TODO date type of units - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE); - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); - -/** - * @brief This class implements a linear trajectory generator in Cartesian - * space. - * The Cartesian trajetory are based on trapezoid velocity profile. - */ -class TrajectoryGeneratorLIN : public TrajectoryGenerator -{ -public: - /** - * @brief Constructor of LIN Trajectory Generator - * @throw TrajectoryGeneratorInvalidLimitsException - * @param model: robot model - * @param planner_limits: limits in joint and Cartesian spaces - */ - TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits, - const std::string& group_name); - -private: - void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; - - void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, double sampling_time, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; - - /** - * @brief construct a KDL::Path object for a Cartesian straight line - * @return a unique pointer of the path object. null_ptr in case of an error. - */ - std::unique_ptr setPathLIN(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp new file mode 100644 index 0000000000..3c301b61e3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp @@ -0,0 +1,86 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a linear trajectory generator in Cartesian + * space. + * The Cartesian trajetory are based on trapezoid velocity profile. + */ +class TrajectoryGeneratorLIN : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of LIN Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: robot model + * @param planner_limits: limits in joint and Cartesian spaces + */ + TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); + +private: + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, double sampling_time, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; + + /** + * @brief construct a KDL::Path object for a Cartesian straight line + * @return a unique pointer of the path object. null_ptr in case of an error. + */ + std::unique_ptr setPathLIN(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index cc73ff4839..3404a24268 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,64 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include -#include -#include - -namespace pilz_industrial_motion_planner -{ -// TODO date type of units - -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpVelocityProfileSyncFailed, moveit_msgs::msg::MoveItErrorCodes::FAILURE); -CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpNoIkSolutionForGoalPose, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); - -/** - * @brief This class implements a point-to-point trajectory generator based on - * VelocityProfileATrap. - */ -class TrajectoryGeneratorPTP : public TrajectoryGenerator -{ -public: - /** - * @brief Constructor of PTP Trajectory Generator - * @throw TrajectoryGeneratorInvalidLimitsException - * @param model: a map of joint limits information - */ - TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits, - const std::string& group_name); - -private: - void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override; - - /** - * @brief plan ptp joint trajectory with zero start velocity - * @param start_pos - * @param goal_pos - * @param joint_trajectory - * @param group_name - * @param velocity_scaling_factor - * @param acceleration_scaling_factor - * @param sampling_time - */ - void planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, double velocity_scaling_factor, - double acceleration_scaling_factor, double sampling_time); - - void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, double sampling_time, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; - -private: - static constexpr double MIN_MOVEMENT = 0.001; - pilz_industrial_motion_planner::JointLimitsContainer joint_limits_; - // most strict joint limits - JointLimit most_strict_limit_; -}; - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp new file mode 100644 index 0000000000..f4514c123f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp @@ -0,0 +1,96 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpVelocityProfileSyncFailed, moveit_msgs::msg::MoveItErrorCodes::FAILURE); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PtpNoIkSolutionForGoalPose, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a point-to-point trajectory generator based on + * VelocityProfileATrap. + */ +class TrajectoryGeneratorPTP : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of PTP Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: a map of joint limits information + */ + TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); + +private: + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override; + + /** + * @brief plan ptp joint trajectory with zero start velocity + * @param start_pos + * @param goal_pos + * @param joint_trajectory + * @param group_name + * @param velocity_scaling_factor + * @param acceleration_scaling_factor + * @param sampling_time + */ + void planPTP(const std::map& start_pos, const std::map& goal_pos, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, double sampling_time); + + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, double sampling_time, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; + +private: + static constexpr double MIN_MOVEMENT = 0.001; + pilz_industrial_motion_planner::JointLimitsContainer joint_limits_; + // most strict joint limits + JointLimit most_strict_limit_; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h index 62bf589757..b499ddf931 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,183 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace pilz_industrial_motion_planner -{ -/** - * @brief A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. - * Differences to VelocityProfile_Trap: - * - Maximal acceleration and deceleration can be different, resulting - * an asymmetric trapezoid shaped velocity profile. - * - Function to generate full synchronized PTP trajectory is provided. - * - Function to generate trapezoid shaped velocity profile with start - * velocity. - */ -class VelocityProfileATrap : public KDL::VelocityProfile -{ -public: - /** - * @brief Constructor - * @param max_vel: maximal velocity (absolute value, always positive) - * @param max_acc: maximal acceleration (absolute value, always positive) - * @param max_dec: maximal deceleration (absolute value, always positive) - */ - VelocityProfileATrap(double max_vel = 0, double max_acc = 0, double max_dec = 0); - - /** - * @brief compute the fastest profile - * Algorithm: - * - compute the minimal distance which is needed to reach maximal velocity - * - if maximal velocity can be reached - * - compute the coefficients of the trajectory - * - if maximal velocity can not be reached - * - compute the new velocity can be reached - * - compute the coefficients based on this new velocity - * - * @param pos1: start position - * @param pos2: goal position - */ - void SetProfile(double pos1, double pos2) override; - - /** - * @brief Profile scaled by the total duration - * @param pos1: start position - * @param pos2: goal position - * @param duration: trajectory duration (must be longer than fastest case, - * otherwise will be ignored) - */ - void SetProfileDuration(double pos1, double pos2, double duration) override; - - /** - * @brief Profile with given acceleration/constant/deceleration durations. - * Each duration must obey the maximal velocity/acceleration/deceleration - * constraints. - * Otherwise the operation will be ignored. - * Algorithm: - * - compute the maximal velocity of given durations - * - compute the acceleration and deceleration of given duraitons - * - if limits are fulfilled - * - compute the coefficients - * @param pos1: start position - * @param pos2: goal position - * @param acc_duration: time of acceleration phase - * @param const_duration: time of constant phase - * @param dec_duration: time of deceleration phase - * @return true if the combination of three durations is valid - */ - bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3); - - /** - * @brief Profile with start velocity - * Note: This function is not general and is currently only used for live - * control (vel1*(pos2-pos1)>0). - * @param pos1: start position - * @param pos2: goal position - * @param vel1: start velocity - * @return - */ - bool setProfileStartVelocity(double pos1, double pos2, double vel1); - - /** - * @brief get the time of first phase - * @return - */ - double firstPhaseDuration() const - { - return t_a_; - } - /** - * @brief get the time of second phase - * @return - */ - double secondPhaseDuration() const - { - return t_b_; - } - /** - * @brief get the time of third phase - * @return - */ - double thirdPhaseDuration() const - { - return t_c_; - } - - /** - * @brief Compares two Asymmetric Trapezoidal Velocity Profiles. - * - * @return True if equal, false otherwise. - */ - bool operator==(const VelocityProfileATrap& other) const; - - /** - * @brief Duration - * @return total duration of the trajectory - */ - double Duration() const override; - /** - * @brief Get position at given time - * @param time - * @return - */ - double Pos(double time) const override; - /** - * @brief Get velocity at given time - * @param time - * @return - */ - double Vel(double time) const override; - /** - * @brief Get given acceleration/deceleration at given time - * @param time - * @return - */ - double Acc(double time) const override; - /** - * @brief Write basic information - * @param os - */ - void Write(std::ostream& os) const override; - /** - * @brief returns copy of current VelocityProfile object - * @return - */ - KDL::VelocityProfile* Clone() const override; - - friend std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p); // LCOV_EXCL_LINE - - ~VelocityProfileATrap() override; - -private: - /// helper functions - void setEmptyProfile(); - -private: - /// specification of the motion profile : - const double max_vel_; - const double max_acc_; - const double max_dec_; - double start_pos_; - double end_pos_; - - /// for initial velocity - double start_vel_; - - /// three phases of trapezoid - double a1_, a2_, a3_; /// coef. from ^0 -> ^2 of first phase - double b1_, b2_, b3_; /// of second phase - double c1_, c2_, c3_; /// of third phase - - /// time of three phases - double t_a_; /// the duration of first phase - double t_b_; /// the duration of second phase - double t_c_; /// the duration of third phase -}; - -std::ostream& operator<<(std::ostream& os, - const VelocityProfileATrap& p); // LCOV_EXCL_LINE - -} // namespace pilz_industrial_motion_planner +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.hpp new file mode 100644 index 0000000000..62bf589757 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/velocity_profile_atrap.hpp @@ -0,0 +1,215 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. + * Differences to VelocityProfile_Trap: + * - Maximal acceleration and deceleration can be different, resulting + * an asymmetric trapezoid shaped velocity profile. + * - Function to generate full synchronized PTP trajectory is provided. + * - Function to generate trapezoid shaped velocity profile with start + * velocity. + */ +class VelocityProfileATrap : public KDL::VelocityProfile +{ +public: + /** + * @brief Constructor + * @param max_vel: maximal velocity (absolute value, always positive) + * @param max_acc: maximal acceleration (absolute value, always positive) + * @param max_dec: maximal deceleration (absolute value, always positive) + */ + VelocityProfileATrap(double max_vel = 0, double max_acc = 0, double max_dec = 0); + + /** + * @brief compute the fastest profile + * Algorithm: + * - compute the minimal distance which is needed to reach maximal velocity + * - if maximal velocity can be reached + * - compute the coefficients of the trajectory + * - if maximal velocity can not be reached + * - compute the new velocity can be reached + * - compute the coefficients based on this new velocity + * + * @param pos1: start position + * @param pos2: goal position + */ + void SetProfile(double pos1, double pos2) override; + + /** + * @brief Profile scaled by the total duration + * @param pos1: start position + * @param pos2: goal position + * @param duration: trajectory duration (must be longer than fastest case, + * otherwise will be ignored) + */ + void SetProfileDuration(double pos1, double pos2, double duration) override; + + /** + * @brief Profile with given acceleration/constant/deceleration durations. + * Each duration must obey the maximal velocity/acceleration/deceleration + * constraints. + * Otherwise the operation will be ignored. + * Algorithm: + * - compute the maximal velocity of given durations + * - compute the acceleration and deceleration of given duraitons + * - if limits are fulfilled + * - compute the coefficients + * @param pos1: start position + * @param pos2: goal position + * @param acc_duration: time of acceleration phase + * @param const_duration: time of constant phase + * @param dec_duration: time of deceleration phase + * @return true if the combination of three durations is valid + */ + bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3); + + /** + * @brief Profile with start velocity + * Note: This function is not general and is currently only used for live + * control (vel1*(pos2-pos1)>0). + * @param pos1: start position + * @param pos2: goal position + * @param vel1: start velocity + * @return + */ + bool setProfileStartVelocity(double pos1, double pos2, double vel1); + + /** + * @brief get the time of first phase + * @return + */ + double firstPhaseDuration() const + { + return t_a_; + } + /** + * @brief get the time of second phase + * @return + */ + double secondPhaseDuration() const + { + return t_b_; + } + /** + * @brief get the time of third phase + * @return + */ + double thirdPhaseDuration() const + { + return t_c_; + } + + /** + * @brief Compares two Asymmetric Trapezoidal Velocity Profiles. + * + * @return True if equal, false otherwise. + */ + bool operator==(const VelocityProfileATrap& other) const; + + /** + * @brief Duration + * @return total duration of the trajectory + */ + double Duration() const override; + /** + * @brief Get position at given time + * @param time + * @return + */ + double Pos(double time) const override; + /** + * @brief Get velocity at given time + * @param time + * @return + */ + double Vel(double time) const override; + /** + * @brief Get given acceleration/deceleration at given time + * @param time + * @return + */ + double Acc(double time) const override; + /** + * @brief Write basic information + * @param os + */ + void Write(std::ostream& os) const override; + /** + * @brief returns copy of current VelocityProfile object + * @return + */ + KDL::VelocityProfile* Clone() const override; + + friend std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p); // LCOV_EXCL_LINE + + ~VelocityProfileATrap() override; + +private: + /// helper functions + void setEmptyProfile(); + +private: + /// specification of the motion profile : + const double max_vel_; + const double max_acc_; + const double max_dec_; + double start_pos_; + double end_pos_; + + /// for initial velocity + double start_vel_; + + /// three phases of trapezoid + double a1_, a2_, a3_; /// coef. from ^0 -> ^2 of first phase + double b1_, b2_, b3_; /// of second phase + double c1_, c2_, c3_; /// of third phase + + /// time of three phases + double t_a_; /// the duration of first phase + double t_b_; /// the duration of second phase + double t_c_; /// the duration of third phase +}; + +std::ostream& operator<<(std::ostream& os, + const VelocityProfileATrap& p); // LCOV_EXCL_LINE + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index 36458d5423..d222dd1e7c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include @@ -40,15 +40,15 @@ #include #include -#include -#include +#include +#include #include #include "cartesian_limits_parameters.hpp" -#include -#include -#include -#include +#include +#include +#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index e0bec40209..61a8655f86 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -32,11 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include -#include -#include +#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 55127d0334..1422a271d8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp index 42c6b51f88..fb6eef5015 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_validator.cpp @@ -32,13 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include -#include -#include +#include +#include -#include +#include bool pilz_industrial_motion_planner::JointLimitsValidator::validateAllPositionLimitsEqual( const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp index b99fc6c1fb..48b6774364 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index d6bec343d7..fd93f3e1f9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -36,21 +36,21 @@ // Modified by Pilz GmbH & Co. KG -#include +#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp index 8233f0b67e..935fc19c15 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -36,13 +36,13 @@ // Modified by Pilz GmbH & Co. KG -#include +#include -#include -#include -#include +#include +#include +#include -#include +#include #include namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp index 27589d2bb2..2c4515d4e6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index 0bde5aa46f..fbf19bf372 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -34,14 +34,14 @@ #include -#include +#include -#include -#include -#include +#include +#include +#include #include "cartesian_limits_parameters.hpp" -#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index 0513d22b49..2f84dde8d9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -32,11 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include -#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp index 7fa110d0f5..0d575fe496 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include pilz_industrial_motion_planner::PlanningContextLoader::PlanningContextLoader() : limits_set_(false), model_set_(false) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp index 7052088e2f..1ab4ab0471 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -35,10 +35,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp index 7deb454f1d..a3c261f9be 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -32,10 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp index 429ec42657..905ee08237 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 84e6679709..41deb7ccd7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -32,13 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include #include #include -#include +#include #include namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 7a4ea937bf..e4c5d6a4d8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include -#include +#include #include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 4b0482ca64..6f17fbc3a2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include @@ -41,10 +41,10 @@ #include #include -#include +#include #include -#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index e0ef8ce126..4e92524bfc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include #include #include @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 941290b90e..4b51582099 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -32,14 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include -#include +#include #include #include #include -#include +#include #include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index f7c45ac29a..6113b603eb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp index 76d0354cf0..836845f1c2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/velocity_profile_atrap.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp index 46dfa8300e..04d1f7403d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_list_manager.cpp @@ -41,23 +41,23 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include "test_utils.h" +#include "test_utils.hpp" -#include -#include +#include +#include const std::string ROBOT_DESCRIPTION_STR{ "robot_description" }; const std::string EMPTY_VALUE{ "" }; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_planning.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_planning.cpp index 26929b6513..c32cd99ae7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_planning.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_command_planning.cpp @@ -39,20 +39,20 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include -#include -#include +#include +#include #include #include #include -#include "test_utils.h" +#include "test_utils.hpp" const double EPSILON = 1.0e-6; const std::string PLAN_SERVICE_NAME = "/plan_kinematic_path"; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_get_solver_tip_frame.cpp index e52c50d487..dc430eff0a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_get_solver_tip_frame.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_get_solver_tip_frame.cpp @@ -39,12 +39,12 @@ #include -#include -#include -#include +#include +#include +#include #include -#include +#include static const std::string ROBOT_DESCRIPTION_PARAM{ "robot_description" }; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp index 498e743e6b..1fe6977944 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_plan_components_builder.cpp @@ -39,11 +39,11 @@ #include -#include -#include +#include +#include -#include -#include +#include +#include const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); const std::string ROBOT_DESCRIPTION_STR{ "robot_description" }; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp index 1c55d013e3..81cd51dcd7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action.cpp @@ -43,20 +43,20 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include #include -#include +#include -#include -#include -#include +#include +#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action_preemption.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action_preemption.cpp index a160138445..6f8a481512 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action_preemption.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_action_preemption.cpp @@ -43,21 +43,21 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include #include #include -#include +#include -#include -#include -#include +#include +#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp index dd1d50a630..78fb4bf41e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/src/integrationtest_sequence_service_capability.cpp @@ -39,22 +39,22 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include #include #include -#include -#include +#include +#include #include #include -#include +#include // Parameters from the node const std::string TEST_DATA_FILE_NAME("testdata_file_name"); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 5dcfad0cde..24acb2b0a2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -33,13 +33,13 @@ *********************************************************************/ #include -#include -#include +#include +#include #include #include #include -#include "test_utils.h" +#include "test_utils.hpp" // Logger static const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_industrial_motion_planner.test_utils"); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp similarity index 98% rename from moveit_planners/pilz_industrial_motion_planner/test/test_utils.h rename to moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp index 758c5ecada..87c2d319ab 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp @@ -43,15 +43,15 @@ #endif #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp index 110f67b418..8973e71962 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp @@ -40,9 +40,9 @@ #include #include -#include +#include -#include +#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp index 6c2bb24522..3992ea7fb3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limit.cpp @@ -36,8 +36,8 @@ #include -#include -#include +#include +#include using namespace pilz_industrial_motion_planner; using namespace pilz_industrial_motion_planner::joint_limits_interface; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp index cdf7002625..eab1e99ae3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_aggregator.cpp @@ -34,12 +34,12 @@ #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp index 0a4ce0d7ec..4977106928 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_container.cpp @@ -34,8 +34,8 @@ #include -#include -#include +#include +#include using namespace pilz_industrial_motion_planner; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp index 8ff46d3dcf..e421e52437 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_joint_limits_validator.cpp @@ -34,9 +34,9 @@ #include -#include +#include -#include +#include using namespace pilz_industrial_motion_planner; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index 24493d2cdc..33f9e2dc31 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -36,11 +36,11 @@ #include -#include -#include +#include +#include -#include -#include "test_utils.h" +#include +#include "test_utils.hpp" #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp index cb22d36d50..db67c3c6b9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp @@ -36,9 +36,9 @@ #include -#include -#include -#include +#include +#include +#include using namespace pilz_industrial_motion_planner; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index a0540e5360..48dfe23752 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -35,19 +35,19 @@ #include #include -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include "test_utils.h" +#include "test_utils.hpp" // parameters from parameter server const std::string PARAM_PLANNING_GROUP_NAME("planning_group"); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp index 0f937f99ca..57d75e9241 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context_loaders.cpp @@ -36,12 +36,12 @@ #include -#include -#include +#include +#include -#include +#include -#include "test_utils.h" +#include "test_utils.hpp" #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 3a109cddcb..63f65f9903 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -36,23 +36,23 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include -#include "test_utils.h" +#include +#include +#include +#include +#include +#include "test_utils.hpp" #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 66cee20108..63f2783aeb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -46,18 +46,18 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include "test_utils.h" +#include +#include +#include +#include +#include "test_utils.hpp" #define _USE_MATH_DEFINES diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp index dafd6fa535..3a495456f0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator.cpp @@ -36,7 +36,7 @@ #include -#include +#include using namespace pilz_industrial_motion_planner; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index 7e462e287a..33fdb0af0e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -36,18 +36,18 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include "test_utils.h" +#include +#include +#include +#include +#include "test_utils.hpp" #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index 9db58f5ede..fbc31afa11 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -35,21 +35,21 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "test_utils.h" +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "test_utils.hpp" #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index cde5c06618..6f049ed9a4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -36,17 +36,17 @@ #include -#include -#include -#include -#include -#include "test_utils.h" - -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include "test_utils.hpp" + +#include +#include +#include +#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp index 6a423b6f45..3fcb254aac 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_ptp.cpp @@ -36,13 +36,13 @@ #include -#include -#include -#include "test_utils.h" +#include +#include +#include "test_utils.hpp" -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_velocity_profile_atrap.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_velocity_profile_atrap.cpp index 0fa4317ae7..8140c25112 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_velocity_profile_atrap.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_velocity_profile_atrap.cpp @@ -34,7 +34,7 @@ #include -#include +#include // Modultest Level1 of Class VelocityProfileATrap #define EPSILON 1.0e-10 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h index 5da4b067a6..bb84bbcac6 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /* * Copyright (c) 2018 Pilz GmbH & Co. KG * @@ -15,160 +27,5 @@ */ #pragma once - -#include -#include -#include -#include -#include - -#include - -#include - -namespace testing -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.async_test"); -/** - * @brief Test class that allows the handling of asynchronous test objects - * - * The class provides the two basic functions AsyncTest::barricade and AsyncTest::triggerClearEvent. - * During the test setup gates between the steps with one or more clear events. Allow passing on by calling - * triggerClearEvent after a test. - * - * \e Usage:
- * Suppose you want to test a function that calls another function asynchronously, like the following example: - * - * \code - * void asyncCall(std::function fun) - * { - * std::thread t(fun); - * t.detach(); - * } - * \endcode - * - * You expect that fun gets called, so your test thread has to wait for the completion, else it would fail. This can be - * achieved via: - * - * \code - * class MyTest : public testing::Test, public testing::AsyncTest - * { - * public: - * MOCK_METHOD0(myMethod, void()); - * }; - * - * TEST_F(MyTest, testCase) - * { - * EXPECT_CALL(*this, myMethod()).Times(1).WillOnce(ACTION_OPEN_BARRIER_VOID("myMethod")); - * const int timeout_ms {100}; - * asyncCall([this]{ myMethod(); }); - * BARRIER("myMethod", timeout_ms) << "Timed-out waiting for myMethod call."; - * } - * \endcode - */ -class AsyncTest -{ -public: - /** - * @brief Triggers a clear event. If a call to barricade is currently pending it will unblock as soon as all clear - * events are triggered. Else the event is put on the waitlist. This waitlist is emptied upon a call to barricade. - * - * @param event The event that is triggered - */ - void triggerClearEvent(const std::string& event); - - /** - * @brief Will block until the event given by clear_event is triggered or a timeout is reached. - * Unblocks immediately, if the event was on the waitlist. - * - * @param clear_event Event that allows the test to pass on - * @param timeout_ms Timeout [ms] (optional). - * @return True if the event was triggered, false otherwise. - */ - bool barricade(const std::string& clear_event, const int timeout_ms = -1); - - /** - * @brief Will block until all events given by clear_events are triggered or a timeout is reached. - * Events on the waitlist are taken into account, too. - * - * @param clear_events List of events that allow the test to pass on - * @param timeout_ms Timeout [ms] (optional). - * @return True if all events were triggered, false otherwise. - */ - bool barricade(std::initializer_list clear_events, const int timeout_ms = -1); - -protected: - std::mutex m_; - std::condition_variable cv_; - std::set clear_events_{}; - std::set waitlist_{}; -}; - -// for better readability in tests -#define BARRIER(...) EXPECT_TRUE(barricade(__VA_ARGS__)) -#define BARRIER_FATAL(...) ASSERT_TRUE(barricade(__VA_ARGS__)) - -#define ACTION_OPEN_BARRIER(str) \ - ::testing::InvokeWithoutArgs([this](void) { \ - this->triggerClearEvent(str); \ - return true; \ - }) -#define ACTION_OPEN_BARRIER_VOID(str) ::testing::InvokeWithoutArgs([this]() { this->triggerClearEvent(str); }) - -inline void AsyncTest::triggerClearEvent(const std::string& event) -{ - std::lock_guard lk(m_); - - if (clear_events_.empty()) - { - RCLCPP_DEBUG(LOGGER, "Clearing Barricade[%s]", event.c_str()); - waitlist_.insert(event); - } - else if (clear_events_.erase(event) < 1) - { - RCLCPP_WARN(LOGGER, "Triggered event " << event << " despite not waiting for it."); - } - cv_.notify_one(); -} - -inline bool AsyncTest::barricade(const std::string& clear_event, const int timeout_ms) -{ - return barricade({ clear_event }, timeout_ms); -} - -inline bool AsyncTest::barricade(std::initializer_list clear_events, const int timeout_ms) -{ - std::unique_lock lk(m_); - - std::stringstream events_stringstream; - for (const auto& event : clear_events) - { - events_stringstream << event << ", "; - } - RCLCPP_DEBUG(LOGGER, "Adding Barricade[%s]", events_stringstream.str().c_str()); - - std::copy_if(clear_events.begin(), clear_events.end(), std::inserter(clear_events_, clear_events_.end()), - [this](const std::string& event) { return this->waitlist_.count(event) == 0; }); - waitlist_.clear(); - - auto end_time_point = std::chrono::system_clock::now() + std::chrono::milliseconds(timeout_ms); - - while (!clear_events_.empty()) - { - if (timeout_ms < 0) - { - cv_.wait(lk); - } - else - { - std::cv_status status = cv_.wait_for(lk, end_time_point - std::chrono::system_clock::now()); - if (status == std::cv_status::timeout) - { - return clear_events_.empty(); - } - } - } - return true; -} - -} // namespace testing +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.hpp new file mode 100644 index 0000000000..5da4b067a6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/async_test.hpp @@ -0,0 +1,174 @@ +/* + * Copyright (c) 2018 Pilz GmbH & Co. KG + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include + +namespace testing +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.async_test"); +/** + * @brief Test class that allows the handling of asynchronous test objects + * + * The class provides the two basic functions AsyncTest::barricade and AsyncTest::triggerClearEvent. + * During the test setup gates between the steps with one or more clear events. Allow passing on by calling + * triggerClearEvent after a test. + * + * \e Usage:
+ * Suppose you want to test a function that calls another function asynchronously, like the following example: + * + * \code + * void asyncCall(std::function fun) + * { + * std::thread t(fun); + * t.detach(); + * } + * \endcode + * + * You expect that fun gets called, so your test thread has to wait for the completion, else it would fail. This can be + * achieved via: + * + * \code + * class MyTest : public testing::Test, public testing::AsyncTest + * { + * public: + * MOCK_METHOD0(myMethod, void()); + * }; + * + * TEST_F(MyTest, testCase) + * { + * EXPECT_CALL(*this, myMethod()).Times(1).WillOnce(ACTION_OPEN_BARRIER_VOID("myMethod")); + * const int timeout_ms {100}; + * asyncCall([this]{ myMethod(); }); + * BARRIER("myMethod", timeout_ms) << "Timed-out waiting for myMethod call."; + * } + * \endcode + */ +class AsyncTest +{ +public: + /** + * @brief Triggers a clear event. If a call to barricade is currently pending it will unblock as soon as all clear + * events are triggered. Else the event is put on the waitlist. This waitlist is emptied upon a call to barricade. + * + * @param event The event that is triggered + */ + void triggerClearEvent(const std::string& event); + + /** + * @brief Will block until the event given by clear_event is triggered or a timeout is reached. + * Unblocks immediately, if the event was on the waitlist. + * + * @param clear_event Event that allows the test to pass on + * @param timeout_ms Timeout [ms] (optional). + * @return True if the event was triggered, false otherwise. + */ + bool barricade(const std::string& clear_event, const int timeout_ms = -1); + + /** + * @brief Will block until all events given by clear_events are triggered or a timeout is reached. + * Events on the waitlist are taken into account, too. + * + * @param clear_events List of events that allow the test to pass on + * @param timeout_ms Timeout [ms] (optional). + * @return True if all events were triggered, false otherwise. + */ + bool barricade(std::initializer_list clear_events, const int timeout_ms = -1); + +protected: + std::mutex m_; + std::condition_variable cv_; + std::set clear_events_{}; + std::set waitlist_{}; +}; + +// for better readability in tests +#define BARRIER(...) EXPECT_TRUE(barricade(__VA_ARGS__)) +#define BARRIER_FATAL(...) ASSERT_TRUE(barricade(__VA_ARGS__)) + +#define ACTION_OPEN_BARRIER(str) \ + ::testing::InvokeWithoutArgs([this](void) { \ + this->triggerClearEvent(str); \ + return true; \ + }) +#define ACTION_OPEN_BARRIER_VOID(str) ::testing::InvokeWithoutArgs([this]() { this->triggerClearEvent(str); }) + +inline void AsyncTest::triggerClearEvent(const std::string& event) +{ + std::lock_guard lk(m_); + + if (clear_events_.empty()) + { + RCLCPP_DEBUG(LOGGER, "Clearing Barricade[%s]", event.c_str()); + waitlist_.insert(event); + } + else if (clear_events_.erase(event) < 1) + { + RCLCPP_WARN(LOGGER, "Triggered event " << event << " despite not waiting for it."); + } + cv_.notify_one(); +} + +inline bool AsyncTest::barricade(const std::string& clear_event, const int timeout_ms) +{ + return barricade({ clear_event }, timeout_ms); +} + +inline bool AsyncTest::barricade(std::initializer_list clear_events, const int timeout_ms) +{ + std::unique_lock lk(m_); + + std::stringstream events_stringstream; + for (const auto& event : clear_events) + { + events_stringstream << event << ", "; + } + RCLCPP_DEBUG(LOGGER, "Adding Barricade[%s]", events_stringstream.str().c_str()); + + std::copy_if(clear_events.begin(), clear_events.end(), std::inserter(clear_events_, clear_events_.end()), + [this](const std::string& event) { return this->waitlist_.count(event) == 0; }); + waitlist_.clear(); + + auto end_time_point = std::chrono::system_clock::now() + std::chrono::milliseconds(timeout_ms); + + while (!clear_events_.empty()) + { + if (timeout_ms < 0) + { + cv_.wait(lk); + } + else + { + std::cv_status status = cv_.wait_for(lk, end_time_point - std::chrono::system_clock::now()); + if (status == std::cv_status::timeout) + { + return clear_events_.empty(); + } + } + } + return true; +} + +} // namespace testing diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h index 4c1cabef2d..702a79adc6 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,94 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include "motioncmd.h" - -namespace pilz_industrial_motion_planner_testutils -{ -template -class BaseCmd : public MotionCmd -{ -public: - BaseCmd() = default; - BaseCmd(const BaseCmd&) = default; - BaseCmd(BaseCmd&&) noexcept = default; - BaseCmd& operator=(const BaseCmd&) = default; - BaseCmd& operator=(BaseCmd&&) noexcept = default; - virtual ~BaseCmd() = default; - -public: - planning_interface::MotionPlanRequest toRequest() const override; - - void setStartConfiguration(StartType start); - void setGoalConfiguration(GoalType goal); - - StartType& getStartConfiguration(); - const StartType& getStartConfiguration() const; - - GoalType& getGoalConfiguration(); - const GoalType& getGoalConfiguration() const; - -private: - virtual std::string getPlannerId() const = 0; - -protected: - GoalType goal_; - StartType start_; -}; - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -template -inline void BaseCmd::setStartConfiguration(StartType start) -{ - start_ = start; -} - -template -inline void BaseCmd::setGoalConfiguration(GoalType goal) -{ - goal_ = goal; -} - -template -inline StartType& BaseCmd::getStartConfiguration() -{ - return start_; -} - -template -inline const StartType& BaseCmd::getStartConfiguration() const -{ - return start_; -} - -template -inline GoalType& BaseCmd::getGoalConfiguration() -{ - return goal_; -} - -template -inline const GoalType& BaseCmd::getGoalConfiguration() const -{ - return goal_; -} - -template -planning_interface::MotionPlanRequest BaseCmd::toRequest() const -{ - planning_interface::MotionPlanRequest req; - req.planner_id = getPlannerId(); - req.group_name = planning_group_; - - req.max_velocity_scaling_factor = vel_scale_; - req.max_acceleration_scaling_factor = acc_scale_; - - req.start_state = start_.toMoveitMsgsRobotState(); - req.goal_constraints.push_back(goal_.toGoalConstraints()); - - return req; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.hpp new file mode 100644 index 0000000000..19649f14fe --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/basecmd.hpp @@ -0,0 +1,126 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include "motioncmd.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +template +class BaseCmd : public MotionCmd +{ +public: + BaseCmd() = default; + BaseCmd(const BaseCmd&) = default; + BaseCmd(BaseCmd&&) noexcept = default; + BaseCmd& operator=(const BaseCmd&) = default; + BaseCmd& operator=(BaseCmd&&) noexcept = default; + virtual ~BaseCmd() = default; + +public: + planning_interface::MotionPlanRequest toRequest() const override; + + void setStartConfiguration(StartType start); + void setGoalConfiguration(GoalType goal); + + StartType& getStartConfiguration(); + const StartType& getStartConfiguration() const; + + GoalType& getGoalConfiguration(); + const GoalType& getGoalConfiguration() const; + +private: + virtual std::string getPlannerId() const = 0; + +protected: + GoalType goal_; + StartType start_; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline void BaseCmd::setStartConfiguration(StartType start) +{ + start_ = start; +} + +template +inline void BaseCmd::setGoalConfiguration(GoalType goal) +{ + goal_ = goal; +} + +template +inline StartType& BaseCmd::getStartConfiguration() +{ + return start_; +} + +template +inline const StartType& BaseCmd::getStartConfiguration() const +{ + return start_; +} + +template +inline GoalType& BaseCmd::getGoalConfiguration() +{ + return goal_; +} + +template +inline const GoalType& BaseCmd::getGoalConfiguration() const +{ + return goal_; +} + +template +planning_interface::MotionPlanRequest BaseCmd::toRequest() const +{ + planning_interface::MotionPlanRequest req; + req.planner_id = getPlannerId(); + req.group_name = planning_group_; + + req.max_velocity_scaling_factor = vel_scale_; + req.max_acceleration_scaling_factor = acc_scale_; + + req.start_state = start_.toMoveitMsgsRobotState(); + req.goal_constraints.push_back(goal_.toGoalConstraints()); + + return req; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h index b6057abdb3..350c54f5b0 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,151 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "robotconfiguration.h" -#include "jointconfiguration.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Class to define a robot configuration in space - * with the help of cartesian coordinates. - */ -class CartesianConfiguration : public RobotConfiguration -{ -public: - CartesianConfiguration(); - - CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config); - - CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config, - const moveit::core::RobotModelConstPtr& robot_model); - -public: - moveit_msgs::msg::Constraints toGoalConstraints() const override; - moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override; - - void setLinkName(const std::string& link_name); - const std::string& getLinkName() const; - - void setPose(const geometry_msgs::msg::Pose& pose); - const geometry_msgs::msg::Pose& getPose() const; - geometry_msgs::msg::Pose& getPose(); - - void setSeed(const JointConfiguration& config); - const JointConfiguration& getSeed() const; - //! @brief States if a seed for the cartesian configuration is set. - bool hasSeed() const; - - void setPoseTolerance(const double tol); - const std::optional getPoseTolerance() const; - - void setAngleTolerance(const double tol); - const std::optional getAngleTolerance() const; - -private: - static geometry_msgs::msg::Pose toPose(const std::vector& pose); - static geometry_msgs::msg::PoseStamped toStampedPose(const geometry_msgs::msg::Pose& pose); - -private: - std::string link_name_; - geometry_msgs::msg::Pose pose_; - - //! @brief The dimensions of the sphere associated with the target region - //! of the position constraint. - std::optional tolerance_pose_; - - //! @brief The value to assign to the absolute tolerances of the - //! orientation constraint. - std::optional tolerance_angle_; - - //! @brief The seed for computing the IK solution of the cartesian configuration. - std::optional seed_; -}; - -std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/); - -inline void CartesianConfiguration::setLinkName(const std::string& link_name) -{ - link_name_ = link_name; -} - -inline const std::string& CartesianConfiguration::getLinkName() const -{ - return link_name_; -} - -inline void CartesianConfiguration::setPose(const geometry_msgs::msg::Pose& pose) -{ - pose_ = pose; -} - -inline const geometry_msgs::msg::Pose& CartesianConfiguration::getPose() const -{ - return pose_; -} - -inline geometry_msgs::msg::Pose& CartesianConfiguration::getPose() -{ - return pose_; -} - -inline moveit_msgs::msg::Constraints CartesianConfiguration::toGoalConstraints() const -{ - if (!tolerance_pose_ || !tolerance_angle_) - { - return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_)); - } - else - { - return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_), tolerance_pose_.value(), - tolerance_angle_.value()); - } -} - -inline void CartesianConfiguration::setSeed(const JointConfiguration& config) -{ - seed_ = config; -} - -inline const JointConfiguration& CartesianConfiguration::getSeed() const -{ - return seed_.value(); -} - -inline bool CartesianConfiguration::hasSeed() const -{ - return seed_.has_value(); -} - -inline void CartesianConfiguration::setPoseTolerance(const double tol) -{ - tolerance_pose_ = tol; -} - -inline const std::optional CartesianConfiguration::getPoseTolerance() const -{ - return tolerance_pose_; -} - -inline void CartesianConfiguration::setAngleTolerance(const double tol) -{ - tolerance_angle_ = tol; -} - -inline const std::optional CartesianConfiguration::getAngleTolerance() const -{ - return tolerance_angle_; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.hpp new file mode 100644 index 0000000000..e9ff450c64 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.hpp @@ -0,0 +1,183 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "robotconfiguration.hpp" +#include "jointconfiguration.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define a robot configuration in space + * with the help of cartesian coordinates. + */ +class CartesianConfiguration : public RobotConfiguration +{ +public: + CartesianConfiguration(); + + CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config); + + CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model); + +public: + moveit_msgs::msg::Constraints toGoalConstraints() const override; + moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override; + + void setLinkName(const std::string& link_name); + const std::string& getLinkName() const; + + void setPose(const geometry_msgs::msg::Pose& pose); + const geometry_msgs::msg::Pose& getPose() const; + geometry_msgs::msg::Pose& getPose(); + + void setSeed(const JointConfiguration& config); + const JointConfiguration& getSeed() const; + //! @brief States if a seed for the cartesian configuration is set. + bool hasSeed() const; + + void setPoseTolerance(const double tol); + const std::optional getPoseTolerance() const; + + void setAngleTolerance(const double tol); + const std::optional getAngleTolerance() const; + +private: + static geometry_msgs::msg::Pose toPose(const std::vector& pose); + static geometry_msgs::msg::PoseStamped toStampedPose(const geometry_msgs::msg::Pose& pose); + +private: + std::string link_name_; + geometry_msgs::msg::Pose pose_; + + //! @brief The dimensions of the sphere associated with the target region + //! of the position constraint. + std::optional tolerance_pose_; + + //! @brief The value to assign to the absolute tolerances of the + //! orientation constraint. + std::optional tolerance_angle_; + + //! @brief The seed for computing the IK solution of the cartesian configuration. + std::optional seed_; +}; + +std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/); + +inline void CartesianConfiguration::setLinkName(const std::string& link_name) +{ + link_name_ = link_name; +} + +inline const std::string& CartesianConfiguration::getLinkName() const +{ + return link_name_; +} + +inline void CartesianConfiguration::setPose(const geometry_msgs::msg::Pose& pose) +{ + pose_ = pose; +} + +inline const geometry_msgs::msg::Pose& CartesianConfiguration::getPose() const +{ + return pose_; +} + +inline geometry_msgs::msg::Pose& CartesianConfiguration::getPose() +{ + return pose_; +} + +inline moveit_msgs::msg::Constraints CartesianConfiguration::toGoalConstraints() const +{ + if (!tolerance_pose_ || !tolerance_angle_) + { + return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_)); + } + else + { + return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_), tolerance_pose_.value(), + tolerance_angle_.value()); + } +} + +inline void CartesianConfiguration::setSeed(const JointConfiguration& config) +{ + seed_ = config; +} + +inline const JointConfiguration& CartesianConfiguration::getSeed() const +{ + return seed_.value(); +} + +inline bool CartesianConfiguration::hasSeed() const +{ + return seed_.has_value(); +} + +inline void CartesianConfiguration::setPoseTolerance(const double tol) +{ + tolerance_pose_ = tol; +} + +inline const std::optional CartesianConfiguration::getPoseTolerance() const +{ + return tolerance_pose_; +} + +inline void CartesianConfiguration::setAngleTolerance(const double tol) +{ + tolerance_angle_ = tol; +} + +inline const std::optional CartesianConfiguration::getAngleTolerance() const +{ + return tolerance_angle_; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h index ca05ab7a06..72f8210465 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,55 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -#include "cartesianconfiguration.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Helper class to build moveit_msgs::msg::Constraints from a - * given configuration. - */ -class CartesianPathConstraintsBuilder -{ -public: - CartesianPathConstraintsBuilder& setConstraintName(const std::string& constraint_name); - CartesianPathConstraintsBuilder& setConfiguration(const CartesianConfiguration& configuration); - - moveit_msgs::msg::Constraints toPathConstraints() const; - -private: - std::string constraint_name_; - CartesianConfiguration configuration_; -}; - -inline CartesianPathConstraintsBuilder& -CartesianPathConstraintsBuilder::setConstraintName(const std::string& constraint_name) -{ - constraint_name_ = constraint_name; - return *this; -} - -inline CartesianPathConstraintsBuilder& -CartesianPathConstraintsBuilder::setConfiguration(const CartesianConfiguration& configuration) -{ - configuration_ = configuration; - return *this; -} - -inline moveit_msgs::msg::Constraints CartesianPathConstraintsBuilder::toPathConstraints() const -{ - moveit_msgs::msg::PositionConstraint pos_constraint; - pos_constraint.link_name = configuration_.getLinkName(); - pos_constraint.constraint_region.primitive_poses.push_back(configuration_.getPose()); - - moveit_msgs::msg::Constraints path_constraints; - path_constraints.name = constraint_name_; - path_constraints.position_constraints.push_back(pos_constraint); - return path_constraints; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.hpp new file mode 100644 index 0000000000..d684738597 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianpathconstraintsbuilder.hpp @@ -0,0 +1,87 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include "cartesianconfiguration.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Helper class to build moveit_msgs::msg::Constraints from a + * given configuration. + */ +class CartesianPathConstraintsBuilder +{ +public: + CartesianPathConstraintsBuilder& setConstraintName(const std::string& constraint_name); + CartesianPathConstraintsBuilder& setConfiguration(const CartesianConfiguration& configuration); + + moveit_msgs::msg::Constraints toPathConstraints() const; + +private: + std::string constraint_name_; + CartesianConfiguration configuration_; +}; + +inline CartesianPathConstraintsBuilder& +CartesianPathConstraintsBuilder::setConstraintName(const std::string& constraint_name) +{ + constraint_name_ = constraint_name; + return *this; +} + +inline CartesianPathConstraintsBuilder& +CartesianPathConstraintsBuilder::setConfiguration(const CartesianConfiguration& configuration) +{ + configuration_ = configuration; + return *this; +} + +inline moveit_msgs::msg::Constraints CartesianPathConstraintsBuilder::toPathConstraints() const +{ + moveit_msgs::msg::PositionConstraint pos_constraint; + pos_constraint.link_name = configuration_.getLinkName(); + pos_constraint.constraint_region.primitive_poses.push_back(configuration_.getPose()); + + moveit_msgs::msg::Constraints path_constraints; + path_constraints.name = constraint_name_; + path_constraints.position_constraints.push_back(pos_constraint); + return path_constraints; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h index 39acff221d..de30af49eb 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,25 +45,5 @@ *********************************************************************/ #pragma once - -#include "circauxiliary.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Class to define the center point of the circle - * on which the robot is supposed to move via circ command. - */ -template -class Center : public CircAuxiliary -{ -private: - std::string getConstraintName() const override; -}; - -template -std::string Center::getConstraintName() const -{ - return "center"; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.hpp new file mode 100644 index 0000000000..161417ac33 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/center.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include "circauxiliary.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define the center point of the circle + * on which the robot is supposed to move via circ command. + */ +template +class Center : public CircAuxiliary +{ +private: + std::string getConstraintName() const override; +}; + +template +std::string Center::getConstraintName() const +{ + return "center"; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h index 128564b304..07c15ca8a8 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,27 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace pilz_industrial_motion_planner_testutils -{ -::testing::AssertionResult isAtExpectedPosition(const moveit::core::RobotState& expected, - const moveit::core::RobotState& actual, const double epsilon, - const std::string& group_name = "") -{ - if (group_name.empty() && expected.distance(actual) <= epsilon) - { - return ::testing::AssertionSuccess(); - } - else if (!group_name.empty() && expected.distance(actual, actual.getJointModelGroup(group_name)) <= epsilon) - { - return ::testing::AssertionSuccess(); - } - std::stringstream msg; - msg << " expected: " << expected << " actual: " << actual; - - return ::testing::AssertionFailure() << msg.str(); -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.hpp new file mode 100644 index 0000000000..2a9ec854bd --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.hpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +::testing::AssertionResult isAtExpectedPosition(const moveit::core::RobotState& expected, + const moveit::core::RobotState& actual, const double epsilon, + const std::string& group_name = "") +{ + if (group_name.empty() && expected.distance(actual) <= epsilon) + { + return ::testing::AssertionSuccess(); + } + else if (!group_name.empty() && expected.distance(actual, actual.getJointModelGroup(group_name)) <= epsilon) + { + return ::testing::AssertionSuccess(); + } + std::stringstream msg; + msg << " expected: " << expected << " actual: " << actual; + + return ::testing::AssertionFailure() << msg.str(); +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h index a6560814e1..2de70b4407 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,71 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include "basecmd.h" -#include "circauxiliary.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Data class storing all information regarding a Circ command. - */ -template -class Circ : public BaseCmd -{ -public: - Circ() : BaseCmd() - { - } - -public: - void setAuxiliaryConfiguration(AuxiliaryType auxiliary); - AuxiliaryType& getAuxiliaryConfiguration(); - const AuxiliaryType& getAuxiliaryConfiguration() const; - -public: - planning_interface::MotionPlanRequest toRequest() const override; - -private: - std::string getPlannerId() const override; - -private: - AuxiliaryType auxiliary_; -}; - -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -template -inline void Circ::setAuxiliaryConfiguration(AuxiliaryType auxiliary) -{ - auxiliary_ = auxiliary; -} - -template -inline std::string Circ::getPlannerId() const -{ - return "CIRC"; -} - -template -inline planning_interface::MotionPlanRequest Circ::toRequest() const -{ - planning_interface::MotionPlanRequest req{ BaseCmd::toRequest() }; - req.path_constraints = auxiliary_.toPathConstraints(); - - return req; -} - -template -inline AuxiliaryType& Circ::getAuxiliaryConfiguration() -{ - return auxiliary_; -} - -template -inline const AuxiliaryType& Circ::getAuxiliaryConfiguration() const -{ - return auxiliary_; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.hpp new file mode 100644 index 0000000000..cc48d853e0 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ.hpp @@ -0,0 +1,103 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include "basecmd.hpp" +#include "circauxiliary.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Circ command. + */ +template +class Circ : public BaseCmd +{ +public: + Circ() : BaseCmd() + { + } + +public: + void setAuxiliaryConfiguration(AuxiliaryType auxiliary); + AuxiliaryType& getAuxiliaryConfiguration(); + const AuxiliaryType& getAuxiliaryConfiguration() const; + +public: + planning_interface::MotionPlanRequest toRequest() const override; + +private: + std::string getPlannerId() const override; + +private: + AuxiliaryType auxiliary_; +}; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline void Circ::setAuxiliaryConfiguration(AuxiliaryType auxiliary) +{ + auxiliary_ = auxiliary; +} + +template +inline std::string Circ::getPlannerId() const +{ + return "CIRC"; +} + +template +inline planning_interface::MotionPlanRequest Circ::toRequest() const +{ + planning_interface::MotionPlanRequest req{ BaseCmd::toRequest() }; + req.path_constraints = auxiliary_.toPathConstraints(); + + return req; +} + +template +inline AuxiliaryType& Circ::getAuxiliaryConfiguration() +{ + return auxiliary_; +} + +template +inline const AuxiliaryType& Circ::getAuxiliaryConfiguration() const +{ + return auxiliary_; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h index 0109def4f7..9420daeb3a 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,14 +45,5 @@ *********************************************************************/ #pragma once - -#include "center.h" -#include "interim.h" -#include "cartesianconfiguration.h" -#include "cartesianpathconstraintsbuilder.h" - -namespace pilz_industrial_motion_planner_testutils -{ -using CartesianCenter = Center; -using CartesianInterim = Interim; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.hpp new file mode 100644 index 0000000000..dac95fc0ca --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circ_auxiliary_types.hpp @@ -0,0 +1,46 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include "center.hpp" +#include "interim.hpp" +#include "cartesianconfiguration.hpp" +#include "cartesianpathconstraintsbuilder.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +using CartesianCenter = Center; +using CartesianInterim = Interim; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h index e3553c1446..07af3992a7 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,56 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Base class to define an auxiliary point needed to specify - * circ commands. - */ -template -class CircAuxiliary -{ -public: - void setConfiguration(const ConfigType& auxiliary_config); - ConfigType& getConfiguration(); - const ConfigType& getConfiguration() const; - -public: - moveit_msgs::msg::Constraints toPathConstraints() const; - -private: - virtual std::string getConstraintName() const = 0; - -protected: - ConfigType auxiliary_config_; -}; - -template -void CircAuxiliary::setConfiguration(const ConfigType& auxiliary_config) -{ - auxiliary_config_ = auxiliary_config; -} - -template -inline ConfigType& CircAuxiliary::getConfiguration() -{ - return auxiliary_config_; -} - -template -inline const ConfigType& CircAuxiliary::getConfiguration() const -{ - return auxiliary_config_; -} - -template -inline moveit_msgs::msg::Constraints CircAuxiliary::toPathConstraints() const -{ - return BuilderType().setConstraintName(getConstraintName()).setConfiguration(getConfiguration()).toPathConstraints(); -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.hpp new file mode 100644 index 0000000000..e3553c1446 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/circauxiliary.hpp @@ -0,0 +1,88 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Base class to define an auxiliary point needed to specify + * circ commands. + */ +template +class CircAuxiliary +{ +public: + void setConfiguration(const ConfigType& auxiliary_config); + ConfigType& getConfiguration(); + const ConfigType& getConfiguration() const; + +public: + moveit_msgs::msg::Constraints toPathConstraints() const; + +private: + virtual std::string getConstraintName() const = 0; + +protected: + ConfigType auxiliary_config_; +}; + +template +void CircAuxiliary::setConfiguration(const ConfigType& auxiliary_config) +{ + auxiliary_config_ = auxiliary_config; +} + +template +inline ConfigType& CircAuxiliary::getConfiguration() +{ + return auxiliary_config_; +} + +template +inline const ConfigType& CircAuxiliary::getConfiguration() const +{ + return auxiliary_config_; +} + +template +inline moveit_msgs::msg::Constraints CircAuxiliary::toPathConstraints() const +{ + return BuilderType().setConstraintName(getConstraintName()).setConfiguration(getConfiguration()).toPathConstraints(); +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h index 697cc5781f..be492d4406 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,34 +45,5 @@ *********************************************************************/ #pragma once - -#include "ptp.h" -#include "lin.h" -#include "circ.h" -#include "gripper.h" -#include "jointconfiguration.h" -#include "cartesianconfiguration.h" -#include "circ_auxiliary_types.h" - -#include - -namespace pilz_industrial_motion_planner_testutils -{ -typedef Ptp PtpJoint; -typedef Ptp PtpJointCart; -typedef Ptp PtpCart; - -typedef Lin LinJoint; -typedef Lin LinJointCart; -typedef Lin LinCart; - -typedef Circ CircCenterCart; -typedef Circ CircInterimCart; - -typedef Circ CircJointCenterCart; -typedef Circ CircJointInterimCart; - -typedef std::variant - CmdVariant; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.hpp new file mode 100644 index 0000000000..a1a37ba6eb --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.hpp @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include "ptp.hpp" +#include "lin.hpp" +#include "circ.hpp" +#include "gripper.hpp" +#include "jointconfiguration.hpp" +#include "cartesianconfiguration.hpp" +#include "circ_auxiliary_types.hpp" + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +typedef Ptp PtpJoint; +typedef Ptp PtpJointCart; +typedef Ptp PtpCart; + +typedef Lin LinJoint; +typedef Lin LinJointCart; +typedef Lin LinCart; + +typedef Circ CircCenterCart; +typedef Circ CircInterimCart; + +typedef Circ CircJointCenterCart; +typedef Circ CircJointInterimCart; + +typedef std::variant + CmdVariant; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h index 5ac22e7a0e..53e2ec5b47 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,16 +45,5 @@ *********************************************************************/ #pragma once - -/* - * @brief This file contains all default values needed for testing. - */ -namespace pilz_industrial_motion_planner_testutils -{ -static constexpr double DEFAULT_VEL{ 0.01 }; -static constexpr double DEFAULT_ACC{ 0.01 }; -static constexpr double DEFAULT_BLEND_RADIUS{ 0.01 }; - -static constexpr double DEFAULT_VEL_GRIPPER{ 0.5 }; -static constexpr double DEFAULT_ACC_GRIPPER{ 0.8 }; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.hpp new file mode 100644 index 0000000000..5ac22e7a0e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/default_values.hpp @@ -0,0 +1,48 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +/* + * @brief This file contains all default values needed for testing. + */ +namespace pilz_industrial_motion_planner_testutils +{ +static constexpr double DEFAULT_VEL{ 0.01 }; +static constexpr double DEFAULT_ACC{ 0.01 }; +static constexpr double DEFAULT_BLEND_RADIUS{ 0.01 }; + +static constexpr double DEFAULT_VEL_GRIPPER{ 0.5 }; +static constexpr double DEFAULT_ACC_GRIPPER{ 0.8 }; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h index fa213e4ab4..cc8e21b40f 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,17 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace pilz_industrial_motion_planner_testutils -{ -class TestDataLoaderReadingException : public std::runtime_error -{ -public: - TestDataLoaderReadingException(const std::string& error_desc) : std::runtime_error(error_desc) - { - } -}; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.hpp new file mode 100644 index 0000000000..fa213e4ab4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/exception_types.hpp @@ -0,0 +1,49 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +class TestDataLoaderReadingException : public std::runtime_error +{ +public: + TestDataLoaderReadingException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h index dd4a03d07d..118128f15c 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,27 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include -#include - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Interface class to express that a derived class can be converted - * into a moveit_msgs::msg::Constraints. - */ -class GoalConstraintMsgConvertible -{ -public: - GoalConstraintMsgConvertible() = default; - GoalConstraintMsgConvertible(const GoalConstraintMsgConvertible&) = default; - GoalConstraintMsgConvertible(GoalConstraintMsgConvertible&&) = default; - GoalConstraintMsgConvertible& operator=(const GoalConstraintMsgConvertible&) = default; - GoalConstraintMsgConvertible& operator=(GoalConstraintMsgConvertible&&) = default; - virtual ~GoalConstraintMsgConvertible() = default; - virtual moveit_msgs::msg::Constraints toGoalConstraints() const = 0; -}; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.hpp new file mode 100644 index 0000000000..a27ca8210f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/goalconstraintsmsgconvertible.hpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a moveit_msgs::msg::Constraints. + */ +class GoalConstraintMsgConvertible +{ +public: + GoalConstraintMsgConvertible() = default; + GoalConstraintMsgConvertible(const GoalConstraintMsgConvertible&) = default; + GoalConstraintMsgConvertible(GoalConstraintMsgConvertible&&) = default; + GoalConstraintMsgConvertible& operator=(const GoalConstraintMsgConvertible&) = default; + GoalConstraintMsgConvertible& operator=(GoalConstraintMsgConvertible&&) = default; + virtual ~GoalConstraintMsgConvertible() = default; + virtual moveit_msgs::msg::Constraints toGoalConstraints() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h index 5b3aca63c1..fd3d01bec1 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,13 +45,5 @@ *********************************************************************/ #pragma once - -#include "ptp.h" -#include "jointconfiguration.h" - -namespace pilz_industrial_motion_planner_testutils -{ -class Gripper : public Ptp -{ -}; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.hpp new file mode 100644 index 0000000000..645fb84304 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/gripper.hpp @@ -0,0 +1,45 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include "ptp.hpp" +#include "jointconfiguration.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +class Gripper : public Ptp +{ +}; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h index 29ca954184..2e2e3df04d 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,25 +45,5 @@ *********************************************************************/ #pragma once - -#include "circauxiliary.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Class to define a point on the circle on which the robot is supposed - * to move via circ command. - */ -template -class Interim : public CircAuxiliary -{ -private: - std::string getConstraintName() const override; -}; - -template -std::string Interim::getConstraintName() const -{ - return "interim"; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.hpp new file mode 100644 index 0000000000..efeff7fb51 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/interim.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include "circauxiliary.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define a point on the circle on which the robot is supposed + * to move via circ command. + */ +template +class Interim : public CircAuxiliary +{ +private: + std::string getConstraintName() const override; +}; + +template +std::string Interim::getConstraintName() const +{ + return "interim"; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h index 2daf37f92e..1d9f70c06b 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,110 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "robotconfiguration.h" - -namespace pilz_industrial_motion_planner_testutils -{ -class JointConfigurationException : public std::runtime_error -{ -public: - JointConfigurationException(const std::string& error_desc) : std::runtime_error(error_desc) - { - } -}; - -using CreateJointNameFunc = std::function; - -/** - * @brief Class to define a robot configuration in space with the help - * of joint values. - */ -class JointConfiguration : public RobotConfiguration -{ -public: - JointConfiguration(); - - JointConfiguration(const std::string& group_name, const std::vector& config, - CreateJointNameFunc&& create_joint_name_func); - - JointConfiguration(const std::string& group_name, const std::vector& config, - const moveit::core::RobotModelConstPtr& robot_model); - -public: - void setJoint(const size_t index, const double value); - double getJoint(const size_t index) const; - const std::vector getJoints() const; - - size_t size() const; - - moveit_msgs::msg::Constraints toGoalConstraints() const override; - moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override; - - sensor_msgs::msg::JointState toSensorMsg() const; - - moveit::core::RobotState toRobotState() const; - - void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func); - -private: - moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithoutModel() const; - moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithModel() const; - - moveit_msgs::msg::Constraints toGoalConstraintsWithoutModel() const; - moveit_msgs::msg::Constraints toGoalConstraintsWithModel() const; - -private: - //! Joint positions - std::vector joints_; - - CreateJointNameFunc create_joint_name_func_; -}; - -std::ostream& operator<<(std::ostream& /*os*/, const JointConfiguration& /*obj*/); - -inline moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraints() const -{ - return robot_model_ ? toGoalConstraintsWithModel() : toGoalConstraintsWithoutModel(); -} - -inline moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotState() const -{ - return robot_model_ ? toMoveitMsgsRobotStateWithModel() : toMoveitMsgsRobotStateWithoutModel(); -} - -inline void JointConfiguration::setJoint(const size_t index, const double value) -{ - joints_.at(index) = value; -} - -inline double JointConfiguration::getJoint(const size_t index) const -{ - return joints_.at(index); -} - -inline const std::vector JointConfiguration::getJoints() const -{ - return joints_; -} - -inline size_t JointConfiguration::size() const -{ - return joints_.size(); -} - -inline void JointConfiguration::setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func) -{ - create_joint_name_func_ = std::move(create_joint_name_func); -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.hpp new file mode 100644 index 0000000000..c016f8ec4f --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/jointconfiguration.hpp @@ -0,0 +1,142 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "robotconfiguration.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +class JointConfigurationException : public std::runtime_error +{ +public: + JointConfigurationException(const std::string& error_desc) : std::runtime_error(error_desc) + { + } +}; + +using CreateJointNameFunc = std::function; + +/** + * @brief Class to define a robot configuration in space with the help + * of joint values. + */ +class JointConfiguration : public RobotConfiguration +{ +public: + JointConfiguration(); + + JointConfiguration(const std::string& group_name, const std::vector& config, + CreateJointNameFunc&& create_joint_name_func); + + JointConfiguration(const std::string& group_name, const std::vector& config, + const moveit::core::RobotModelConstPtr& robot_model); + +public: + void setJoint(const size_t index, const double value); + double getJoint(const size_t index) const; + const std::vector getJoints() const; + + size_t size() const; + + moveit_msgs::msg::Constraints toGoalConstraints() const override; + moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override; + + sensor_msgs::msg::JointState toSensorMsg() const; + + moveit::core::RobotState toRobotState() const; + + void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func); + +private: + moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithoutModel() const; + moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithModel() const; + + moveit_msgs::msg::Constraints toGoalConstraintsWithoutModel() const; + moveit_msgs::msg::Constraints toGoalConstraintsWithModel() const; + +private: + //! Joint positions + std::vector joints_; + + CreateJointNameFunc create_joint_name_func_; +}; + +std::ostream& operator<<(std::ostream& /*os*/, const JointConfiguration& /*obj*/); + +inline moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraints() const +{ + return robot_model_ ? toGoalConstraintsWithModel() : toGoalConstraintsWithoutModel(); +} + +inline moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotState() const +{ + return robot_model_ ? toMoveitMsgsRobotStateWithModel() : toMoveitMsgsRobotStateWithoutModel(); +} + +inline void JointConfiguration::setJoint(const size_t index, const double value) +{ + joints_.at(index) = value; +} + +inline double JointConfiguration::getJoint(const size_t index) const +{ + return joints_.at(index); +} + +inline const std::vector JointConfiguration::getJoints() const +{ + return joints_; +} + +inline size_t JointConfiguration::size() const +{ + return joints_.size(); +} + +inline void JointConfiguration::setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func) +{ + create_joint_name_func_ = std::move(create_joint_name_func); +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h index 9d5f250faa..03faea1f5d 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,32 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include "basecmd.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Data class storing all information regarding a linear command. - */ -template -class Lin : public BaseCmd -{ -public: - Lin() : BaseCmd() - { - } - -private: - std::string getPlannerId() const override; -}; - -// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -template -inline std::string Lin::getPlannerId() const -{ - return "LIN"; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.hpp new file mode 100644 index 0000000000..46fab92ce6 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/lin.hpp @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include "basecmd.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a linear command. + */ +template +class Lin : public BaseCmd +{ +public: + Lin() : BaseCmd() + { + } + +private: + std::string getPlannerId() const override; +}; + +// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline std::string Lin::getPlannerId() const +{ + return "LIN"; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h index 54c6d9456e..ffca77b320 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,58 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include "motionplanrequestconvertible.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Base class for commands storing all general information of a command. - */ -class MotionCmd : public MotionPlanRequestConvertible -{ -public: - MotionCmd() : MotionPlanRequestConvertible() - { - } - -public: - void setPlanningGroup(const std::string& planning_group); - const std::string& getPlanningGroup() const; - - void setVelocityScale(double velocity_scale); - void setAccelerationScale(double acceleration_scale); - -protected: - std::string planning_group_; - //! Link to which all cartesian poses refer to. - std::string target_link_; - double vel_scale_{ 1.0 }; - double acc_scale_{ 1.0 }; -}; - -inline void MotionCmd::setPlanningGroup(const std::string& planning_group) -{ - planning_group_ = planning_group; -} - -inline const std::string& MotionCmd::getPlanningGroup() const -{ - return planning_group_; -} - -inline void MotionCmd::setVelocityScale(double velocity_scale) -{ - vel_scale_ = velocity_scale; -} - -inline void MotionCmd::setAccelerationScale(double acceleration_scale) -{ - acc_scale_ = acceleration_scale; -} - -using MotionCmdUPtr = std::unique_ptr; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.hpp new file mode 100644 index 0000000000..8c1064bb10 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motioncmd.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include "motionplanrequestconvertible.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Base class for commands storing all general information of a command. + */ +class MotionCmd : public MotionPlanRequestConvertible +{ +public: + MotionCmd() : MotionPlanRequestConvertible() + { + } + +public: + void setPlanningGroup(const std::string& planning_group); + const std::string& getPlanningGroup() const; + + void setVelocityScale(double velocity_scale); + void setAccelerationScale(double acceleration_scale); + +protected: + std::string planning_group_; + //! Link to which all cartesian poses refer to. + std::string target_link_; + double vel_scale_{ 1.0 }; + double acc_scale_{ 1.0 }; +}; + +inline void MotionCmd::setPlanningGroup(const std::string& planning_group) +{ + planning_group_ = planning_group; +} + +inline const std::string& MotionCmd::getPlanningGroup() const +{ + return planning_group_; +} + +inline void MotionCmd::setVelocityScale(double velocity_scale) +{ + vel_scale_ = velocity_scale; +} + +inline void MotionCmd::setAccelerationScale(double acceleration_scale) +{ + acc_scale_ = acceleration_scale; +} + +using MotionCmdUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h index 3b3d9046cd..b8dae56d66 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,19 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Interface class to express that a derived class can be converted - * into a planning_interface::MotionPlanRequest. - */ -class MotionPlanRequestConvertible -{ -public: - virtual planning_interface::MotionPlanRequest toRequest() const = 0; -}; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.hpp new file mode 100644 index 0000000000..8503429d0a --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/motionplanrequestconvertible.hpp @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a planning_interface::MotionPlanRequest. + */ +class MotionPlanRequestConvertible +{ +public: + virtual planning_interface::MotionPlanRequest toRequest() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h index 04dab225d9..21429ca89a 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,32 +45,5 @@ *********************************************************************/ #pragma once - -#include - -#include "basecmd.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Data class storing all information regarding a Ptp command. - */ -template -class Ptp : public BaseCmd -{ -public: - Ptp() : BaseCmd() - { - } - -private: - std::string getPlannerId() const override; -}; - -// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -template -inline std::string Ptp::getPlannerId() const -{ - return "PTP"; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.hpp new file mode 100644 index 0000000000..951161d7ff --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/ptp.hpp @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +#include "basecmd.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Ptp command. + */ +template +class Ptp : public BaseCmd +{ +public: + Ptp() : BaseCmd() + { + } + +private: + std::string getPlannerId() const override; +}; + +// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +template +inline std::string Ptp::getPlannerId() const +{ + return "PTP"; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h index 07fd1c545b..c7e6e521e9 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,57 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include - -#include "goalconstraintsmsgconvertible.h" -#include "robotstatemsgconvertible.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Class to define robot configuration in space. - */ -class RobotConfiguration : public RobotStateMsgConvertible, public GoalConstraintMsgConvertible -{ -public: - RobotConfiguration(); - - RobotConfiguration(const std::string& group_name); - - RobotConfiguration(const std::string& group_name, const moveit::core::RobotModelConstPtr& robot_model); - -public: - void setRobotModel(moveit::core::RobotModelConstPtr robot_model); - void setGroupName(const std::string& group_name); - std::string getGroupName() const; - void clearModel(); - -protected: - std::string group_name_; - moveit::core::RobotModelConstPtr robot_model_; -}; - -inline void RobotConfiguration::setRobotModel(moveit::core::RobotModelConstPtr robot_model) -{ - robot_model_ = std::move(robot_model); -} - -inline void RobotConfiguration::setGroupName(const std::string& group_name) -{ - group_name_ = group_name; -} - -inline std::string RobotConfiguration::getGroupName() const -{ - return group_name_; -} - -inline void RobotConfiguration::clearModel() -{ - robot_model_ = nullptr; -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.hpp new file mode 100644 index 0000000000..f96e61cd52 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotconfiguration.hpp @@ -0,0 +1,89 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +#include "goalconstraintsmsgconvertible.hpp" +#include "robotstatemsgconvertible.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Class to define robot configuration in space. + */ +class RobotConfiguration : public RobotStateMsgConvertible, public GoalConstraintMsgConvertible +{ +public: + RobotConfiguration(); + + RobotConfiguration(const std::string& group_name); + + RobotConfiguration(const std::string& group_name, const moveit::core::RobotModelConstPtr& robot_model); + +public: + void setRobotModel(moveit::core::RobotModelConstPtr robot_model); + void setGroupName(const std::string& group_name); + std::string getGroupName() const; + void clearModel(); + +protected: + std::string group_name_; + moveit::core::RobotModelConstPtr robot_model_; +}; + +inline void RobotConfiguration::setRobotModel(moveit::core::RobotModelConstPtr robot_model) +{ + robot_model_ = std::move(robot_model); +} + +inline void RobotConfiguration::setGroupName(const std::string& group_name) +{ + group_name_ = group_name; +} + +inline std::string RobotConfiguration::getGroupName() const +{ + return group_name_; +} + +inline void RobotConfiguration::clearModel() +{ + robot_model_ = nullptr; +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h index 7e9da57924..c5a05d8ab3 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,25 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Interface class to express that a derived class can be converted - * into a moveit_msgs::msg::RobotState. - */ -class RobotStateMsgConvertible -{ -public: - RobotStateMsgConvertible() = default; - RobotStateMsgConvertible(const RobotStateMsgConvertible&) = default; - RobotStateMsgConvertible(RobotStateMsgConvertible&&) = default; - RobotStateMsgConvertible& operator=(const RobotStateMsgConvertible&) = default; - RobotStateMsgConvertible& operator=(RobotStateMsgConvertible&&) = default; - virtual ~RobotStateMsgConvertible() = default; - virtual moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const = 0; -}; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.hpp new file mode 100644 index 0000000000..89d1d3cc66 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/robotstatemsgconvertible.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Interface class to express that a derived class can be converted + * into a moveit_msgs::msg::RobotState. + */ +class RobotStateMsgConvertible +{ +public: + RobotStateMsgConvertible() = default; + RobotStateMsgConvertible(const RobotStateMsgConvertible&) = default; + RobotStateMsgConvertible(RobotStateMsgConvertible&&) = default; + RobotStateMsgConvertible& operator=(const RobotStateMsgConvertible&) = default; + RobotStateMsgConvertible& operator=(RobotStateMsgConvertible&&) = default; + virtual ~RobotStateMsgConvertible() = default; + virtual moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const = 0; +}; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h index e18d517080..69ef4701f5 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,113 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include - -#include - -#include "command_types_typedef.h" -#include "motioncmd.h" -#include - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Data class storing all information regarding a Sequence command. - */ -class Sequence -{ -public: - /** - * @brief Adds a command to the end of the sequence. - * @param cmd The command which has to be added. - */ - void add(const CmdVariant& cmd, const double blend_radius = 0.); - - /** - * @brief Returns the number of commands. - */ - size_t size() const; - - template - T& getCmd(const size_t index_cmd); - - template - const T& getCmd(const size_t index_cmd) const; - - /** - * @return TRUE if the specified command is of the specified type, - * otherwise FALSE. - */ - template - bool cmdIsOfType(const size_t index_cmd) const; - - /** - * @brief Returns the specific command as base class reference. - * This function allows the user to operate on the sequence without - * having knowledge of the underlying specific command type. - */ - MotionCmd& getCmd(const size_t index_cmd); - - void setAllBlendRadiiToZero(); - void setBlendRadius(const size_t index_cmd, const double blend_radius); - double getBlendRadius(const size_t index_cmd) const; - - /** - * @brief Deletes all commands from index 'start' to index 'end'. - */ - void erase(const size_t start, const size_t end); - - moveit_msgs::msg::MotionSequenceRequest toRequest() const; - -private: - using TCmdRadiiPair = std::pair; - std::vector cmds_; -}; - -inline void Sequence::add(const CmdVariant& cmd, const double blend_radius) -{ - cmds_.emplace_back(cmd, blend_radius); -} - -inline size_t Sequence::size() const -{ - return cmds_.size(); -} - -template -inline T& Sequence::getCmd(const size_t index_cmd) -{ - return std::get(cmds_.at(index_cmd).first); -} - -template -inline const T& Sequence::getCmd(const size_t index_cmd) const -{ - return std::get(cmds_.at(index_cmd).first); -} - -inline double Sequence::getBlendRadius(const size_t index_cmd) const -{ - return cmds_.at(index_cmd).second; -} - -inline void Sequence::setBlendRadius(const size_t index_cmd, const double blend_radius) -{ - cmds_.at(index_cmd).second = blend_radius; -} - -inline void Sequence::setAllBlendRadiiToZero() -{ - std::for_each(cmds_.begin(), cmds_.end(), [](TCmdRadiiPair& cmd) { cmd.second = 0.; }); -} - -template -inline bool Sequence::cmdIsOfType(const size_t index_cmd) const -{ - return std::holds_alternative(cmds_.at(index_cmd).first); -} -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.hpp new file mode 100644 index 0000000000..18477b4615 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.hpp @@ -0,0 +1,145 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include + +#include "command_types_typedef.hpp" +#include "motioncmd.hpp" +#include + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Data class storing all information regarding a Sequence command. + */ +class Sequence +{ +public: + /** + * @brief Adds a command to the end of the sequence. + * @param cmd The command which has to be added. + */ + void add(const CmdVariant& cmd, const double blend_radius = 0.); + + /** + * @brief Returns the number of commands. + */ + size_t size() const; + + template + T& getCmd(const size_t index_cmd); + + template + const T& getCmd(const size_t index_cmd) const; + + /** + * @return TRUE if the specified command is of the specified type, + * otherwise FALSE. + */ + template + bool cmdIsOfType(const size_t index_cmd) const; + + /** + * @brief Returns the specific command as base class reference. + * This function allows the user to operate on the sequence without + * having knowledge of the underlying specific command type. + */ + MotionCmd& getCmd(const size_t index_cmd); + + void setAllBlendRadiiToZero(); + void setBlendRadius(const size_t index_cmd, const double blend_radius); + double getBlendRadius(const size_t index_cmd) const; + + /** + * @brief Deletes all commands from index 'start' to index 'end'. + */ + void erase(const size_t start, const size_t end); + + moveit_msgs::msg::MotionSequenceRequest toRequest() const; + +private: + using TCmdRadiiPair = std::pair; + std::vector cmds_; +}; + +inline void Sequence::add(const CmdVariant& cmd, const double blend_radius) +{ + cmds_.emplace_back(cmd, blend_radius); +} + +inline size_t Sequence::size() const +{ + return cmds_.size(); +} + +template +inline T& Sequence::getCmd(const size_t index_cmd) +{ + return std::get(cmds_.at(index_cmd).first); +} + +template +inline const T& Sequence::getCmd(const size_t index_cmd) const +{ + return std::get(cmds_.at(index_cmd).first); +} + +inline double Sequence::getBlendRadius(const size_t index_cmd) const +{ + return cmds_.at(index_cmd).second; +} + +inline void Sequence::setBlendRadius(const size_t index_cmd, const double blend_radius) +{ + cmds_.at(index_cmd).second = blend_radius; +} + +inline void Sequence::setAllBlendRadiiToZero() +{ + std::for_each(cmds_.begin(), cmds_.end(), [](TCmdRadiiPair& cmd) { cmd.second = 0.; }); +} + +template +inline bool Sequence::cmdIsOfType(const size_t index_cmd) const +{ + return std::holds_alternative(cmds_.at(index_cmd).first); +} +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h index 10ea232571..a975a76663 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,86 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -#include - -#include "jointconfiguration.h" -#include "cartesianconfiguration.h" -#include "command_types_typedef.h" -#include "sequence.h" -#include "gripper.h" - -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Abstract base class describing the interface to access - * test data like robot poses and robot commands. - */ -class TestdataLoader -{ -public: - TestdataLoader() = default; - - TestdataLoader(moveit::core::RobotModelConstPtr robot_model) : robot_model_(std::move(robot_model)) - { - } - - TestdataLoader(const TestdataLoader&) = default; - TestdataLoader(TestdataLoader&&) = default; - TestdataLoader& operator=(const TestdataLoader&) = default; - TestdataLoader& operator=(TestdataLoader&&) = default; - virtual ~TestdataLoader() = default; - -public: - void setRobotModel(moveit::core::RobotModelConstPtr robot_model); - - virtual JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const = 0; - - virtual CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const = 0; - - /** - * @brief Returns the command with the specified name from the test data. - */ - virtual PtpJoint getPtpJoint(const std::string& cmd_name) const = 0; - virtual PtpCart getPtpCart(const std::string& cmd_name) const = 0; - virtual PtpJointCart getPtpJointCart(const std::string& cmd_name) const = 0; - - /** - * @brief Returns the command with the specified name from the test data. - */ - virtual LinJoint getLinJoint(const std::string& cmd_name) const = 0; - virtual LinCart getLinCart(const std::string& cmd_name) const = 0; - virtual LinJointCart getLinJointCart(const std::string& cmd_name) const = 0; - - /** - * @brief Returns the command with the specified name from the test data. - */ - virtual CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const = 0; - virtual CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const = 0; - virtual CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const = 0; - virtual CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const = 0; - - /** - * @brief Returns the command with the specified name from the test data. - */ - virtual Sequence getSequence(const std::string& cmd_name) const = 0; - - /** - * @brief Returns the command with the specified name from the test data. - */ - virtual Gripper getGripper(const std::string& cmd_name) const = 0; - -protected: - moveit::core::RobotModelConstPtr robot_model_; -}; - -inline void TestdataLoader::setRobotModel(moveit::core::RobotModelConstPtr robot_model) -{ - robot_model_ = std::move(robot_model); -} - -using TestdataLoaderUPtr = std::unique_ptr; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.hpp new file mode 100644 index 0000000000..ca596e5757 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/testdata_loader.hpp @@ -0,0 +1,118 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include + +#include + +#include "jointconfiguration.hpp" +#include "cartesianconfiguration.hpp" +#include "command_types_typedef.hpp" +#include "sequence.hpp" +#include "gripper.hpp" + +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Abstract base class describing the interface to access + * test data like robot poses and robot commands. + */ +class TestdataLoader +{ +public: + TestdataLoader() = default; + + TestdataLoader(moveit::core::RobotModelConstPtr robot_model) : robot_model_(std::move(robot_model)) + { + } + + TestdataLoader(const TestdataLoader&) = default; + TestdataLoader(TestdataLoader&&) = default; + TestdataLoader& operator=(const TestdataLoader&) = default; + TestdataLoader& operator=(TestdataLoader&&) = default; + virtual ~TestdataLoader() = default; + +public: + void setRobotModel(moveit::core::RobotModelConstPtr robot_model); + + virtual JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const = 0; + + virtual CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual PtpJoint getPtpJoint(const std::string& cmd_name) const = 0; + virtual PtpCart getPtpCart(const std::string& cmd_name) const = 0; + virtual PtpJointCart getPtpJointCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual LinJoint getLinJoint(const std::string& cmd_name) const = 0; + virtual LinCart getLinCart(const std::string& cmd_name) const = 0; + virtual LinJointCart getLinJointCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const = 0; + virtual CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const = 0; + virtual CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const = 0; + virtual CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual Sequence getSequence(const std::string& cmd_name) const = 0; + + /** + * @brief Returns the command with the specified name from the test data. + */ + virtual Gripper getGripper(const std::string& cmd_name) const = 0; + +protected: + moveit::core::RobotModelConstPtr robot_model_; +}; + +inline void TestdataLoader::setRobotModel(moveit::core::RobotModelConstPtr robot_model) +{ + robot_model_ = std::move(robot_model); +} + +using TestdataLoaderUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h index 1486b1fd59..35a42a250a 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,45 +45,5 @@ *********************************************************************/ #pragma once - -#include - -namespace pilz_industrial_motion_planner_testutils -{ -const std::string EMPTY_STR{}; - -const std::string XML_ATTR_STR{ "" }; -const std::string JOINT_STR{ "joints" }; -const std::string POSE_STR{ "pos" }; -const std::string XYZ_QUAT_STR{ "xyzQuat" }; -const std::string XYZ_EULER_STR{ "xyzEuler" }; -const std::string SEED_STR{ "seed" }; - -const std::string PTP_STR{ "ptp" }; -const std::string LIN_STR{ "lin" }; -const std::string CIRC_STR{ "circ" }; -const std::string BLEND_STR{ "blend" }; -const std::string GRIPPER_STR{ "gripper" }; - -const std::string PLANNING_GROUP_STR{ "planningGroup" }; -const std::string TARGET_LINK_STR{ "targetLink" }; -const std::string START_POS_STR{ "startPos" }; -const std::string END_POS_STR{ "endPos" }; -const std::string INTERMEDIATE_POS_STR{ "intermediatePos" }; -const std::string CENTER_POS_STR{ "centerPos" }; -const std::string VEL_STR{ "vel" }; -const std::string ACC_STR{ "acc" }; - -const std::string POSES_PATH_STR{ "testdata.poses" }; -const std::string PTPS_PATH_STR{ "testdata." + PTP_STR + "s" }; -const std::string LINS_PATH_STR{ "testdata." + LIN_STR + "s" }; -const std::string CIRCS_PATH_STR{ "testdata." + CIRC_STR + "s" }; -const std::string SEQUENCE_PATH_STR{ "testdata.sequences" }; -const std::string GRIPPERS_PATH_STR{ "testdata." + GRIPPER_STR + "s" }; - -const std::string NAME_PATH_STR{ XML_ATTR_STR + ".name" }; -const std::string CMD_TYPE_PATH_STR{ XML_ATTR_STR + ".type" }; -const std::string BLEND_RADIUS_PATH_STR{ XML_ATTR_STR + ".blend_radius" }; -const std::string LINK_NAME_PATH_STR{ XML_ATTR_STR + ".link_name" }; -const std::string GROUP_NAME_PATH_STR{ XML_ATTR_STR + ".group_name" }; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.hpp new file mode 100644 index 0000000000..1486b1fd59 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_constants.hpp @@ -0,0 +1,77 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include + +namespace pilz_industrial_motion_planner_testutils +{ +const std::string EMPTY_STR{}; + +const std::string XML_ATTR_STR{ "" }; +const std::string JOINT_STR{ "joints" }; +const std::string POSE_STR{ "pos" }; +const std::string XYZ_QUAT_STR{ "xyzQuat" }; +const std::string XYZ_EULER_STR{ "xyzEuler" }; +const std::string SEED_STR{ "seed" }; + +const std::string PTP_STR{ "ptp" }; +const std::string LIN_STR{ "lin" }; +const std::string CIRC_STR{ "circ" }; +const std::string BLEND_STR{ "blend" }; +const std::string GRIPPER_STR{ "gripper" }; + +const std::string PLANNING_GROUP_STR{ "planningGroup" }; +const std::string TARGET_LINK_STR{ "targetLink" }; +const std::string START_POS_STR{ "startPos" }; +const std::string END_POS_STR{ "endPos" }; +const std::string INTERMEDIATE_POS_STR{ "intermediatePos" }; +const std::string CENTER_POS_STR{ "centerPos" }; +const std::string VEL_STR{ "vel" }; +const std::string ACC_STR{ "acc" }; + +const std::string POSES_PATH_STR{ "testdata.poses" }; +const std::string PTPS_PATH_STR{ "testdata." + PTP_STR + "s" }; +const std::string LINS_PATH_STR{ "testdata." + LIN_STR + "s" }; +const std::string CIRCS_PATH_STR{ "testdata." + CIRC_STR + "s" }; +const std::string SEQUENCE_PATH_STR{ "testdata.sequences" }; +const std::string GRIPPERS_PATH_STR{ "testdata." + GRIPPER_STR + "s" }; + +const std::string NAME_PATH_STR{ XML_ATTR_STR + ".name" }; +const std::string CMD_TYPE_PATH_STR{ XML_ATTR_STR + ".type" }; +const std::string BLEND_RADIUS_PATH_STR{ XML_ATTR_STR + ".blend_radius" }; +const std::string LINK_NAME_PATH_STR{ XML_ATTR_STR + ".link_name" }; +const std::string GROUP_NAME_PATH_STR{ XML_ATTR_STR + ".group_name" }; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h index 213c11a321..24d2d23744 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,194 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include -#include -#include -#include - -#include - -#include - -namespace pt = boost::property_tree; -namespace pilz_industrial_motion_planner_testutils -{ -/** - * @brief Implements a test data loader which uses a xml file - * to store the test data. - * - * The Xml-file has the following structure: - * - * - * - * - * - * j1 j2 j3 j4 j5 j6 - * - * x y z wx wy wz w - * s1 s2 s3 s4 s5 s6 - * - * j_gripper - * - * - * - * j1 j2 j3 j4 j5 j6 - * x y z wx wy wz w - * j_gripper - * - * - * - * - * - * MyTestPos1 - * MyTestPos2 - * 0.1 - * 0.2 - * - * - * - * - * - * manipulator - * prbt_tcp - * MyTestPos1 - * MyTestPos2 - * 0.3 - * 0.4 - * - * - * - * - * - * manipulator - * prbt_tcp - * MyTestPos1 - * MyTestPos1 - * MyTestPos2 - * MyTestPos1 - * 0.2 - * 0.5 - * - * - * - * - * - * - * - * - * - * - * - * - */ - -class XmlTestdataLoader : public TestdataLoader -{ -public: - XmlTestdataLoader(const std::string& path_filename); - XmlTestdataLoader(const std::string& path_filename, const moveit::core::RobotModelConstPtr& robot_model); - ~XmlTestdataLoader() override; - -public: - JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const override; - - CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const override; - - PtpJoint getPtpJoint(const std::string& cmd_name) const override; - PtpCart getPtpCart(const std::string& cmd_name) const override; - PtpJointCart getPtpJointCart(const std::string& cmd_name) const override; - - LinJoint getLinJoint(const std::string& cmd_name) const override; - LinCart getLinCart(const std::string& cmd_name) const override; - LinJointCart getLinJointCart(const std::string& cmd_name) const override; - - CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const override; - CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const override; - CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const override; - CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const override; - - Sequence getSequence(const std::string& cmd_name) const override; - - Gripper getGripper(const std::string& cmd_name) const override; - -private: - /** - * @brief Use this function to search for a node (like an pos or cmd) - * with a given name. - * - * @param tree Tree containing the node. - * @param name Name of node to look for. - */ - const pt::ptree::value_type& findNodeWithName(const boost::property_tree::ptree& tree, const std::string& name, - const std::string& key, const std::string& path = "") const; - - /** - * @brief Use this function to search for a cmd-node with a given name. - */ - const pt::ptree::value_type& findCmd(const std::string& cmd_name, const std::string& cmd_path, - const std::string& cmd_key) const; - - CartesianCenter getCartesianCenter(const std::string& cmd_name, const std::string& planning_group) const; - - CartesianInterim getCartesianInterim(const std::string& cmd_name, const std::string& planning_group) const; - -private: - JointConfiguration getJoints(const boost::property_tree::ptree& joints_tree, const std::string& group_name) const; - -private: - /** - * @brief Converts string vector to double vector. - */ - static inline std::vector strVec2doubleVec(std::vector& strVec); - -public: - /** - * @brief Abstract base class providing a GENERIC getter-function signature - * which can be used to load DIFFERENT command types (like Ptp, Lin, etc.) - * from the test data file. - */ - class AbstractCmdGetterAdapter - { - public: - AbstractCmdGetterAdapter() = default; - AbstractCmdGetterAdapter(const AbstractCmdGetterAdapter&) = default; - AbstractCmdGetterAdapter(AbstractCmdGetterAdapter&&) = default; - AbstractCmdGetterAdapter& operator=(const AbstractCmdGetterAdapter&) = default; - AbstractCmdGetterAdapter& operator=(AbstractCmdGetterAdapter&&) = default; - virtual ~AbstractCmdGetterAdapter() = default; - - virtual CmdVariant getCmd(const std::string& /*cmd_name*/) const = 0; - }; - -private: - std::string path_filename_; - pt::ptree tree_{}; - - using AbstractCmdGetterUPtr = std::unique_ptr; - - //! Stores the mapping between command type and the getter function - //! which have to be called. - //! - //! Please note: - //! This mapping is only relevant for sequence commands. - std::map cmd_getter_funcs_; - -private: - const pt::ptree::value_type empty_value_type_{}; - const pt::ptree empty_tree_{}; -}; - -std::vector XmlTestdataLoader::strVec2doubleVec(std::vector& strVec) -{ - std::vector vec; - - vec.resize(strVec.size()); - std::transform(strVec.begin(), strVec.end(), vec.begin(), [](const std::string& val) { return std::stod(val); }); - - return vec; -} - -using XmlTestDataLoaderUPtr = std::unique_ptr; -} // namespace pilz_industrial_motion_planner_testutils +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.hpp b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.hpp new file mode 100644 index 0000000000..3e2baa5bd3 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.hpp @@ -0,0 +1,226 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * 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 Pilz GmbH & Co. KG 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. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include + +namespace pt = boost::property_tree; +namespace pilz_industrial_motion_planner_testutils +{ +/** + * @brief Implements a test data loader which uses a xml file + * to store the test data. + * + * The Xml-file has the following structure: + * + * + * + * + * + * j1 j2 j3 j4 j5 j6 + * + * x y z wx wy wz w + * s1 s2 s3 s4 s5 s6 + * + * j_gripper + * + * + * + * j1 j2 j3 j4 j5 j6 + * x y z wx wy wz w + * j_gripper + * + * + * + * + * + * MyTestPos1 + * MyTestPos2 + * 0.1 + * 0.2 + * + * + * + * + * + * manipulator + * prbt_tcp + * MyTestPos1 + * MyTestPos2 + * 0.3 + * 0.4 + * + * + * + * + * + * manipulator + * prbt_tcp + * MyTestPos1 + * MyTestPos1 + * MyTestPos2 + * MyTestPos1 + * 0.2 + * 0.5 + * + * + * + * + * + * + * + * + * + * + * + * + */ + +class XmlTestdataLoader : public TestdataLoader +{ +public: + XmlTestdataLoader(const std::string& path_filename); + XmlTestdataLoader(const std::string& path_filename, const moveit::core::RobotModelConstPtr& robot_model); + ~XmlTestdataLoader() override; + +public: + JointConfiguration getJoints(const std::string& pos_name, const std::string& group_name) const override; + + CartesianConfiguration getPose(const std::string& pos_name, const std::string& group_name) const override; + + PtpJoint getPtpJoint(const std::string& cmd_name) const override; + PtpCart getPtpCart(const std::string& cmd_name) const override; + PtpJointCart getPtpJointCart(const std::string& cmd_name) const override; + + LinJoint getLinJoint(const std::string& cmd_name) const override; + LinCart getLinCart(const std::string& cmd_name) const override; + LinJointCart getLinJointCart(const std::string& cmd_name) const override; + + CircCenterCart getCircCartCenterCart(const std::string& cmd_name) const override; + CircInterimCart getCircCartInterimCart(const std::string& cmd_name) const override; + CircJointCenterCart getCircJointCenterCart(const std::string& cmd_name) const override; + CircJointInterimCart getCircJointInterimCart(const std::string& cmd_name) const override; + + Sequence getSequence(const std::string& cmd_name) const override; + + Gripper getGripper(const std::string& cmd_name) const override; + +private: + /** + * @brief Use this function to search for a node (like an pos or cmd) + * with a given name. + * + * @param tree Tree containing the node. + * @param name Name of node to look for. + */ + const pt::ptree::value_type& findNodeWithName(const boost::property_tree::ptree& tree, const std::string& name, + const std::string& key, const std::string& path = "") const; + + /** + * @brief Use this function to search for a cmd-node with a given name. + */ + const pt::ptree::value_type& findCmd(const std::string& cmd_name, const std::string& cmd_path, + const std::string& cmd_key) const; + + CartesianCenter getCartesianCenter(const std::string& cmd_name, const std::string& planning_group) const; + + CartesianInterim getCartesianInterim(const std::string& cmd_name, const std::string& planning_group) const; + +private: + JointConfiguration getJoints(const boost::property_tree::ptree& joints_tree, const std::string& group_name) const; + +private: + /** + * @brief Converts string vector to double vector. + */ + static inline std::vector strVec2doubleVec(std::vector& strVec); + +public: + /** + * @brief Abstract base class providing a GENERIC getter-function signature + * which can be used to load DIFFERENT command types (like Ptp, Lin, etc.) + * from the test data file. + */ + class AbstractCmdGetterAdapter + { + public: + AbstractCmdGetterAdapter() = default; + AbstractCmdGetterAdapter(const AbstractCmdGetterAdapter&) = default; + AbstractCmdGetterAdapter(AbstractCmdGetterAdapter&&) = default; + AbstractCmdGetterAdapter& operator=(const AbstractCmdGetterAdapter&) = default; + AbstractCmdGetterAdapter& operator=(AbstractCmdGetterAdapter&&) = default; + virtual ~AbstractCmdGetterAdapter() = default; + + virtual CmdVariant getCmd(const std::string& /*cmd_name*/) const = 0; + }; + +private: + std::string path_filename_; + pt::ptree tree_{}; + + using AbstractCmdGetterUPtr = std::unique_ptr; + + //! Stores the mapping between command type and the getter function + //! which have to be called. + //! + //! Please note: + //! This mapping is only relevant for sequence commands. + std::map cmd_getter_funcs_; + +private: + const pt::ptree::value_type empty_value_type_{}; + const pt::ptree empty_tree_{}; +}; + +std::vector XmlTestdataLoader::strVec2doubleVec(std::vector& strVec) +{ + std::vector vec; + + vec.resize(strVec.size()); + std::transform(strVec.begin(), strVec.end(), vec.begin(), [](const std::string& val) { return std::stod(val); }); + + return vec; +} + +using XmlTestDataLoaderUPtr = std::unique_ptr; +} // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp index 3cd8c9f9d4..9c4a6da560 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/cartesianconfiguration.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp index feedad72e1..6f53bdc91f 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/jointconfiguration.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include -#include +#include namespace pilz_industrial_motion_planner_testutils { diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp index b267f725ee..35b49e987f 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/robotconfiguration.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp index e449b14e2a..e914b73cd3 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp index 4f775d678e..7240a0da7a 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -32,16 +32,16 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include #include -#include -#include -#include +#include +#include +#include namespace pt = boost::property_tree; namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/stomp/include/stomp_moveit/conversion_functions.h b/moveit_planners/stomp/include/stomp_moveit/conversion_functions.h new file mode 100644 index 0000000000..a4f6a35503 --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/conversion_functions.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief Helper functions for converting between MoveIt types and plain Eigen types. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp index 20c5d83773..f4a94824ad 100644 --- a/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp @@ -39,8 +39,8 @@ #pragma once -#include -#include +#include +#include namespace stomp_moveit { diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.h b/moveit_planners/stomp/include/stomp_moveit/cost_functions.h new file mode 100644 index 0000000000..125fa9068c --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief MoveIt-based cost functions that can be passed to STOMP via a ComposableTask. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp index 705f796cba..b910cd3393 100644 --- a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp @@ -41,8 +41,8 @@ #include -#include -#include +#include +#include #include #include diff --git a/moveit_planners/stomp/include/stomp_moveit/filter_functions.h b/moveit_planners/stomp/include/stomp_moveit/filter_functions.h new file mode 100644 index 0000000000..5d1872c522 --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/filter_functions.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief Filter functions that can be passed to STOMP via a ComposableTask. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp index f1ec47b3bb..fe4006a32c 100644 --- a/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp @@ -40,7 +40,7 @@ #pragma once #include -#include +#include #include #include diff --git a/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.h b/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.h new file mode 100644 index 0000000000..32efbac874 --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, 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 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. + *********************************************************************/ + +/** @file + * @author Mrinal Kalakrishnan + * @brief Implementation of a multi-variate Gaussian used for randomizing path waypoints + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/noise_generators.h b/moveit_planners/stomp/include/stomp_moveit/noise_generators.h new file mode 100644 index 0000000000..c27ac1c1d6 --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/noise_generators.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief Noise generator functions for randomizing trajectories in STOMP via a ComposableTask. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.h b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.h new file mode 100644 index 0000000000..1bc31847bf --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief Planning Context implementation for STOMP + **/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp index e7a33c5aec..a5add5e515 100644 --- a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp @@ -39,7 +39,7 @@ #pragma once -#include +#include #include diff --git a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_task.h b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_task.h new file mode 100644 index 0000000000..9781a0c10b --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_task.h @@ -0,0 +1,69 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief A STOMP task definition that allows injecting custom functions for planning. + * + * STOMP's task interface can be used for customizing the planning objective, in particular the code used for sampling + * new random trajectories and for computing costs and validity of waypoint candidates. In order to allow building + * generic planning tasks at runtime, the ComposableTask class enables combining generic function types for planning: + * + * - NoiseGeneratorFn: computes randomized paths + * - CostFn: computes waypoint costs and path validity + * - FilterFn: applies a filter to path waypoints + * - PostIterationFn: reports on planning progress at each iteration (see STOMP documentation) + * - DoneFn: reports on planning result when STOMP run terminates + * + * Each of these functions use Eigen types for representing path and waypoints. + * The Eigen::MatrixXd 'values' refer to full path candidates where rows are the joint dimensions + * and columns are the waypoints. Accordingly, Eigen::VectorXd is used for representing cost values, + * one value for each waypoint. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.h b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.h new file mode 100644 index 0000000000..ab9c11003d --- /dev/null +++ b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.h @@ -0,0 +1,54 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + * @brief: Helper functions for visualizing trajectory markers for STOMP planning iterations. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp index 518f73c855..2bea61fcda 100644 --- a/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/trajectory_visualization.hpp @@ -42,8 +42,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index de5d43a6ff..4d37e61de6 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -49,8 +49,8 @@ #include #include -#include -#include +#include +#include #include namespace stomp_moveit diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp index d035de8c15..4df96995c5 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp @@ -50,8 +50,8 @@ #include #include #include -#include -#include +#include +#include #include using namespace moveit::core; diff --git a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h index 37454204c5..fe3f29d1a1 100644 --- a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h +++ b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,25 +47,5 @@ /* Author: Mathias Lüdtke */ #pragma once - -#include -#include - -namespace moveit_ros_control_interface -{ -MOVEIT_CLASS_FORWARD(ControllerHandleAllocator); // Defines ControllerHandleAllocatorPtr, ConstPtr, WeakPtr... etc - -/** - * Base class for MoveItControllerHandle allocators - */ -class ControllerHandleAllocator -{ -public: - virtual moveit_controller_manager::MoveItControllerHandlePtr - alloc(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::vector& resources) = 0; - virtual ~ControllerHandleAllocator() - { - } -}; - -} // namespace moveit_ros_control_interface +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.hpp b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.hpp new file mode 100644 index 0000000000..f44229a4b3 --- /dev/null +++ b/moveit_plugins/moveit_ros_control_interface/include/moveit_ros_control_interface/ControllerHandle.hpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Fraunhofer IPA + * 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 Fraunhofer IPA 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: Mathias Lüdtke */ + +#pragma once + +#include +#include + +namespace moveit_ros_control_interface +{ +MOVEIT_CLASS_FORWARD(ControllerHandleAllocator); // Defines ControllerHandleAllocatorPtr, ConstPtr, WeakPtr... etc + +/** + * Base class for MoveItControllerHandle allocators + */ +class ControllerHandleAllocator +{ +public: + virtual moveit_controller_manager::MoveItControllerHandlePtr + alloc(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::vector& resources) = 0; + virtual ~ControllerHandleAllocator() + { + } +}; + +} // namespace moveit_ros_control_interface diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index eb7b2e8205..42d5378d0e 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -34,10 +34,10 @@ /* Author: Mathias Lüdtke */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_plugins/moveit_ros_control_interface/src/empty_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/empty_controller_plugin.cpp index 0b2887031b..a8e23607d6 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/empty_controller_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/empty_controller_plugin.cpp @@ -35,7 +35,7 @@ /* Author: Paul Gesel */ #include -#include +#include #include namespace moveit_ros_control_interface diff --git a/moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp index d7e213a52a..8d6e9fafad 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp @@ -34,9 +34,9 @@ /* Author: Joseph Schornak */ -#include +#include #include -#include +#include #include #include diff --git a/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp index 6ad36fbd90..048d3edec3 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/joint_trajectory_controller_plugin.cpp @@ -34,9 +34,9 @@ /* Author: Mathias Lüdtke */ -#include +#include #include -#include +#include #include #include diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index 72213aa0cc..ade93c7118 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,243 +48,5 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ #pragma once - -#include -#include -#include -#include -#include - -namespace moveit_simple_controller_manager -{ -using namespace std::chrono_literals; - -/* - * This exist solely to inject addJoint/getJoints into base non-templated class. - */ -class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveItControllerHandle -{ -public: - ActionBasedControllerHandleBase(const std::string& name, const std::string& logger_name) - : moveit_controller_manager::MoveItControllerHandle(name), logger_(rclcpp::get_logger(logger_name)) - { - } - - virtual void addJoint(const std::string& name) = 0; - virtual void getJoints(std::vector& joints) = 0; - // TODO(JafarAbdi): Revise parameter lookup - // virtual void configure(XmlRpc::XmlRpcValue& /* config */) - // { - // } - -protected: - const rclcpp::Logger logger_; -}; - -MOVEIT_CLASS_FORWARD( - ActionBasedControllerHandleBase); // Defines ActionBasedControllerHandleBasePtr, ConstPtr, WeakPtr... etc - -/** - * @brief Base class for controller handles that interact with a controller through a ROS action server. - */ -template -class ActionBasedControllerHandle : public ActionBasedControllerHandleBase -{ -public: - ActionBasedControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns, - const std::string& logger_name) - : ActionBasedControllerHandleBase(name, logger_name), node_(node), done_(true), namespace_(ns) - { - // Creating the action client does not ensure that the action server is actually running. Executing trajectories - // through the controller handle will fail if the server is not running when an action goal message is sent. - controller_action_client_ = rclcpp_action::create_client(node, getActionName()); - last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; - } - - /** - * @brief Cancels trajectory execution by triggering the controller action server's cancellation callback. - * @return True if cancellation was accepted, false if cancellation failed. - */ - bool cancelExecution() override - { - if (!controller_action_client_) - return false; - if (!done_) - { - RCLCPP_INFO_STREAM(logger_, "Cancelling execution for " << name_); - auto cancel_result_future = controller_action_client_->async_cancel_goal(current_goal_); - - const auto& result = cancel_result_future.get(); - if (!result) - RCLCPP_ERROR(logger_, "Failed to cancel goal"); - - last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; - done_ = true; - } - return true; - } - - /** - * @brief Callback function to call when the result is received from the controller action server. - * @param wrapped_result - */ - virtual void - controllerDoneCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) = 0; - - /** - * @brief Blocks waiting for the action result to be received. - * @param timeout Duration to wait for a result before failing. Default value indicates no timeout. - * @return True if a result was received, false on timeout. - */ - bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration::from_seconds(-1.0)) override - { - auto result_callback_done = std::make_shared>(); - auto result_future = controller_action_client_->async_get_result( - current_goal_, [this, result_callback_done](const auto& wrapped_result) { - controllerDoneCallback(wrapped_result); - result_callback_done->set_value(true); - }); - if (timeout < std::chrono::nanoseconds(0)) - { - result_future.wait(); - } - else - { - std::future_status status; - if (node_->get_parameter("use_sim_time").as_bool()) - { - const auto start = node_->now(); - do - { - status = result_future.wait_for(50ms); - if ((status == std::future_status::timeout) and ((node_->now() - start) > timeout)) - { - RCLCPP_WARN(logger_, "waitForExecution timed out"); - return false; - } - } while (status == std::future_status::timeout); - } - else - { - status = result_future.wait_for(timeout.to_chrono>()); - if (status == std::future_status::timeout) - { - RCLCPP_WARN(logger_, "waitForExecution timed out"); - return false; - } - } - } - // To accommodate for the delay after the future for the result is ready and the time controllerDoneCallback takes to finish - result_callback_done->get_future().wait(); - return true; - } - - moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override - { - return last_exec_; - } - - void addJoint(const std::string& name) override - { - joints_.push_back(name); - } - - void getJoints(std::vector& joints) override - { - joints = joints_; - } - -protected: - /** - * @brief A pointer to the node, required to read parameters and get the time. - */ - const rclcpp::Node::SharedPtr node_; - - /** - * @brief Check if the controller's action server is ready to receive action goals. - * @return True if the action server is ready, false if it is not ready or does not exist. - */ - bool isConnected() const - { - return controller_action_client_->action_server_is_ready(); - } - - /** - * @brief Get the full name of the action using the action namespace and base name. - * @return The action name. - */ - std::string getActionName() const - { - if (namespace_.empty()) - { - return name_; - } - else - { - return name_ + "/" + namespace_; - } - } - - /** - * @brief Indicate that the controller handle is done executing the trajectory and set the controller manager handle's - * ExecutionStatus based on the received action ResultCode. - * @param rclcpp_action::ResultCode to convert to moveit_controller_manager::ExecutionStatus. - */ - void finishControllerExecution(const rclcpp_action::ResultCode& state) - { - RCLCPP_DEBUG_STREAM(logger_, "Controller " << name_ << " is done with state " << static_cast(state)); - if (state == rclcpp_action::ResultCode::SUCCEEDED) - { - last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; - } - else if (state == rclcpp_action::ResultCode::ABORTED) - { - last_exec_ = moveit_controller_manager::ExecutionStatus::ABORTED; - } - else if (state == rclcpp_action::ResultCode::CANCELED) - { - last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; - } - else if (state == rclcpp_action::ResultCode::UNKNOWN) - { - last_exec_ = moveit_controller_manager::ExecutionStatus::UNKNOWN; - } - else - { - last_exec_ = moveit_controller_manager::ExecutionStatus::FAILED; - } - done_ = true; - } - - /** - * @brief Status after last trajectory execution. - */ - moveit_controller_manager::ExecutionStatus last_exec_; - - /** - * @brief Indicates whether the controller handle is done executing its current trajectory. - */ - bool done_; - - /** - * @brief The controller namespace. The controller action server's topics will map to name/ns/goal, name/ns/result, etc. - */ - std::string namespace_; - - /** - * @brief The joints controlled by this controller. - */ - std::vector joints_; - - /** - * @brief Action client to send trajectories to the controller's action server. - */ - typename rclcpp_action::Client::SharedPtr controller_action_client_; - - /** - * @brief Current goal that has been sent to the action server. - */ - typename rclcpp_action::ClientGoalHandle::SharedPtr current_goal_; -}; - -} // namespace moveit_simple_controller_manager +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.hpp new file mode 100644 index 0000000000..cce6cc991a --- /dev/null +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.hpp @@ -0,0 +1,278 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Unbounded Robotics Inc. + * Copyright (c) 2012, 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 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: Michael Ferguson, Ioan Sucan, E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit_simple_controller_manager +{ +using namespace std::chrono_literals; + +/* + * This exist solely to inject addJoint/getJoints into base non-templated class. + */ +class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveItControllerHandle +{ +public: + ActionBasedControllerHandleBase(const std::string& name, const std::string& logger_name) + : moveit_controller_manager::MoveItControllerHandle(name), logger_(rclcpp::get_logger(logger_name)) + { + } + + virtual void addJoint(const std::string& name) = 0; + virtual void getJoints(std::vector& joints) = 0; + // TODO(JafarAbdi): Revise parameter lookup + // virtual void configure(XmlRpc::XmlRpcValue& /* config */) + // { + // } + +protected: + const rclcpp::Logger logger_; +}; + +MOVEIT_CLASS_FORWARD( + ActionBasedControllerHandleBase); // Defines ActionBasedControllerHandleBasePtr, ConstPtr, WeakPtr... etc + +/** + * @brief Base class for controller handles that interact with a controller through a ROS action server. + */ +template +class ActionBasedControllerHandle : public ActionBasedControllerHandleBase +{ +public: + ActionBasedControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns, + const std::string& logger_name) + : ActionBasedControllerHandleBase(name, logger_name), node_(node), done_(true), namespace_(ns) + { + // Creating the action client does not ensure that the action server is actually running. Executing trajectories + // through the controller handle will fail if the server is not running when an action goal message is sent. + controller_action_client_ = rclcpp_action::create_client(node, getActionName()); + last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; + } + + /** + * @brief Cancels trajectory execution by triggering the controller action server's cancellation callback. + * @return True if cancellation was accepted, false if cancellation failed. + */ + bool cancelExecution() override + { + if (!controller_action_client_) + return false; + if (!done_) + { + RCLCPP_INFO_STREAM(logger_, "Cancelling execution for " << name_); + auto cancel_result_future = controller_action_client_->async_cancel_goal(current_goal_); + + const auto& result = cancel_result_future.get(); + if (!result) + RCLCPP_ERROR(logger_, "Failed to cancel goal"); + + last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; + done_ = true; + } + return true; + } + + /** + * @brief Callback function to call when the result is received from the controller action server. + * @param wrapped_result + */ + virtual void + controllerDoneCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) = 0; + + /** + * @brief Blocks waiting for the action result to be received. + * @param timeout Duration to wait for a result before failing. Default value indicates no timeout. + * @return True if a result was received, false on timeout. + */ + bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration::from_seconds(-1.0)) override + { + auto result_callback_done = std::make_shared>(); + auto result_future = controller_action_client_->async_get_result( + current_goal_, [this, result_callback_done](const auto& wrapped_result) { + controllerDoneCallback(wrapped_result); + result_callback_done->set_value(true); + }); + if (timeout < std::chrono::nanoseconds(0)) + { + result_future.wait(); + } + else + { + std::future_status status; + if (node_->get_parameter("use_sim_time").as_bool()) + { + const auto start = node_->now(); + do + { + status = result_future.wait_for(50ms); + if ((status == std::future_status::timeout) and ((node_->now() - start) > timeout)) + { + RCLCPP_WARN(logger_, "waitForExecution timed out"); + return false; + } + } while (status == std::future_status::timeout); + } + else + { + status = result_future.wait_for(timeout.to_chrono>()); + if (status == std::future_status::timeout) + { + RCLCPP_WARN(logger_, "waitForExecution timed out"); + return false; + } + } + } + // To accommodate for the delay after the future for the result is ready and the time controllerDoneCallback takes to finish + result_callback_done->get_future().wait(); + return true; + } + + moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override + { + return last_exec_; + } + + void addJoint(const std::string& name) override + { + joints_.push_back(name); + } + + void getJoints(std::vector& joints) override + { + joints = joints_; + } + +protected: + /** + * @brief A pointer to the node, required to read parameters and get the time. + */ + const rclcpp::Node::SharedPtr node_; + + /** + * @brief Check if the controller's action server is ready to receive action goals. + * @return True if the action server is ready, false if it is not ready or does not exist. + */ + bool isConnected() const + { + return controller_action_client_->action_server_is_ready(); + } + + /** + * @brief Get the full name of the action using the action namespace and base name. + * @return The action name. + */ + std::string getActionName() const + { + if (namespace_.empty()) + { + return name_; + } + else + { + return name_ + "/" + namespace_; + } + } + + /** + * @brief Indicate that the controller handle is done executing the trajectory and set the controller manager handle's + * ExecutionStatus based on the received action ResultCode. + * @param rclcpp_action::ResultCode to convert to moveit_controller_manager::ExecutionStatus. + */ + void finishControllerExecution(const rclcpp_action::ResultCode& state) + { + RCLCPP_DEBUG_STREAM(logger_, "Controller " << name_ << " is done with state " << static_cast(state)); + if (state == rclcpp_action::ResultCode::SUCCEEDED) + { + last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; + } + else if (state == rclcpp_action::ResultCode::ABORTED) + { + last_exec_ = moveit_controller_manager::ExecutionStatus::ABORTED; + } + else if (state == rclcpp_action::ResultCode::CANCELED) + { + last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; + } + else if (state == rclcpp_action::ResultCode::UNKNOWN) + { + last_exec_ = moveit_controller_manager::ExecutionStatus::UNKNOWN; + } + else + { + last_exec_ = moveit_controller_manager::ExecutionStatus::FAILED; + } + done_ = true; + } + + /** + * @brief Status after last trajectory execution. + */ + moveit_controller_manager::ExecutionStatus last_exec_; + + /** + * @brief Indicates whether the controller handle is done executing its current trajectory. + */ + bool done_; + + /** + * @brief The controller namespace. The controller action server's topics will map to name/ns/goal, name/ns/result, etc. + */ + std::string namespace_; + + /** + * @brief The joints controlled by this controller. + */ + std::vector joints_; + + /** + * @brief Action client to send trajectories to the controller's action server. + */ + typename rclcpp_action::Client::SharedPtr controller_action_client_; + + /** + * @brief Current goal that has been sent to the action server. + */ + typename rclcpp_action::ClientGoalHandle::SharedPtr current_goal_; +}; + +} // namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.h index 48f4cc93a6..bae97d1949 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,55 +47,5 @@ /* Author: Paul Gesel */ #pragma once - -#include -#include - -namespace moveit_simple_controller_manager -{ -/* - * An interface for controllers that have no handle, e.g. chained controllers like an Admittance controller - */ -class EmptyControllerHandle : public moveit_controller_manager::MoveItControllerHandle -{ -public: - /* Topics will map to name/ns/goal, name/ns/result, etc */ - EmptyControllerHandle(const std::string& name, const std::string& logger_name) - : moveit_controller_manager::MoveItControllerHandle(name), logger_(rclcpp::get_logger(logger_name)) - { - } - - bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override - { - RCLCPP_ERROR_STREAM(logger_, "This controller handle does not support trajectory execution."); - return false; - } - - bool cancelExecution() override - { - return true; - } - - /** - * @brief Function called when the TrajectoryExecutionManager waits for a trajectory to finish. - * @return Always returns true because a trajectory is never in progress. - */ - bool waitForExecution(const rclcpp::Duration& /* timeout */) override - { - return true; - } - - /** - * @brief Gets the last trajectory execution status. - * @return Always returns ExecutionStatus::FAILED. - */ - moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override - { - return moveit_controller_manager::ExecutionStatus::FAILED; - } - -private: - const rclcpp::Logger logger_; -}; - -} // end namespace moveit_simple_controller_manager +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.hpp new file mode 100644 index 0000000000..a65b8e8b75 --- /dev/null +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/empty_controller_handle.hpp @@ -0,0 +1,89 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, PickNik 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 Fraunhofer IPA 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: Paul Gesel */ + +#pragma once + +#include +#include + +namespace moveit_simple_controller_manager +{ +/* + * An interface for controllers that have no handle, e.g. chained controllers like an Admittance controller + */ +class EmptyControllerHandle : public moveit_controller_manager::MoveItControllerHandle +{ +public: + /* Topics will map to name/ns/goal, name/ns/result, etc */ + EmptyControllerHandle(const std::string& name, const std::string& logger_name) + : moveit_controller_manager::MoveItControllerHandle(name), logger_(rclcpp::get_logger(logger_name)) + { + } + + bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override + { + RCLCPP_ERROR_STREAM(logger_, "This controller handle does not support trajectory execution."); + return false; + } + + bool cancelExecution() override + { + return true; + } + + /** + * @brief Function called when the TrajectoryExecutionManager waits for a trajectory to finish. + * @return Always returns true because a trajectory is never in progress. + */ + bool waitForExecution(const rclcpp::Duration& /* timeout */) override + { + return true; + } + + /** + * @brief Gets the last trajectory execution status. + * @return Always returns ExecutionStatus::FAILED. + */ + moveit_controller_manager::ExecutionStatus getLastExecutionStatus() override + { + return moveit_controller_manager::ExecutionStatus::FAILED; + } + +private: + const rclcpp::Logger logger_; +}; + +} // end namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h index 70ae825680..f51462cb8c 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,43 +48,5 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ #pragma once - -#include -#include -#include -#include - -namespace moveit_simple_controller_manager -{ -/* - * This is generally used for arms, but could also be used for multi-dof hands, - * or anything using a control_mgs/FollowJointTrajectoryAction. - */ -class FollowJointTrajectoryControllerHandle - : public ActionBasedControllerHandle -{ -public: - FollowJointTrajectoryControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, - const std::string& action_ns) - : ActionBasedControllerHandle( - node, name, action_ns, "moveit.simple_controller_manager.follow_joint_trajectory_controller_handle") - { - } - - bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override; - - // TODO(JafarAbdi): Revise parameter lookup - // void configure(XmlRpc::XmlRpcValue& config) override; - -protected: - static control_msgs::msg::JointTolerance& getTolerance(std::vector& tolerances, - const std::string& name); - - void controllerDoneCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) - override; - - control_msgs::action::FollowJointTrajectory::Goal goal_template_; -}; - -} // end namespace moveit_simple_controller_manager +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.hpp new file mode 100644 index 0000000000..6327784fc7 --- /dev/null +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.hpp @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Unbounded Robotics Inc. + * Copyright (c) 2012, 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 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: Michael Ferguson, Ioan Sucan, E. Gil Jones */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit_simple_controller_manager +{ +/* + * This is generally used for arms, but could also be used for multi-dof hands, + * or anything using a control_mgs/FollowJointTrajectoryAction. + */ +class FollowJointTrajectoryControllerHandle + : public ActionBasedControllerHandle +{ +public: + FollowJointTrajectoryControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, + const std::string& action_ns) + : ActionBasedControllerHandle( + node, name, action_ns, "moveit.simple_controller_manager.follow_joint_trajectory_controller_handle") + { + } + + bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override; + + // TODO(JafarAbdi): Revise parameter lookup + // void configure(XmlRpc::XmlRpcValue& config) override; + +protected: + static control_msgs::msg::JointTolerance& getTolerance(std::vector& tolerances, + const std::string& name); + + void controllerDoneCallback( + const rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) + override; + + control_msgs::action::FollowJointTrajectory::Goal goal_template_; +}; + +} // end namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h index 42f9b8a976..761eaf3ef9 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,206 +48,5 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ #pragma once - -#include -#include -#include - -namespace moveit_simple_controller_manager -{ -/* - * This is an interface for a gripper using control_msgs/GripperCommandAction - * action interface (single DOF). - */ -class GripperControllerHandle : public ActionBasedControllerHandle -{ -public: - /* Topics will map to name/ns/goal, name/ns/result, etc */ - GripperControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns, - const double max_effort = 0.0) - : ActionBasedControllerHandle( - node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle") - , allow_failure_(false) - , parallel_jaw_gripper_(false) - , max_effort_(max_effort) - { - } - - bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override - { - RCLCPP_DEBUG_STREAM(logger_, "Received new trajectory for " << name_); - - if (!controller_action_client_) - return false; - - if (!isConnected()) - { - RCLCPP_ERROR_STREAM(logger_, "Action client not connected to action server: " << getActionName()); - return false; - } - - if (!trajectory.multi_dof_joint_trajectory.points.empty()) - { - RCLCPP_ERROR(logger_, "Gripper cannot execute multi-dof trajectories."); - return false; - } - - if (trajectory.joint_trajectory.points.empty()) - { - RCLCPP_ERROR(logger_, "GripperController requires at least one joint trajectory point."); - return false; - } - - // TODO(JafarAbdi): Enable when msg streaming operator is available - // if (trajectory.joint_trajectory.points.size() > 1) - // { - // RCLCPP_DEBUG_STREAM(logger_, "Trajectory: " << trajectory.joint_trajectory); - // } - - if (trajectory.joint_trajectory.joint_names.empty()) - { - RCLCPP_ERROR(logger_, "No joint names specified"); - return false; - } - - std::vector gripper_joint_indexes; - for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i) - { - if (command_joints_.find(trajectory.joint_trajectory.joint_names[i]) != command_joints_.end()) - { - gripper_joint_indexes.push_back(i); - if (!parallel_jaw_gripper_) - break; - } - } - - if (gripper_joint_indexes.empty()) - { - RCLCPP_WARN(logger_, "No command_joint was specified for the MoveIt controller gripper handle. \ - Please see GripperControllerHandle::addCommandJoint() and \ - GripperControllerHandle::setCommandJoint(). Assuming index 0."); - gripper_joint_indexes.push_back(0); - } - - // goal to be sent - control_msgs::action::GripperCommand::Goal goal; - goal.command.position = 0.0; - - // send last point - int tpoint = trajectory.joint_trajectory.points.size() - 1; - RCLCPP_DEBUG(logger_, "Sending command from trajectory point %d", tpoint); - - // fill in goal from last point - for (std::size_t idx : gripper_joint_indexes) - { - if (trajectory.joint_trajectory.points[tpoint].positions.size() <= idx) - { - RCLCPP_ERROR(logger_, "GripperController expects a joint trajectory with one \ - point that specifies at least the position of joint \ - '%s', but insufficient positions provided", - trajectory.joint_trajectory.joint_names[idx].c_str()); - return false; - } - goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx]; - - if (trajectory.joint_trajectory.points[tpoint].effort.size() > idx) - { - goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx]; - } - else - { - goal.command.max_effort = max_effort_; - } - } - rclcpp_action::Client::SendGoalOptions send_goal_options; - // Active callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::Client::GoalHandle::SharedPtr& - /* unused-arg */) { RCLCPP_DEBUG_STREAM(logger_, name_ << " started execution"); }; - // Send goal - auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options); - current_goal_ = current_goal_future.get(); - if (!current_goal_) - { - RCLCPP_ERROR(logger_, "Goal was rejected by server"); - return false; - } - - done_ = false; - last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; - return true; - } - - void setCommandJoint(const std::string& name) - { - command_joints_.clear(); - addCommandJoint(name); - } - - void addCommandJoint(const std::string& name) - { - command_joints_.insert(name); - } - - void allowFailure(bool allow) - { - allow_failure_ = allow; - } - - void setParallelJawGripper(const std::string& left, const std::string& right) - { - addCommandJoint(left); - addCommandJoint(right); - parallel_jaw_gripper_ = true; - } - -private: - void controllerDoneCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) override - { - if (wrapped_result.code == rclcpp_action::ResultCode::ABORTED && allow_failure_) - { - finishControllerExecution(rclcpp_action::ResultCode::SUCCEEDED); - } - else - { - finishControllerExecution(wrapped_result.code); - } - } - - /* - * Some gripper drivers may indicate a failure if they do not close all the way when - * an object is in the gripper. - */ - bool allow_failure_; - - /* - * A common setup is where there are two joints that each move - * half the overall distance. Thus, the command to the gripper - * should be the sum of the two joint distances. - * - * When this is set, command_joints_ should be of size 2, - * and the command will be the sum of the two joints. - */ - bool parallel_jaw_gripper_; - - /* - * The ``max_effort`` used in the GripperCommand message when no ``max_effort`` was - * specified in the requested trajectory. Defaults to ``0.0``. - */ - double max_effort_; - - /* - * A GripperCommand message has only a single float64 for the - * "command", thus only a single joint angle can be sent -- however, - * due to the complexity of making grippers look correct in a URDF, - * they typically have >1 joints. The "command_joint" is the joint - * whose position value will be sent in the GripperCommand action. A - * set of names is provided for acceptable joint names. If any of - * the joints specified is found, the value corresponding to that - * joint is considered the command. - */ - std::set command_joints_; -}; // namespace moveit_simple_controller_manager - -} // end namespace moveit_simple_controller_manager +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp new file mode 100644 index 0000000000..6f2dc9a21b --- /dev/null +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp @@ -0,0 +1,241 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Unbounded Robotics Inc. + * Copyright (c) 2012, 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 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: Michael Ferguson, Ioan Sucan, E. Gil Jones */ + +#pragma once + +#include +#include +#include + +namespace moveit_simple_controller_manager +{ +/* + * This is an interface for a gripper using control_msgs/GripperCommandAction + * action interface (single DOF). + */ +class GripperControllerHandle : public ActionBasedControllerHandle +{ +public: + /* Topics will map to name/ns/goal, name/ns/result, etc */ + GripperControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns, + const double max_effort = 0.0) + : ActionBasedControllerHandle( + node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle") + , allow_failure_(false) + , parallel_jaw_gripper_(false) + , max_effort_(max_effort) + { + } + + bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override + { + RCLCPP_DEBUG_STREAM(logger_, "Received new trajectory for " << name_); + + if (!controller_action_client_) + return false; + + if (!isConnected()) + { + RCLCPP_ERROR_STREAM(logger_, "Action client not connected to action server: " << getActionName()); + return false; + } + + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(logger_, "Gripper cannot execute multi-dof trajectories."); + return false; + } + + if (trajectory.joint_trajectory.points.empty()) + { + RCLCPP_ERROR(logger_, "GripperController requires at least one joint trajectory point."); + return false; + } + + // TODO(JafarAbdi): Enable when msg streaming operator is available + // if (trajectory.joint_trajectory.points.size() > 1) + // { + // RCLCPP_DEBUG_STREAM(logger_, "Trajectory: " << trajectory.joint_trajectory); + // } + + if (trajectory.joint_trajectory.joint_names.empty()) + { + RCLCPP_ERROR(logger_, "No joint names specified"); + return false; + } + + std::vector gripper_joint_indexes; + for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i) + { + if (command_joints_.find(trajectory.joint_trajectory.joint_names[i]) != command_joints_.end()) + { + gripper_joint_indexes.push_back(i); + if (!parallel_jaw_gripper_) + break; + } + } + + if (gripper_joint_indexes.empty()) + { + RCLCPP_WARN(logger_, "No command_joint was specified for the MoveIt controller gripper handle. \ + Please see GripperControllerHandle::addCommandJoint() and \ + GripperControllerHandle::setCommandJoint(). Assuming index 0."); + gripper_joint_indexes.push_back(0); + } + + // goal to be sent + control_msgs::action::GripperCommand::Goal goal; + goal.command.position = 0.0; + + // send last point + int tpoint = trajectory.joint_trajectory.points.size() - 1; + RCLCPP_DEBUG(logger_, "Sending command from trajectory point %d", tpoint); + + // fill in goal from last point + for (std::size_t idx : gripper_joint_indexes) + { + if (trajectory.joint_trajectory.points[tpoint].positions.size() <= idx) + { + RCLCPP_ERROR(logger_, "GripperController expects a joint trajectory with one \ + point that specifies at least the position of joint \ + '%s', but insufficient positions provided", + trajectory.joint_trajectory.joint_names[idx].c_str()); + return false; + } + goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx]; + + if (trajectory.joint_trajectory.points[tpoint].effort.size() > idx) + { + goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx]; + } + else + { + goal.command.max_effort = max_effort_; + } + } + rclcpp_action::Client::SendGoalOptions send_goal_options; + // Active callback + send_goal_options.goal_response_callback = + [this](const rclcpp_action::Client::GoalHandle::SharedPtr& + /* unused-arg */) { RCLCPP_DEBUG_STREAM(logger_, name_ << " started execution"); }; + // Send goal + auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options); + current_goal_ = current_goal_future.get(); + if (!current_goal_) + { + RCLCPP_ERROR(logger_, "Goal was rejected by server"); + return false; + } + + done_ = false; + last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; + return true; + } + + void setCommandJoint(const std::string& name) + { + command_joints_.clear(); + addCommandJoint(name); + } + + void addCommandJoint(const std::string& name) + { + command_joints_.insert(name); + } + + void allowFailure(bool allow) + { + allow_failure_ = allow; + } + + void setParallelJawGripper(const std::string& left, const std::string& right) + { + addCommandJoint(left); + addCommandJoint(right); + parallel_jaw_gripper_ = true; + } + +private: + void controllerDoneCallback( + const rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) override + { + if (wrapped_result.code == rclcpp_action::ResultCode::ABORTED && allow_failure_) + { + finishControllerExecution(rclcpp_action::ResultCode::SUCCEEDED); + } + else + { + finishControllerExecution(wrapped_result.code); + } + } + + /* + * Some gripper drivers may indicate a failure if they do not close all the way when + * an object is in the gripper. + */ + bool allow_failure_; + + /* + * A common setup is where there are two joints that each move + * half the overall distance. Thus, the command to the gripper + * should be the sum of the two joint distances. + * + * When this is set, command_joints_ should be of size 2, + * and the command will be the sum of the two joints. + */ + bool parallel_jaw_gripper_; + + /* + * The ``max_effort`` used in the GripperCommand message when no ``max_effort`` was + * specified in the requested trajectory. Defaults to ``0.0``. + */ + double max_effort_; + + /* + * A GripperCommand message has only a single float64 for the + * "command", thus only a single joint angle can be sent -- however, + * due to the complexity of making grippers look correct in a URDF, + * they typically have >1 joints. The "command_joint" is the joint + * whose position value will be sent in the GripperCommand action. A + * set of names is provided for acceptable joint names. If any of + * the joints specified is found, the value corresponding to that + * joint is considered the command. + */ + std::set command_joints_; +}; // namespace moveit_simple_controller_manager + +} // end namespace moveit_simple_controller_manager diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index c7b49e0751..17afae2a80 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -35,7 +35,7 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#include +#include using namespace std::placeholders; diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index 40af565902..394765c8cb 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -35,9 +35,9 @@ /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_py/src/moveit/core.cpp b/moveit_py/src/moveit/core.cpp index 652a442c01..e65f2b1caa 100644 --- a/moveit_py/src/moveit/core.cpp +++ b/moveit_py/src/moveit/core.cpp @@ -34,18 +34,18 @@ /* Author: Peter David Fagan */ -#include "moveit_core/collision_detection/collision_common.h" -#include "moveit_core/collision_detection/collision_matrix.h" -#include "moveit_core/collision_detection/world.h" -#include "moveit_core/controller_manager/controller_manager.h" -#include "moveit_core/kinematic_constraints/utils.h" -#include "moveit_core/planning_interface/planning_response.h" -#include "moveit_core/planning_scene/planning_scene.h" -#include "moveit_core/robot_model/joint_model.h" -#include "moveit_core/robot_model/joint_model_group.h" -#include "moveit_core/robot_model/robot_model.h" -#include "moveit_core/robot_state/robot_state.h" -#include "moveit_core/robot_trajectory/robot_trajectory.h" +#include "moveit_core/collision_detection/collision_common.hpp" +#include "moveit_core/collision_detection/collision_matrix.hpp" +#include "moveit_core/collision_detection/world.hpp" +#include "moveit_core/controller_manager/controller_manager.hpp" +#include "moveit_core/kinematic_constraints/utils.hpp" +#include "moveit_core/planning_interface/planning_response.hpp" +#include "moveit_core/planning_scene/planning_scene.hpp" +#include "moveit_core/robot_model/joint_model.hpp" +#include "moveit_core/robot_model/joint_model_group.hpp" +#include "moveit_core/robot_model/robot_model.hpp" +#include "moveit_core/robot_state/robot_state.hpp" +#include "moveit_core/robot_trajectory/robot_trajectory.hpp" PYBIND11_MODULE(core, m) { diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp index 6125fa71af..f5744a980d 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "collision_common.h" +#include "collision_common.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp similarity index 96% rename from moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h rename to moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp index 63af366183..39d28cc71d 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp @@ -34,10 +34,12 @@ /* Author: Peter David Fagan */ +#pragma once + #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp index b0ea225050..c3be69f620 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "collision_matrix.h" +#include "collision_matrix.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp similarity index 96% rename from moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h rename to moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp index 68854c532d..f01700f197 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp @@ -34,10 +34,12 @@ /* Author: Peter David Fagan */ +#pragma once + #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp index ee2e69cc62..37c3f54a7a 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp @@ -34,7 +34,7 @@ /* Author: Jafar Uruç */ -#include "world.h" +#include "world.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.h b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp similarity index 97% rename from moveit_py/src/moveit/moveit_core/collision_detection/world.h rename to moveit_py/src/moveit/moveit_core/collision_detection/world.hpp index 2dd0b8ef5b..4eef37ef47 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.h +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp @@ -34,10 +34,12 @@ /* Author: Jafar Uruç */ +#pragma once + #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp index ce9165ce0f..dffcb5550b 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "controller_manager.h" +#include "controller_manager.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp similarity index 97% rename from moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h rename to moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp index 2fa5390292..d52926f30f 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include #include diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp index fa1ee2fea7..9f055cb289 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -34,10 +34,10 @@ /* Author: Peter David Fagan */ -#include "utils.h" +#include "utils.hpp" #include -#include -#include +#include +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp similarity index 98% rename from moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h rename to moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp index 161e3b03fa..b60e619288 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp @@ -34,11 +34,13 @@ /* Author: Peter David Fagan */ +#pragma once + #include #include #include #include -#include +#include #include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp index 8ebf5437f4..92119a1ff6 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "planning_response.h" +#include "planning_response.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp similarity index 92% rename from moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h rename to moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp index 88943db5d6..fdbae99d9f 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp @@ -39,10 +39,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp index b6120cd58e..b87c6edda6 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -34,8 +34,8 @@ /* Author: Peter David Fagan */ -#include "planning_scene.h" -#include +#include "planning_scene.hpp" +#include #include #include diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp similarity index 93% rename from moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h rename to moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp index 14a6d1ca1d..ac0b5e8355 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp @@ -39,9 +39,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp index 7d335b0b7f..9428dc2e00 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp @@ -34,8 +34,8 @@ /* Author: Jafar Uruç */ -#include "joint_model.h" -#include +#include "joint_model.hpp" +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp similarity index 97% rename from moveit_py/src/moveit/moveit_core/robot_model/joint_model.h rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp index bcd72d0171..4502593195 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp index 7d51171047..5325ee4159 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "joint_model_group.h" +#include "joint_model_group.hpp" #include namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp similarity index 98% rename from moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp index be54b09e6a..95d91e3065 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp index 314cc06b06..a1da21f4be 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp @@ -34,12 +34,12 @@ /* Author: Peter David Fagan */ -#include "robot_model.h" +#include "robot_model.hpp" #include #include #include #include -#include +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.hpp similarity index 100% rename from moveit_py/src/moveit/moveit_core/robot_model/robot_model.h rename to moveit_py/src/moveit/moveit_core/robot_model/robot_model.hpp diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp index ed97ef08b6..e0d6016c68 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -34,11 +34,11 @@ /* Author: Peter David Fagan */ -#include "robot_state.h" +#include "robot_state.hpp" #include -#include +#include #include -#include +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp similarity index 97% rename from moveit_py/src/moveit/moveit_core/robot_state/robot_state.h rename to moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp index 7ac1497b29..d0223b1126 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp @@ -44,9 +44,9 @@ #endif #include #pragma GCC diagnostic pop -#include +#include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index a18c121c96..b5360cbdc4 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -34,9 +34,9 @@ /* Author: Peter David Fagan */ -#include "robot_trajectory.h" -#include -#include +#include "robot_trajectory.hpp" +#include +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp similarity index 97% rename from moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h rename to moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp index 7e1cca5ace..293d346614 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp index fc79cdb5bc..5ea7a5ce00 100644 --- a/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "transforms.h" +#include "transforms.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_core/transforms/transforms.h b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp similarity index 98% rename from moveit_py/src/moveit/moveit_core/transforms/transforms.h rename to moveit_py/src/moveit/moveit_core/transforms/transforms.hpp index c8a72ccdb9..55aff743df 100644 --- a/moveit_py/src/moveit/moveit_core/transforms/transforms.h +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h index 48c5540fe1..6554326a71 100644 --- a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,48 +47,5 @@ /* Author: Peter David Fagan */ #pragma once - -#include -#include -#include -#include - -namespace py = pybind11; - -namespace moveit_py -{ -namespace moveit_py_utils -{ -geometry_msgs::msg::PoseStamped poseStampedToCpp(const py::object& pose_stamped); - -// TODO(peterdavidfagan): consider creating typecaster -geometry_msgs::msg::Pose poseToCpp(const py::object& pose); -py::object poseToPy(geometry_msgs::msg::Pose pose); - -geometry_msgs::msg::Point pointToCpp(const py::object& point); - -geometry_msgs::msg::Vector3 vector3ToCpp(const py::object& vector3); - -geometry_msgs::msg::Quaternion quaternionToCpp(const py::object& quaternion); - -shape_msgs::msg::SolidPrimitive solidPrimitiveToCpp(const py::object& primitive); - -shape_msgs::msg::MeshTriangle meshTriangleToCpp(const py::object& mesh_triangle); - -shape_msgs::msg::Mesh meshToCpp(const py::object& mesh); - -moveit_msgs::msg::BoundingVolume boundingVolumeToCpp(const py::object& bounding_volume); - -moveit_msgs::msg::JointConstraint jointConstraintToCpp(const py::object& joint_constraint); - -moveit_msgs::msg::PositionConstraint positionConstraintToCpp(const py::object& position_constraint); - -moveit_msgs::msg::OrientationConstraint orientationConstraintToCpp(const py::object& orientation_constraint); - -moveit_msgs::msg::VisibilityConstraint visibilityConstraintToCpp(const py::object& visibility_constraint); - -moveit_msgs::msg::CollisionObject collisionObjectToCpp(const py::object& collision_object); - -moveit_msgs::msg::Constraints constraintsToCpp(const py::object& constraints); -} // namespace moveit_py_utils -} // namespace moveit_py +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp new file mode 100644 index 0000000000..48c5540fe1 --- /dev/null +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp @@ -0,0 +1,82 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * 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 PickNik Inc. 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: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace moveit_py_utils +{ +geometry_msgs::msg::PoseStamped poseStampedToCpp(const py::object& pose_stamped); + +// TODO(peterdavidfagan): consider creating typecaster +geometry_msgs::msg::Pose poseToCpp(const py::object& pose); +py::object poseToPy(geometry_msgs::msg::Pose pose); + +geometry_msgs::msg::Point pointToCpp(const py::object& point); + +geometry_msgs::msg::Vector3 vector3ToCpp(const py::object& vector3); + +geometry_msgs::msg::Quaternion quaternionToCpp(const py::object& quaternion); + +shape_msgs::msg::SolidPrimitive solidPrimitiveToCpp(const py::object& primitive); + +shape_msgs::msg::MeshTriangle meshTriangleToCpp(const py::object& mesh_triangle); + +shape_msgs::msg::Mesh meshToCpp(const py::object& mesh); + +moveit_msgs::msg::BoundingVolume boundingVolumeToCpp(const py::object& bounding_volume); + +moveit_msgs::msg::JointConstraint jointConstraintToCpp(const py::object& joint_constraint); + +moveit_msgs::msg::PositionConstraint positionConstraintToCpp(const py::object& position_constraint); + +moveit_msgs::msg::OrientationConstraint orientationConstraintToCpp(const py::object& orientation_constraint); + +moveit_msgs::msg::VisibilityConstraint visibilityConstraintToCpp(const py::object& visibility_constraint); + +moveit_msgs::msg::CollisionObject collisionObjectToCpp(const py::object& collision_object); + +moveit_msgs::msg::Constraints constraintsToCpp(const py::object& constraints); +} // namespace moveit_py_utils +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h index da589020f8..d5d06c50a3 100644 --- a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,102 +47,5 @@ /* Author: Peter David Fagan, Robert Haschke */ #pragma once - -#include -#include -#include -#include - -namespace py = pybind11; - -namespace moveit_py -{ -namespace moveit_py_utils -{ -PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); -PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const std::string& ros_msg_name); -} // namespace moveit_py_utils -} // namespace moveit_py - -namespace pybind11 -{ -namespace detail -{ -// Base class for type conversion (C++ <-> python) of ROS message types -template -struct RosMsgTypeCaster -{ - // C++ -> Python - // TODO: add error handling - static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) - { - // serialize src - rclcpp::Serialization serializer; - rclcpp::SerializedMessage serialized_msg; - serializer.serialize_message(&src, &serialized_msg); - py::bytes bytes = py::bytes(reinterpret_cast(serialized_msg.get_rcl_serialized_message().buffer), - serialized_msg.get_rcl_serialized_message().buffer_length); - - // get Python object type - const std::string ros_msg_name = rosidl_generator_traits::name(); - - // find delimiting '/' in ros_msg_name - std::size_t pos1 = ros_msg_name.find('/'); - std::size_t pos2 = ros_msg_name.find('/', pos1 + 1); - py::module m = py::module::import((ros_msg_name.substr(0, pos1) + ".msg").c_str()); - - // retrieve type instance - py::object cls = m.attr(ros_msg_name.substr(pos2 + 1).c_str()); - - // deserialize into python object - py::module rclpy = py::module::import("rclpy.serialization"); - py::object msg = rclpy.attr("deserialize_message")(bytes, cls); - - return msg.release(); - } - - // Python -> C++ - bool load(handle src, bool /*convert*/) - { - // check datatype of src - if (!moveit_py::moveit_py_utils::convertible(src, rosidl_generator_traits::name())) - return false; - - // serialize src into python buffer - py::module rclpy = py::module::import("rclpy.serialization"); - py::bytes bytes = rclpy.attr("serialize_message")(src); - - // deserialize into C++ object - rcl_serialized_message_t rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); - char* serialized_buffer; - Py_ssize_t length; - if (PYBIND11_BYTES_AS_STRING_AND_SIZE(bytes.ptr(), &serialized_buffer, &length)) - { - throw py::error_already_set(); - } - if (length < 0) - { - throw py::error_already_set(); - } - rcl_serialized_msg.buffer_capacity = length; - rcl_serialized_msg.buffer_length = length; - rcl_serialized_msg.buffer = reinterpret_cast(serialized_buffer); - rmw_ret_t rmw_ret = - rmw_deserialize(&rcl_serialized_msg, rosidl_typesupport_cpp::get_message_type_support_handle(), &value); - if (RMW_RET_OK != rmw_ret) - { - throw std::runtime_error("failed to deserialize ROS message"); - } - return true; - } - - PYBIND11_TYPE_CASTER(T, _()); -}; - -template -struct type_caster::value>> : RosMsgTypeCaster -{ -}; - -} // namespace detail -} // namespace pybind11 +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp new file mode 100644 index 0000000000..da589020f8 --- /dev/null +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp @@ -0,0 +1,136 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan, Robert Haschke + * 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. + * * The name of Robert Haschke may not be use 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: Peter David Fagan, Robert Haschke */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace moveit_py_utils +{ +PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); +PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const std::string& ros_msg_name); +} // namespace moveit_py_utils +} // namespace moveit_py + +namespace pybind11 +{ +namespace detail +{ +// Base class for type conversion (C++ <-> python) of ROS message types +template +struct RosMsgTypeCaster +{ + // C++ -> Python + // TODO: add error handling + static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) + { + // serialize src + rclcpp::Serialization serializer; + rclcpp::SerializedMessage serialized_msg; + serializer.serialize_message(&src, &serialized_msg); + py::bytes bytes = py::bytes(reinterpret_cast(serialized_msg.get_rcl_serialized_message().buffer), + serialized_msg.get_rcl_serialized_message().buffer_length); + + // get Python object type + const std::string ros_msg_name = rosidl_generator_traits::name(); + + // find delimiting '/' in ros_msg_name + std::size_t pos1 = ros_msg_name.find('/'); + std::size_t pos2 = ros_msg_name.find('/', pos1 + 1); + py::module m = py::module::import((ros_msg_name.substr(0, pos1) + ".msg").c_str()); + + // retrieve type instance + py::object cls = m.attr(ros_msg_name.substr(pos2 + 1).c_str()); + + // deserialize into python object + py::module rclpy = py::module::import("rclpy.serialization"); + py::object msg = rclpy.attr("deserialize_message")(bytes, cls); + + return msg.release(); + } + + // Python -> C++ + bool load(handle src, bool /*convert*/) + { + // check datatype of src + if (!moveit_py::moveit_py_utils::convertible(src, rosidl_generator_traits::name())) + return false; + + // serialize src into python buffer + py::module rclpy = py::module::import("rclpy.serialization"); + py::bytes bytes = rclpy.attr("serialize_message")(src); + + // deserialize into C++ object + rcl_serialized_message_t rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + char* serialized_buffer; + Py_ssize_t length; + if (PYBIND11_BYTES_AS_STRING_AND_SIZE(bytes.ptr(), &serialized_buffer, &length)) + { + throw py::error_already_set(); + } + if (length < 0) + { + throw py::error_already_set(); + } + rcl_serialized_msg.buffer_capacity = length; + rcl_serialized_msg.buffer_length = length; + rcl_serialized_msg.buffer = reinterpret_cast(serialized_buffer); + rmw_ret_t rmw_ret = + rmw_deserialize(&rcl_serialized_msg, rosidl_typesupport_cpp::get_message_type_support_handle(), &value); + if (RMW_RET_OK != rmw_ret) + { + throw std::runtime_error("failed to deserialize ROS message"); + } + return true; + } + + PYBIND11_TYPE_CASTER(T, _()); +}; + +template +struct type_caster::value>> : RosMsgTypeCaster +{ +}; + +} // namespace detail +} // namespace pybind11 diff --git a/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp index 407a979493..b596ac889c 100644 --- a/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp +++ b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp @@ -35,7 +35,7 @@ /* Author: Peter David Fagan */ #include -#include +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp b/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp index 138d4f600a..6215ca3dda 100644 --- a/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp +++ b/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan, Robert Haschke */ -#include +#include namespace py = pybind11; namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index 40a037675a..a0318ec127 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "moveit_cpp.h" +#include "moveit_cpp.hpp" #include namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp similarity index 90% rename from moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h rename to moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp index 6c287ebe39..6dd6d55ad5 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp @@ -40,14 +40,14 @@ #include #include #include -#include +#include #include -#include -#include -#include +#include +#include +#include #include -#include "planning_component.h" +#include "planning_component.hpp" namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp index e57083e553..0053e1129d 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "planning_component.h" +#include "planning_component.hpp" #include namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp similarity index 92% rename from moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h rename to moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index f0a7c04cee..69204b818c 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -41,16 +41,16 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include "moveit_cpp.h" -#include "../planning_scene_monitor/planning_scene_monitor.h" -#include "../../moveit_core/planning_interface/planning_response.h" +#include "moveit_cpp.hpp" +#include "../planning_scene_monitor/planning_scene_monitor.hpp" +#include "../../moveit_core/planning_interface/planning_response.hpp" namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index 1df36f6a0c..53a2730013 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -34,7 +34,7 @@ /* Author: Peter David Fagan */ -#include "planning_scene_monitor.h" +#include "planning_scene_monitor.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp similarity index 97% rename from moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h rename to moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp index c7c5713d9a..57b3e492a2 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp @@ -38,12 +38,12 @@ #include #include -#include -#include +#include +#include #include #include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index 3872d17665..1c0693e638 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -34,7 +34,7 @@ /* Author: Matthijs van der Burgh */ -#include "trajectory_execution_manager.h" +#include "trajectory_execution_manager.hpp" namespace moveit_py { diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp similarity index 94% rename from moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h rename to moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp index 692f6f71ac..11f1ff159e 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -38,10 +38,10 @@ #include #include -#include -#include +#include +#include #include -#include +#include namespace py = pybind11; diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp index 5e99f70815..5b1f83b339 100644 --- a/moveit_py/src/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.cpp @@ -34,10 +34,10 @@ /* Author: Peter David Fagan */ -#include "moveit_ros/moveit_cpp/moveit_cpp.h" -#include "moveit_ros/moveit_cpp/planning_component.h" -#include "moveit_ros/planning_scene_monitor/planning_scene_monitor.h" -#include "moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h" +#include "moveit_ros/moveit_cpp/moveit_cpp.hpp" +#include "moveit_ros/moveit_cpp/planning_component.hpp" +#include "moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp" +#include "moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp" PYBIND11_MODULE(planning, m) { diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 6a0be17731..75c1073985 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,185 +47,5 @@ /* Author: Ryan Luna */ #pragma once - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace moveit_ros_benchmarks -{ -/// A class that executes motion plan requests and aggregates data across multiple runs -/// Note: This class operates outside of MoveGroup and does NOT use PlanningRequestAdapters -class BenchmarkExecutor -{ -public: - /// Structure to hold information for a single run of a planner - typedef std::map PlannerRunData; - /// Structure to hold information for a single planner's benchmark data. - typedef std::vector PlannerBenchmarkData; - - /// Definition of a query-start benchmark event function. Invoked before a new query is benchmarked. - typedef std::function - QueryStartEventFunction; - - /// Definition of a query-end benchmark event function. Invoked after a query has finished benchmarking. - typedef std::function - QueryCompletionEventFunction; - - /// Definition of a planner-switch benchmark event function. Invoked before a planner starts any runs for a particular - /// query. - typedef std::function - PlannerStartEventFunction; - - /// Definition of a planner-switch benchmark event function. Invoked after a planner completes all runs for a - /// particular query. - typedef std::function - PlannerCompletionEventFunction; - - /// Definition of a pre-run benchmark event function. Invoked immediately before each planner calls solve(). - typedef std::function PreRunEventFunction; - - /// Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve(). - typedef std::function - PostRunEventFunction; - - BenchmarkExecutor(const rclcpp::Node::SharedPtr& node, - const std::string& robot_description_param = "robot_description"); - virtual ~BenchmarkExecutor(); - - // Initialize the benchmark executor by loading planning pipelines from the - // given set of classes - [[nodiscard]] bool initialize(const std::vector& plugin_classes); - - void addPreRunEvent(const PreRunEventFunction& func); - void addPostRunEvent(const PostRunEventFunction& func); - void addPlannerStartEvent(const PlannerStartEventFunction& func); - void addPlannerCompletionEvent(const PlannerCompletionEventFunction& func); - void addQueryStartEvent(const QueryStartEventFunction& func); - void addQueryCompletionEvent(const QueryCompletionEventFunction& func); - - virtual void clear(); - - virtual bool runBenchmarks(const BenchmarkOptions& options); - -protected: - struct BenchmarkRequest - { - std::string name; - moveit_msgs::msg::MotionPlanRequest request; - }; - - struct StartState - { - moveit_msgs::msg::RobotState state; - std::string name; - }; - - struct PathConstraints - { - std::vector constraints; - std::string name; - }; - - struct TrajectoryConstraints - { - moveit_msgs::msg::TrajectoryConstraints constraints; - std::string name; - }; - - virtual bool initializeBenchmarks(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, - std::vector& queries); - - /// Initialize benchmark query data from start states and constraints - virtual bool loadBenchmarkQueryData(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, - std::vector& start_states, - std::vector& path_constraints, - std::vector& goal_constraints, - std::vector& traj_constraints, - std::vector& queries); - - virtual void collectMetrics(PlannerRunData& metrics, - const planning_interface::MotionPlanDetailedResponse& motion_plan_response, bool solved, - double total_time); - - /// Compute the similarity of each (final) trajectory to all other (final) trajectories in the experiment and write - /// the results to planner_data metrics - void computeAveragePathSimilarities(PlannerBenchmarkData& planner_data, - const std::vector& responses, - const std::vector& solved); - - /// Helper function used by computeAveragePathSimilarities() for computing a heuristic distance metric between two - /// robot trajectories. This function aligns both trajectories in a greedy fashion and computes the mean waypoint - /// distance averaged over all aligned waypoints. Using a greedy approach is more efficient than dynamic time warping, - /// and seems to be sufficient for similar trajectories. - bool computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first, - const robot_trajectory::RobotTrajectory& traj_second, double& result_distance); - - virtual void writeOutput(const BenchmarkRequest& benchmark_request, const std::string& start_time, - double benchmark_duration, const BenchmarkOptions& options); - - void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector& offset); - - /// Check that the desired planning pipelines exist - bool pipelinesExist(const std::map>& planners); - - /// Load the planning scene with the given name from the warehouse - bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg); - - /// Load all states matching the given regular expression from the warehouse - bool loadStates(const std::string& regex, std::vector& start_states); - - /// Load all constraints matching the given regular expression from the warehouse - bool loadPathConstraints(const std::string& regex, std::vector& constraints); - - /// Load all trajectory constraints from the warehouse that match the given regular expression - bool loadTrajectoryConstraints(const std::string& regex, std::vector& constraints); - - /// Load all motion plan requests matching the given regular expression from the warehouse - bool loadQueries(const std::string& regex, const std::string& scene_name, std::vector& queries); - - /// Duplicate the given benchmark request for all combinations of start states and path constraints - void createRequestCombinations(const BenchmarkRequest& benchmark_request, const std::vector& start_states, - const std::vector& path_constraints, - std::vector& combos); - - /// Execute the given motion plan request on the set of planners for the set number of runs - void runBenchmark(moveit_msgs::msg::MotionPlanRequest request, const BenchmarkOptions& options); - - std::shared_ptr planning_scene_monitor_; - std::shared_ptr planning_scene_storage_; - std::shared_ptr planning_scene_world_storage_; - std::shared_ptr robot_state_storage_; - std::shared_ptr constraints_storage_; - std::shared_ptr trajectory_constraints_storage_; - - rclcpp::Node::SharedPtr node_; - warehouse_ros::DatabaseLoader db_loader_; - planning_scene::PlanningScenePtr planning_scene_; - std::shared_ptr moveit_cpp_; - - std::vector benchmark_data_; - - std::vector pre_event_functions_; - std::vector post_event_functions_; - std::vector planner_start_functions_; - std::vector planner_completion_functions_; - std::vector query_start_functions_; - std::vector query_end_functions_; -}; -} // namespace moveit_ros_benchmarks +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.hpp b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.hpp new file mode 100644 index 0000000000..e23c686e18 --- /dev/null +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.hpp @@ -0,0 +1,219 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 Rice University 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: Ryan Luna */ + +#pragma once + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace moveit_ros_benchmarks +{ +/// A class that executes motion plan requests and aggregates data across multiple runs +/// Note: This class operates outside of MoveGroup and does NOT use PlanningRequestAdapters +class BenchmarkExecutor +{ +public: + /// Structure to hold information for a single run of a planner + typedef std::map PlannerRunData; + /// Structure to hold information for a single planner's benchmark data. + typedef std::vector PlannerBenchmarkData; + + /// Definition of a query-start benchmark event function. Invoked before a new query is benchmarked. + typedef std::function + QueryStartEventFunction; + + /// Definition of a query-end benchmark event function. Invoked after a query has finished benchmarking. + typedef std::function + QueryCompletionEventFunction; + + /// Definition of a planner-switch benchmark event function. Invoked before a planner starts any runs for a particular + /// query. + typedef std::function + PlannerStartEventFunction; + + /// Definition of a planner-switch benchmark event function. Invoked after a planner completes all runs for a + /// particular query. + typedef std::function + PlannerCompletionEventFunction; + + /// Definition of a pre-run benchmark event function. Invoked immediately before each planner calls solve(). + typedef std::function PreRunEventFunction; + + /// Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve(). + typedef std::function + PostRunEventFunction; + + BenchmarkExecutor(const rclcpp::Node::SharedPtr& node, + const std::string& robot_description_param = "robot_description"); + virtual ~BenchmarkExecutor(); + + // Initialize the benchmark executor by loading planning pipelines from the + // given set of classes + [[nodiscard]] bool initialize(const std::vector& plugin_classes); + + void addPreRunEvent(const PreRunEventFunction& func); + void addPostRunEvent(const PostRunEventFunction& func); + void addPlannerStartEvent(const PlannerStartEventFunction& func); + void addPlannerCompletionEvent(const PlannerCompletionEventFunction& func); + void addQueryStartEvent(const QueryStartEventFunction& func); + void addQueryCompletionEvent(const QueryCompletionEventFunction& func); + + virtual void clear(); + + virtual bool runBenchmarks(const BenchmarkOptions& options); + +protected: + struct BenchmarkRequest + { + std::string name; + moveit_msgs::msg::MotionPlanRequest request; + }; + + struct StartState + { + moveit_msgs::msg::RobotState state; + std::string name; + }; + + struct PathConstraints + { + std::vector constraints; + std::string name; + }; + + struct TrajectoryConstraints + { + moveit_msgs::msg::TrajectoryConstraints constraints; + std::string name; + }; + + virtual bool initializeBenchmarks(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, + std::vector& queries); + + /// Initialize benchmark query data from start states and constraints + virtual bool loadBenchmarkQueryData(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, + std::vector& start_states, + std::vector& path_constraints, + std::vector& goal_constraints, + std::vector& traj_constraints, + std::vector& queries); + + virtual void collectMetrics(PlannerRunData& metrics, + const planning_interface::MotionPlanDetailedResponse& motion_plan_response, bool solved, + double total_time); + + /// Compute the similarity of each (final) trajectory to all other (final) trajectories in the experiment and write + /// the results to planner_data metrics + void computeAveragePathSimilarities(PlannerBenchmarkData& planner_data, + const std::vector& responses, + const std::vector& solved); + + /// Helper function used by computeAveragePathSimilarities() for computing a heuristic distance metric between two + /// robot trajectories. This function aligns both trajectories in a greedy fashion and computes the mean waypoint + /// distance averaged over all aligned waypoints. Using a greedy approach is more efficient than dynamic time warping, + /// and seems to be sufficient for similar trajectories. + bool computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first, + const robot_trajectory::RobotTrajectory& traj_second, double& result_distance); + + virtual void writeOutput(const BenchmarkRequest& benchmark_request, const std::string& start_time, + double benchmark_duration, const BenchmarkOptions& options); + + void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector& offset); + + /// Check that the desired planning pipelines exist + bool pipelinesExist(const std::map>& planners); + + /// Load the planning scene with the given name from the warehouse + bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg); + + /// Load all states matching the given regular expression from the warehouse + bool loadStates(const std::string& regex, std::vector& start_states); + + /// Load all constraints matching the given regular expression from the warehouse + bool loadPathConstraints(const std::string& regex, std::vector& constraints); + + /// Load all trajectory constraints from the warehouse that match the given regular expression + bool loadTrajectoryConstraints(const std::string& regex, std::vector& constraints); + + /// Load all motion plan requests matching the given regular expression from the warehouse + bool loadQueries(const std::string& regex, const std::string& scene_name, std::vector& queries); + + /// Duplicate the given benchmark request for all combinations of start states and path constraints + void createRequestCombinations(const BenchmarkRequest& benchmark_request, const std::vector& start_states, + const std::vector& path_constraints, + std::vector& combos); + + /// Execute the given motion plan request on the set of planners for the set number of runs + void runBenchmark(moveit_msgs::msg::MotionPlanRequest request, const BenchmarkOptions& options); + + std::shared_ptr planning_scene_monitor_; + std::shared_ptr planning_scene_storage_; + std::shared_ptr planning_scene_world_storage_; + std::shared_ptr robot_state_storage_; + std::shared_ptr constraints_storage_; + std::shared_ptr trajectory_constraints_storage_; + + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseLoader db_loader_; + planning_scene::PlanningScenePtr planning_scene_; + std::shared_ptr moveit_cpp_; + + std::vector benchmark_data_; + + std::vector pre_event_functions_; + std::vector post_event_functions_; + std::vector planner_start_functions_; + std::vector planner_completion_functions_; + std::vector query_start_functions_; + std::vector query_end_functions_; +}; +} // namespace moveit_ros_benchmarks diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h index b0461a90f4..63e4c52e48 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,92 +47,5 @@ /* Author: Ryan Luna */ #pragma once - -#include -#include -#include -#include -#include - -static constexpr int CARTESIAN_DOF = 6; - -namespace moveit_ros_benchmarks -{ -/// TODO(sjahr): Replace with generate_parameter_library config -/// \brief Options to configure a benchmark experiment. The configuration is provided via ROS2 parameters -/// \details Parameter configuration example: -/// benchmark_config: # Benchmark param namespace -/// warehouse: -/// host: # Host address for warehouse -/// port: # Port name for warehouse -/// scene_name: # Scene name to load for this experiment -/// parameters: -/// name: # Experiment name -/// runs: # Number of experiment runs -/// group: # Joint group name -/// timeout: # Experiment timeout -/// output_directory: # Output directory for results file -/// queries_regex: # Number of queries -/// start_states_regex: # Start states -/// goal_constraints_regex: # Goal constrains -/// path_constraints_regex -/// trajectory_constraints_regex -/// predefined_poses_group: # Group where the predefined poses are specified -/// predefined_poses: # List of named targets -/// planning_pipelines: -/// pipeline_names: # List of pipeline names to be loaded by moveit_cpp -/// pipelines: # List of pipeline names to be used by the benchmark tool -/// my_pipeline: # Example pipeline definition -/// name: # Pipeline name -/// planners: # List of planners of the pipeline to be tested -/// parallel_pipelines: # List of parallel planning pipelines to be tested -/// my_parallel_planning_pipeline: -/// pipelines: # List of parallel planning pipelines -/// planner_ids: # Ordered! list planner_ids used by the individual pipelines listed in 'pipeline' -struct BenchmarkOptions -{ - /** \brief Constructor */ - BenchmarkOptions(const rclcpp::Node::SharedPtr& node); - - /** \brief Get all planning pipeline names */ - void getPlanningPipelineNames(std::vector& planning_pipeline_names) const; - - /* \brief Get the frame id of the planning workspace */ - const std::string& getWorkspaceFrameID() const; - /* \brief Get the parameter set of the planning workspace */ - const moveit_msgs::msg::WorkspaceParameters& getWorkspaceParameters() const; - - bool readBenchmarkOptions(const rclcpp::Node::SharedPtr& node); - bool readPlannerConfigs(const rclcpp::Node::SharedPtr& node); - - void readWorkspaceParameters(const rclcpp::Node::SharedPtr& node); - void readGoalOffset(const rclcpp::Node::SharedPtr& node); - - /// Warehouse parameters - std::string hostname; // Host address for warehouse - int port; // Port name for warehouse - std::string scene_name; // Scene name to load for this experiment - - /// Benchmark parameters - int runs; // Number of experiment runs - double timeout; // Experiment timeout - std::string benchmark_name; // Experiment name - std::string group_name; // Joint group name - std::string output_directory; // Output directory for results file - std::string query_regex; // Regex for queries in database - std::string start_state_regex; // Regex for start_states in database - std::string goal_constraint_regex; // Regex for goal_constraints in database - std::string path_constraint_regex; // Regex for path_constraints in database - std::string trajectory_constraint_regex; // Regex for trajectory_constraint in database - std::vector predefined_poses; // List of named targets - std::string predefined_poses_group; // Group where the predefined poses are specified - std::vector goal_offsets = - std::vector(CARTESIAN_DOF); // Offset applied to goal constraints: x, y, z, roll, pitch, yaw - - /// planner configurations - std::map> planning_pipelines; - std::map>> parallel_planning_pipelines; - - moveit_msgs::msg::WorkspaceParameters workspace; -}; -} // namespace moveit_ros_benchmarks +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.hpp b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.hpp new file mode 100644 index 0000000000..b0461a90f4 --- /dev/null +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkOptions.hpp @@ -0,0 +1,126 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, Rice University + * 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 Rice University 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: Ryan Luna */ + +#pragma once + +#include +#include +#include +#include +#include + +static constexpr int CARTESIAN_DOF = 6; + +namespace moveit_ros_benchmarks +{ +/// TODO(sjahr): Replace with generate_parameter_library config +/// \brief Options to configure a benchmark experiment. The configuration is provided via ROS2 parameters +/// \details Parameter configuration example: +/// benchmark_config: # Benchmark param namespace +/// warehouse: +/// host: # Host address for warehouse +/// port: # Port name for warehouse +/// scene_name: # Scene name to load for this experiment +/// parameters: +/// name: # Experiment name +/// runs: # Number of experiment runs +/// group: # Joint group name +/// timeout: # Experiment timeout +/// output_directory: # Output directory for results file +/// queries_regex: # Number of queries +/// start_states_regex: # Start states +/// goal_constraints_regex: # Goal constrains +/// path_constraints_regex +/// trajectory_constraints_regex +/// predefined_poses_group: # Group where the predefined poses are specified +/// predefined_poses: # List of named targets +/// planning_pipelines: +/// pipeline_names: # List of pipeline names to be loaded by moveit_cpp +/// pipelines: # List of pipeline names to be used by the benchmark tool +/// my_pipeline: # Example pipeline definition +/// name: # Pipeline name +/// planners: # List of planners of the pipeline to be tested +/// parallel_pipelines: # List of parallel planning pipelines to be tested +/// my_parallel_planning_pipeline: +/// pipelines: # List of parallel planning pipelines +/// planner_ids: # Ordered! list planner_ids used by the individual pipelines listed in 'pipeline' +struct BenchmarkOptions +{ + /** \brief Constructor */ + BenchmarkOptions(const rclcpp::Node::SharedPtr& node); + + /** \brief Get all planning pipeline names */ + void getPlanningPipelineNames(std::vector& planning_pipeline_names) const; + + /* \brief Get the frame id of the planning workspace */ + const std::string& getWorkspaceFrameID() const; + /* \brief Get the parameter set of the planning workspace */ + const moveit_msgs::msg::WorkspaceParameters& getWorkspaceParameters() const; + + bool readBenchmarkOptions(const rclcpp::Node::SharedPtr& node); + bool readPlannerConfigs(const rclcpp::Node::SharedPtr& node); + + void readWorkspaceParameters(const rclcpp::Node::SharedPtr& node); + void readGoalOffset(const rclcpp::Node::SharedPtr& node); + + /// Warehouse parameters + std::string hostname; // Host address for warehouse + int port; // Port name for warehouse + std::string scene_name; // Scene name to load for this experiment + + /// Benchmark parameters + int runs; // Number of experiment runs + double timeout; // Experiment timeout + std::string benchmark_name; // Experiment name + std::string group_name; // Joint group name + std::string output_directory; // Output directory for results file + std::string query_regex; // Regex for queries in database + std::string start_state_regex; // Regex for start_states in database + std::string goal_constraint_regex; // Regex for goal_constraints in database + std::string path_constraint_regex; // Regex for path_constraints in database + std::string trajectory_constraint_regex; // Regex for trajectory_constraint in database + std::vector predefined_poses; // List of named targets + std::string predefined_poses_group; // Group where the predefined poses are specified + std::vector goal_offsets = + std::vector(CARTESIAN_DOF); // Offset applied to goal constraints: x, y, z, roll, pitch, yaw + + /// planner configurations + std::map> planning_pipelines; + std::map>> parallel_planning_pipelines; + + moveit_msgs::msg::WorkspaceParameters workspace; +}; +} // namespace moveit_ros_benchmarks diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index a53772b130..e43884259b 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -34,12 +34,12 @@ /* Author: Ryan Luna */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index c27f3c9cc1..3a788d8919 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -34,7 +34,7 @@ /* Author: Ryan Luna */ -#include +#include #include using moveit::getLogger; diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index e1a86af6d1..88eab1680d 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -36,8 +36,8 @@ #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h index 7573e83166..2bf0031f55 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,74 +49,5 @@ */ #pragma once - -#include -#include - -#include - -#include - -#include -#include -#include - -namespace moveit::hybrid_planning -{ -// Component node containing the global planner -class GlobalPlannerComponent -{ -public: - /** \brief Constructor */ - GlobalPlannerComponent(const rclcpp::NodeOptions& options); - - /** \brief Destructor */ - ~GlobalPlannerComponent() - { - // Join the thread used for long-running callbacks - if (long_callback_thread_.joinable()) - { - long_callback_thread_.join(); - } - } - - // This function is required to make this class a valid NodeClass - // see https://docs.ros2.org/foxy/api/rclcpp_components/register__node__macro_8hpp.html - // Skip linting due to unconventional function naming - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT - { - return node_->get_node_base_interface(); // NOLINT - } - -private: - std::shared_ptr node_; - - std::string planner_plugin_name_; - - // Global planner plugin loader - std::unique_ptr> global_planner_plugin_loader_; - - // Global planner instance - std::shared_ptr global_planner_instance_; - - // Global planning request action server - rclcpp_action::Server::SharedPtr global_planning_request_server_; - - // Global trajectory publisher - rclcpp::Publisher::SharedPtr global_trajectory_pub_; - - // Goal callback for global planning request action server - void globalPlanningRequestCallback( - const std::shared_ptr>& goal_handle); - - // Initialize planning scene monitor and load pipelines - bool initializeGlobalPlanner(); - - // This thread is used for long-running callbacks. It's a member so they do not go out of scope. - std::thread long_callback_thread_; - - // A unique callback group, to avoid mixing callbacks with other action servers - rclcpp::CallbackGroup::SharedPtr cb_group_; -}; - -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.hpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.hpp new file mode 100644 index 0000000000..586ea6122e --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.hpp @@ -0,0 +1,110 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: A global planner component node that is customizable through plugins. + */ + +#pragma once + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace moveit::hybrid_planning +{ +// Component node containing the global planner +class GlobalPlannerComponent +{ +public: + /** \brief Constructor */ + GlobalPlannerComponent(const rclcpp::NodeOptions& options); + + /** \brief Destructor */ + ~GlobalPlannerComponent() + { + // Join the thread used for long-running callbacks + if (long_callback_thread_.joinable()) + { + long_callback_thread_.join(); + } + } + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/foxy/api/rclcpp_components/register__node__macro_8hpp.html + // Skip linting due to unconventional function naming + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT + { + return node_->get_node_base_interface(); // NOLINT + } + +private: + std::shared_ptr node_; + + std::string planner_plugin_name_; + + // Global planner plugin loader + std::unique_ptr> global_planner_plugin_loader_; + + // Global planner instance + std::shared_ptr global_planner_instance_; + + // Global planning request action server + rclcpp_action::Server::SharedPtr global_planning_request_server_; + + // Global trajectory publisher + rclcpp::Publisher::SharedPtr global_trajectory_pub_; + + // Goal callback for global planning request action server + void globalPlanningRequestCallback( + const std::shared_ptr>& goal_handle); + + // Initialize planning scene monitor and load pipelines + bool initializeGlobalPlanner(); + + // This thread is used for long-running callbacks. It's a member so they do not go out of scope. + std::thread long_callback_thread_; + + // A unique callback group, to avoid mixing callbacks with other action servers + rclcpp::CallbackGroup::SharedPtr cb_group_; +}; + +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h index 7cfcafa96a..8fba00fd4c 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,47 +50,5 @@ */ #pragma once - -#include -#include - -#include - -#include -#include - -namespace moveit::hybrid_planning -{ -/** - * Class GlobalPlannerInterface - Base class for a global planner implementation - */ -class GlobalPlannerInterface -{ -public: - GlobalPlannerInterface() = default; - GlobalPlannerInterface(const GlobalPlannerInterface&) = default; - GlobalPlannerInterface(GlobalPlannerInterface&&) = default; - GlobalPlannerInterface& operator=(const GlobalPlannerInterface&) = default; - GlobalPlannerInterface& operator=(GlobalPlannerInterface&&) = default; - virtual ~GlobalPlannerInterface() = default; - /** - * Initialize global planner - * @return True if initialization was successful - */ - virtual bool initialize(const rclcpp::Node::SharedPtr& node) = 0; - - /** - * Solve global planning problem - * @param global_goal_handle Action goal handle to access the planning goal and publish feedback during planning - * @return Motion Plan that contains the solution for a given motion planning problem - */ - virtual moveit_msgs::msg::MotionPlanResponse plan( - const std::shared_ptr> global_goal_handle) = 0; - - /** - * Reset global planner plugin. This should never fail. - * @return True if reset was successful - */ - virtual bool reset() noexcept = 0; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.hpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.hpp new file mode 100644 index 0000000000..8adce4d776 --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_interface.hpp @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Defines an interface for a global motion planner plugin implementation used in the global planner + component node. + */ + +#pragma once + +#include +#include + +#include + +#include +#include + +namespace moveit::hybrid_planning +{ +/** + * Class GlobalPlannerInterface - Base class for a global planner implementation + */ +class GlobalPlannerInterface +{ +public: + GlobalPlannerInterface() = default; + GlobalPlannerInterface(const GlobalPlannerInterface&) = default; + GlobalPlannerInterface(GlobalPlannerInterface&&) = default; + GlobalPlannerInterface& operator=(const GlobalPlannerInterface&) = default; + GlobalPlannerInterface& operator=(GlobalPlannerInterface&&) = default; + virtual ~GlobalPlannerInterface() = default; + /** + * Initialize global planner + * @return True if initialization was successful + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node) = 0; + + /** + * Solve global planning problem + * @param global_goal_handle Action goal handle to access the planning goal and publish feedback during planning + * @return Motion Plan that contains the solution for a given motion planning problem + */ + virtual moveit_msgs::msg::MotionPlanResponse plan( + const std::shared_ptr> global_goal_handle) = 0; + + /** + * Reset global planner plugin. This should never fail. + * @return True if reset was successful + */ + virtual bool reset() noexcept = 0; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp index 33b86f8325..d52c2e1713 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h index a50afbef64..f19a59ba06 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,29 +48,5 @@ Description: Global planner plugin that utilizes MoveIt's planning pipeline accessed via the MoveItCpp API*/ #pragma once - -#include -#include - -// MoveitCpp -#include -#include - -namespace moveit::hybrid_planning -{ -class MoveItPlanningPipeline : public GlobalPlannerInterface -{ -public: - MoveItPlanningPipeline() = default; - ~MoveItPlanningPipeline() override = default; - bool initialize(const rclcpp::Node::SharedPtr& node) override; - bool reset() noexcept override; - moveit_msgs::msg::MotionPlanResponse - plan(const std::shared_ptr> global_goal_handle) - override; - -private: - rclcpp::Node::SharedPtr node_ptr_; - std::shared_ptr moveit_cpp_; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.hpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.hpp new file mode 100644 index 0000000000..b9eb54400a --- /dev/null +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/include/moveit/global_planner/moveit_planning_pipeline.hpp @@ -0,0 +1,64 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Global planner plugin that utilizes MoveIt's planning pipeline accessed via the MoveItCpp API*/ + +#pragma once + +#include +#include + +// MoveitCpp +#include +#include + +namespace moveit::hybrid_planning +{ +class MoveItPlanningPipeline : public GlobalPlannerInterface +{ +public: + MoveItPlanningPipeline() = default; + ~MoveItPlanningPipeline() override = default; + bool initialize(const rclcpp::Node::SharedPtr& node) override; + bool reset() noexcept override; + moveit_msgs::msg::MotionPlanResponse + plan(const std::shared_ptr> global_goal_handle) + override; + +private: + rclcpp::Node::SharedPtr node_ptr_; + std::shared_ptr moveit_cpp_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp index d93bfde272..6e8a3411c2 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include +#include +#include +#include namespace moveit::hybrid_planning { diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h index 3336370d4d..3e1cd37d04 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,41 +48,5 @@ Description: Defines events that could occur during hybrid planning */ #pragma once - -namespace moveit::hybrid_planning -{ -/** - * Enum class HybridPlanningEvent - This class defines the most basic events that are likely to occur during hybrid planning - */ -enum class HybridPlanningEvent -{ - // Occurs when the hybrid planning manager receives a planning request - HYBRID_PLANNING_REQUEST_RECEIVED, - // Result of the global planning action - GLOBAL_PLANNING_ACTION_SUCCESSFUL, - GLOBAL_PLANNING_ACTION_ABORTED, - GLOBAL_PLANNING_ACTION_CANCELED, - GLOBAL_PLANNING_ACTION_REJECTED, - // Indicates that the global planner found a solution (This solution is not necessarily the last or best solution) - GLOBAL_SOLUTION_AVAILABLE, - // Result of the local planning action - LOCAL_PLANNING_ACTION_SUCCESSFUL, - LOCAL_PLANNING_ACTION_ABORTED, - LOCAL_PLANNING_ACTION_CANCELED, - LOCAL_PLANNING_ACTION_REJECTED, - // Undefined event to allow empty reaction events to indicate failure - UNDEFINED -}; - -/** - * Enum class HybridPlanningAction - This class defines the basic actions that the HP manager can perform - */ -enum class HybridPlanningAction -{ - DO_NOTHING, - RETURN_HP_SUCCESS, - RETURN_HP_FAILURE, - SEND_GLOBAL_SOLVER_REQUEST, - SEND_LOCAL_SOLVER_REQUEST -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.hpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.hpp new file mode 100644 index 0000000000..3336370d4d --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Defines events that could occur during hybrid planning + */ +#pragma once + +namespace moveit::hybrid_planning +{ +/** + * Enum class HybridPlanningEvent - This class defines the most basic events that are likely to occur during hybrid planning + */ +enum class HybridPlanningEvent +{ + // Occurs when the hybrid planning manager receives a planning request + HYBRID_PLANNING_REQUEST_RECEIVED, + // Result of the global planning action + GLOBAL_PLANNING_ACTION_SUCCESSFUL, + GLOBAL_PLANNING_ACTION_ABORTED, + GLOBAL_PLANNING_ACTION_CANCELED, + GLOBAL_PLANNING_ACTION_REJECTED, + // Indicates that the global planner found a solution (This solution is not necessarily the last or best solution) + GLOBAL_SOLUTION_AVAILABLE, + // Result of the local planning action + LOCAL_PLANNING_ACTION_SUCCESSFUL, + LOCAL_PLANNING_ACTION_ABORTED, + LOCAL_PLANNING_ACTION_CANCELED, + LOCAL_PLANNING_ACTION_REJECTED, + // Undefined event to allow empty reaction events to indicate failure + UNDEFINED +}; + +/** + * Enum class HybridPlanningAction - This class defines the basic actions that the HP manager can perform + */ +enum class HybridPlanningAction +{ + DO_NOTHING, + RETURN_HP_SUCCESS, + RETURN_HP_FAILURE, + SEND_GLOBAL_SOLVER_REQUEST, + SEND_LOCAL_SOLVER_REQUEST +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h index 9944622202..05c9a5f22e 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,121 +49,5 @@ */ #pragma once - -#include -#include - -#include -#include -#include - -#include - -#include - -namespace moveit::hybrid_planning -{ -/** - * Class HybridPlanningManager - ROS 2 component node that implements the hybrid planning manager. - */ -class HybridPlanningManager -{ -public: - /** \brief Constructor */ - HybridPlanningManager(const rclcpp::NodeOptions& options); - - /** \brief Destructor */ - ~HybridPlanningManager() - { - // Join the thread used for long-running callbacks - if (long_callback_thread_.joinable()) - { - long_callback_thread_.join(); - } - } - - /** - * Load and initialized planner logic plugin and ROS 2 action and topic interfaces - * @return Initialization successful yes/no - */ - bool initialize(); - - // This function is required to make this class a valid NodeClass - // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT - { - return node_->get_node_base_interface(); // NOLINT - } - - /** - * Cancel any active action goals, including global and local planners - */ - void cancelHybridManagerGoals() noexcept; - - /** - * This handles execution of a hybrid planning goal in a new thread, to avoid blocking the executor. - * @param goal_handle The action server goal - */ - void executeHybridPlannerGoal( - std::shared_ptr> goal_handle); - - /** - * Send global planning request to global planner component - * @return Global planner successfully started yes/no - */ - bool sendGlobalPlannerAction(); - - /** - * Send local planning request to local planner component - * @return Local planner successfully started yes/no - */ - bool sendLocalPlannerAction(); - - /** - * Send back hybrid planning response - * @param success Indicates whether hybrid planning was successful - */ - void sendHybridPlanningResponse(bool success); - - /** - * @brief Process the action result and do an action call if necessary - * - * @param result Result to an event - */ - void processReactionResult(const ReactionResult& result); - -private: - std::shared_ptr node_; - - // Planner logic plugin loader - std::unique_ptr> planner_logic_plugin_loader_; - - // Planner logic instance to implement reactive behavior - std::shared_ptr planner_logic_instance_; - - // Flag that indicates hybrid planning has been canceled - std::atomic stop_hybrid_planning_; - - // Shared hybrid planning goal handle - std::shared_ptr> hybrid_planning_goal_handle_; - - // Frequently updated feedback for the hybrid planning action requester - std::shared_ptr hybrid_planning_progess_; - - // Planning request action clients - rclcpp_action::Client::SharedPtr local_planner_action_client_; - rclcpp_action::Client::SharedPtr global_planner_action_client_; - - // Hybrid planning request action server - rclcpp_action::Server::SharedPtr hybrid_planning_request_server_; - - // Global solution subscriber - rclcpp::Subscription::SharedPtr global_solution_sub_; - - // This thread is used for long-running callbacks. It's a member so they do not go out of scope. - std::thread long_callback_thread_; - - // A unique callback group, to avoid mixing callbacks with other action servers - rclcpp::CallbackGroup::SharedPtr cb_group_; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.hpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.hpp new file mode 100644 index 0000000000..0418959757 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.hpp @@ -0,0 +1,157 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: The hybrid planning manager component node that serves as the control unit of the whole architecture. + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include + +#include + +namespace moveit::hybrid_planning +{ +/** + * Class HybridPlanningManager - ROS 2 component node that implements the hybrid planning manager. + */ +class HybridPlanningManager +{ +public: + /** \brief Constructor */ + HybridPlanningManager(const rclcpp::NodeOptions& options); + + /** \brief Destructor */ + ~HybridPlanningManager() + { + // Join the thread used for long-running callbacks + if (long_callback_thread_.joinable()) + { + long_callback_thread_.join(); + } + } + + /** + * Load and initialized planner logic plugin and ROS 2 action and topic interfaces + * @return Initialization successful yes/no + */ + bool initialize(); + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT + { + return node_->get_node_base_interface(); // NOLINT + } + + /** + * Cancel any active action goals, including global and local planners + */ + void cancelHybridManagerGoals() noexcept; + + /** + * This handles execution of a hybrid planning goal in a new thread, to avoid blocking the executor. + * @param goal_handle The action server goal + */ + void executeHybridPlannerGoal( + std::shared_ptr> goal_handle); + + /** + * Send global planning request to global planner component + * @return Global planner successfully started yes/no + */ + bool sendGlobalPlannerAction(); + + /** + * Send local planning request to local planner component + * @return Local planner successfully started yes/no + */ + bool sendLocalPlannerAction(); + + /** + * Send back hybrid planning response + * @param success Indicates whether hybrid planning was successful + */ + void sendHybridPlanningResponse(bool success); + + /** + * @brief Process the action result and do an action call if necessary + * + * @param result Result to an event + */ + void processReactionResult(const ReactionResult& result); + +private: + std::shared_ptr node_; + + // Planner logic plugin loader + std::unique_ptr> planner_logic_plugin_loader_; + + // Planner logic instance to implement reactive behavior + std::shared_ptr planner_logic_instance_; + + // Flag that indicates hybrid planning has been canceled + std::atomic stop_hybrid_planning_; + + // Shared hybrid planning goal handle + std::shared_ptr> hybrid_planning_goal_handle_; + + // Frequently updated feedback for the hybrid planning action requester + std::shared_ptr hybrid_planning_progess_; + + // Planning request action clients + rclcpp_action::Client::SharedPtr local_planner_action_client_; + rclcpp_action::Client::SharedPtr global_planner_action_client_; + + // Hybrid planning request action server + rclcpp_action::Server::SharedPtr hybrid_planning_request_server_; + + // Global solution subscriber + rclcpp::Subscription::SharedPtr global_solution_sub_; + + // This thread is used for long-running callbacks. It's a member so they do not go out of scope. + std::thread long_callback_thread_; + + // A unique callback group, to avoid mixing callbacks with other action servers + rclcpp::CallbackGroup::SharedPtr cb_group_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h index 4b10cea752..bfd0f87a98 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,111 +49,5 @@ */ #pragma once - -#include -#include -#include -#include - -namespace moveit::hybrid_planning -{ -// Describes the outcome of a reaction to an event in the hybrid planning architecture -struct ReactionResult -{ - ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, const int error_code, - const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING) - : error_message(error_msg), error_code(error_code), action(action) - { - switch (planning_event) - { - case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: - event = "Hybrid planning request received"; - break; - case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: - event = "Global planning action successful"; - break; - case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: - event = "Global planning action aborted"; - break; - case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_CANCELED: - event = "Global planning action canceled"; - break; - case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: - event = "Global solution available"; - break; - case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: - event = "Local planning action successful"; - break; - case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: - event = "Local planning action aborted"; - break; - case HybridPlanningEvent::LOCAL_PLANNING_ACTION_CANCELED: - event = "Local planning action canceled"; - break; - case HybridPlanningEvent::UNDEFINED: - event = "Undefined event"; - break; - case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_REJECTED: - event = "Global planning action rejected"; - break; - case HybridPlanningEvent::LOCAL_PLANNING_ACTION_REJECTED: - event = "Local planning action rejected"; - break; - } - }; - ReactionResult(const std::string& event, const std::string& error_msg, const int error_code, - const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING) - : event(event), error_message(error_msg), error_code(error_code), action(action){}; - - // Event that triggered the reaction - std::string event; - - // Additional error description - std::string error_message; - - // Error code - moveit::core::MoveItErrorCode error_code; - - // Action to that needs to be performed by the HP manager - HybridPlanningAction action; -}; - -/** - * Class PlannerLogicInterface - Base class for a planner logic. The logic defines how to react to different events that - * occur during hybrid planning. Events can be triggered by callback functions of the hybrid planning manager's ROS 2 - * interfaces or timers. They are encoded either inside an enum class or as a string to easily include custom events. - */ -class PlannerLogicInterface -{ -public: - PlannerLogicInterface() = default; - PlannerLogicInterface(const PlannerLogicInterface&) = default; - PlannerLogicInterface(PlannerLogicInterface&&) = default; - PlannerLogicInterface& operator=(const PlannerLogicInterface&) = default; - PlannerLogicInterface& operator=(PlannerLogicInterface&&) = default; - virtual ~PlannerLogicInterface() = default; - - /** - * Initialize the planner logic - * @return true if initialization was successful - */ - virtual bool initialize() - { - return true; - }; - - /** - * React to event defined in HybridPlanningEvent enum - * @param event Basic hybrid planning event - * @return Reaction result that summarizes the outcome of the reaction - */ - virtual ReactionResult react(const HybridPlanningEvent& event) = 0; - - /** - * React to custom event - * @param event Encoded as string - * @return Reaction result that summarizes the outcome of the reaction - */ - virtual ReactionResult react(const std::string& event) = 0; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.hpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.hpp new file mode 100644 index 0000000000..f2710c74cf --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.hpp @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Defines an interface for a planner logic plugin for the hybrid planning manager component node. + */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit::hybrid_planning +{ +// Describes the outcome of a reaction to an event in the hybrid planning architecture +struct ReactionResult +{ + ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, const int error_code, + const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING) + : error_message(error_msg), error_code(error_code), action(action) + { + switch (planning_event) + { + case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: + event = "Hybrid planning request received"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: + event = "Global planning action successful"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: + event = "Global planning action aborted"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_CANCELED: + event = "Global planning action canceled"; + break; + case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: + event = "Global solution available"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: + event = "Local planning action successful"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: + event = "Local planning action aborted"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_CANCELED: + event = "Local planning action canceled"; + break; + case HybridPlanningEvent::UNDEFINED: + event = "Undefined event"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_REJECTED: + event = "Global planning action rejected"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_REJECTED: + event = "Local planning action rejected"; + break; + } + }; + ReactionResult(const std::string& event, const std::string& error_msg, const int error_code, + const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING) + : event(event), error_message(error_msg), error_code(error_code), action(action){}; + + // Event that triggered the reaction + std::string event; + + // Additional error description + std::string error_message; + + // Error code + moveit::core::MoveItErrorCode error_code; + + // Action to that needs to be performed by the HP manager + HybridPlanningAction action; +}; + +/** + * Class PlannerLogicInterface - Base class for a planner logic. The logic defines how to react to different events that + * occur during hybrid planning. Events can be triggered by callback functions of the hybrid planning manager's ROS 2 + * interfaces or timers. They are encoded either inside an enum class or as a string to easily include custom events. + */ +class PlannerLogicInterface +{ +public: + PlannerLogicInterface() = default; + PlannerLogicInterface(const PlannerLogicInterface&) = default; + PlannerLogicInterface(PlannerLogicInterface&&) = default; + PlannerLogicInterface& operator=(const PlannerLogicInterface&) = default; + PlannerLogicInterface& operator=(PlannerLogicInterface&&) = default; + virtual ~PlannerLogicInterface() = default; + + /** + * Initialize the planner logic + * @return true if initialization was successful + */ + virtual bool initialize() + { + return true; + }; + + /** + * React to event defined in HybridPlanningEvent enum + * @param event Basic hybrid planning event + * @return Reaction result that summarizes the outcome of the reaction + */ + virtual ReactionResult react(const HybridPlanningEvent& event) = 0; + + /** + * React to custom event + * @param event Encoded as string + * @return Reaction result that summarizes the outcome of the reaction + */ + virtual ReactionResult react(const std::string& event) = 0; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index b069b9a19c..ec1e3a2f95 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include #include #include diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h index 7fd25903a8..6ba28008e2 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,16 +50,6 @@ invalidated global trajectory. */ -#include - -namespace moveit::hybrid_planning -{ -class ReplanInvalidatedTrajectory : public SinglePlanExecution // Inherit from SinglePlanExecution because we just want - // alter the reaction to local planner events. -{ -public: - ReplanInvalidatedTrajectory() = default; - ~ReplanInvalidatedTrajectory() override = default; - ReactionResult react(const std::string& event) override; -}; -} // namespace moveit::hybrid_planning +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.hpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.hpp new file mode 100644 index 0000000000..cffb19c8ee --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.hpp @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Simple hybrid planning logic that runs the global planner once and starts executing the global solution + with the local planner. In case the local planner detects a collision the global planner is rerun to update the + invalidated global trajectory. + */ + +#pragma once + +#include + +namespace moveit::hybrid_planning +{ +class ReplanInvalidatedTrajectory : public SinglePlanExecution // Inherit from SinglePlanExecution because we just want + // alter the reaction to local planner events. +{ +public: + ReplanInvalidatedTrajectory() = default; + ~ReplanInvalidatedTrajectory() override = default; + ReactionResult react(const std::string& event) override; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h index 3378db79f0..66bef5d62d 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,20 +49,6 @@ with the local planner. */ -#include -#include - -namespace moveit::hybrid_planning -{ -class SinglePlanExecution : public PlannerLogicInterface -{ -public: - SinglePlanExecution() = default; - ~SinglePlanExecution() override = default; - ReactionResult react(const HybridPlanningEvent& event) override; - ReactionResult react(const std::string& event) override; - -private: - bool local_planner_started_ = false; -}; -} // namespace moveit::hybrid_planning +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.hpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.hpp new file mode 100644 index 0000000000..b9671e6160 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.hpp @@ -0,0 +1,58 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: This planner logic plugin runs the global planner once and starts executing the global solution + with the local planner. + */ + +#pragma once + +#include +#include + +namespace moveit::hybrid_planning +{ +class SinglePlanExecution : public PlannerLogicInterface +{ +public: + SinglePlanExecution() = default; + ~SinglePlanExecution() override = default; + ReactionResult react(const HybridPlanningEvent& event) override; + ReactionResult react(const std::string& event) override; + +private: + bool local_planner_started_ = false; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp index 6af466bc1a..ab7d616b43 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include namespace moveit::hybrid_planning { diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp index 5b1170ad24..4de4375f02 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include namespace moveit::hybrid_planning { diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h index d63ddda28a..2a6b43508c 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -39,35 +51,5 @@ */ #pragma once - -#include -#include - -namespace moveit::hybrid_planning -{ -class ForwardTrajectory : public LocalConstraintSolverInterface -{ -public: - ForwardTrajectory() = default; - ~ForwardTrajectory() override = default; - bool initialize(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& /* unused */) override; - bool reset() override; - - moveit_msgs::action::LocalPlanner::Feedback - solve(const robot_trajectory::RobotTrajectory& local_trajectory, - const std::shared_ptr /* unused */, - trajectory_msgs::msg::JointTrajectory& local_solution) override; - -private: - rclcpp::Node::SharedPtr node_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - bool path_invalidation_event_send_; // Send path invalidation event only once - bool stop_before_collision_; - - // Detect when the local planner gets stuck - size_t num_iterations_stuck_; - moveit::core::RobotStatePtr prev_waypoint_target_; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.hpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.hpp new file mode 100644 index 0000000000..d72c363f5e --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.hpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Simple local solver plugin that forwards the next waypoint of the sampled local trajectory. + The local solver stops for two conditions: invalid waypoint (likely due to collision) or if it has been stuck for + several iterations. + */ + +#pragma once + +#include +#include + +namespace moveit::hybrid_planning +{ +class ForwardTrajectory : public LocalConstraintSolverInterface +{ +public: + ForwardTrajectory() = default; + ~ForwardTrajectory() override = default; + bool initialize(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& /* unused */) override; + bool reset() override; + + moveit_msgs::action::LocalPlanner::Feedback + solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr /* unused */, + trajectory_msgs::msg::JointTrajectory& local_solution) override; + +private: + rclcpp::Node::SharedPtr node_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + bool path_invalidation_event_send_; // Send path invalidation event only once + bool stop_before_collision_; + + // Detect when the local planner gets stuck + size_t num_iterations_stuck_; + moveit::core::RobotStatePtr prev_waypoint_target_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index 13086091bc..825129a0d8 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -32,10 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include -#include +#include +#include +#include +#include namespace { diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.h index aa732a4ba5..6d3003c88f 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,32 +50,5 @@ */ #pragma once - -#include -#include -#include - -namespace moveit::hybrid_planning -{ -/** - * \brief Expected feedback types - */ -enum LocalFeedbackEnum -{ - COLLISION_AHEAD = 1, - LOCAL_PLANNER_STUCK = 2 -}; - -[[nodiscard]] constexpr std::string_view toString(const LocalFeedbackEnum& code) -{ - switch (code) - { - case COLLISION_AHEAD: - return "Collision ahead"; - case LOCAL_PLANNER_STUCK: - return "Local planner is stuck"; - default: - __builtin_unreachable(); - } -} -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.hpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.hpp new file mode 100644 index 0000000000..aa732a4ba5 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/feedback_types.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, PickNik 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 PickNik Inc. 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: Andy Zelenak + Description: Define the expected local planner feedback types (usually equivalent to failure + modes). + */ + +#pragma once + +#include +#include +#include + +namespace moveit::hybrid_planning +{ +/** + * \brief Expected feedback types + */ +enum LocalFeedbackEnum +{ + COLLISION_AHEAD = 1, + LOCAL_PLANNER_STUCK = 2 +}; + +[[nodiscard]] constexpr std::string_view toString(const LocalFeedbackEnum& code) +{ + switch (code) + { + case COLLISION_AHEAD: + return "Collision ahead"; + case LOCAL_PLANNER_STUCK: + return "Local planner is stuck"; + default: + __builtin_unreachable(); + } +} +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h index c3390be622..ddb293494a 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,58 +49,5 @@ */ #pragma once - -#include -#include - -#include -#include - -#include -#include - -#include - -#include - -namespace moveit::hybrid_planning -{ -/** - * Class LocalConstraintSolverInterface - Base class for a local constrain solver. - */ -class LocalConstraintSolverInterface -{ -public: - LocalConstraintSolverInterface() = default; - LocalConstraintSolverInterface(const LocalConstraintSolverInterface&) = default; - LocalConstraintSolverInterface(LocalConstraintSolverInterface&&) = default; - LocalConstraintSolverInterface& operator=(const LocalConstraintSolverInterface&) = default; - LocalConstraintSolverInterface& operator=(LocalConstraintSolverInterface&&) = default; - virtual ~LocalConstraintSolverInterface() = default; - /** - * Initialize local constraint solver - * @return True if initialization was successful - */ - virtual bool initialize(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& group_name) = 0; - - /** - * Solve local planning problem for the current iteration - * @param local_trajectory The local trajectory to pursue - * @param local_goal Local goal constraints - * @param local_solution solution plan in joint space - * @return Feedback event from the current solver call i.e. "Collision detected" - */ - virtual moveit_msgs::action::LocalPlanner::Feedback - solve(const robot_trajectory::RobotTrajectory& local_trajectory, - const std::shared_ptr local_goal, - trajectory_msgs::msg::JointTrajectory& local_solution) = 0; - - /** - * Reset local constraint solver to some user-defined initial state - * @return True if reset was successful - */ - virtual bool reset() = 0; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.hpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.hpp new file mode 100644 index 0000000000..31b8354eff --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.hpp @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Defines an interface for a local constraint solver plugin implementation for the local planner component node. + */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +namespace moveit::hybrid_planning +{ +/** + * Class LocalConstraintSolverInterface - Base class for a local constrain solver. + */ +class LocalConstraintSolverInterface +{ +public: + LocalConstraintSolverInterface() = default; + LocalConstraintSolverInterface(const LocalConstraintSolverInterface&) = default; + LocalConstraintSolverInterface(LocalConstraintSolverInterface&&) = default; + LocalConstraintSolverInterface& operator=(const LocalConstraintSolverInterface&) = default; + LocalConstraintSolverInterface& operator=(LocalConstraintSolverInterface&&) = default; + virtual ~LocalConstraintSolverInterface() = default; + /** + * Initialize local constraint solver + * @return True if initialization was successful + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& group_name) = 0; + + /** + * Solve local planning problem for the current iteration + * @param local_trajectory The local trajectory to pursue + * @param local_goal Local goal constraints + * @param local_solution solution plan in joint space + * @return Feedback event from the current solver call i.e. "Collision detected" + */ + virtual moveit_msgs::action::LocalPlanner::Feedback + solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr local_goal, + trajectory_msgs::msg::JointTrajectory& local_solution) = 0; + + /** + * Reset local constraint solver to some user-defined initial state + * @return True if reset was successful + */ + virtual bool reset() = 0; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h index 1ce83d7efc..f570897c4a 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,140 +50,5 @@ */ #pragma once - -#include -#include - -#include - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -// Forward declaration of parameter class allows users to implement custom parameters -namespace local_planner_parameters -{ -MOVEIT_STRUCT_FORWARD(Params); -} -namespace moveit::hybrid_planning -{ -/// Internal local planner states -/// TODO(sjahr) Use lifecycle node? -enum class LocalPlannerState : int8_t -{ - ABORT = -1, - ERROR = 0, - UNCONFIGURED = 1, - AWAIT_GLOBAL_TRAJECTORY = 2, - LOCAL_PLANNING_ACTIVE = 3 -}; - -/** - * Class LocalPlannerComponent - ROS 2 component node that implements a local planner. - */ -class LocalPlannerComponent -{ -public: - /** \brief Constructor */ - LocalPlannerComponent(const rclcpp::NodeOptions& options); - - /** \brief Destructor */ - ~LocalPlannerComponent() - { - // Join the thread used for long-running callbacks - if (long_callback_thread_.joinable()) - { - long_callback_thread_.join(); - } - } - - /** - * Initialize and start planning scene monitor to listen to the planning scene topic. - * Load trajectory_operator and constraint solver plugin. - * Initialize ROS 2 interfaces - * @return true if scene monitor and plugins are successfully initialized - */ - bool initialize(); - - /** - * Handle the planners current job based on the internal state each iteration when the planner is started. - */ - void executeIteration(); - - // This function is required to make this class a valid NodeClass - // see https://docs.ros2.org/foxy/api/rclcpp_components/register__node__macro_8hpp.html - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT - { - return node_->get_node_base_interface(); // NOLINT - } - -private: - /** \brief Reset internal data members including state_ = LocalPlannerState::AWAIT_GLOBAL_TRAJECTORY */ - void reset(); - - std::shared_ptr node_; - - // Planner configuration - std::shared_ptr config_; - - // Current planner state. Must be thread-safe - std::atomic state_; - - // Timer to periodically call executeIteration() - rclcpp::TimerBase::SharedPtr timer_; - - // Latest action goal handle - std::shared_ptr> local_planning_goal_handle_; - - // Local planner feedback - std::shared_ptr local_planner_feedback_; - - // Planning scene monitor to get the current planning scene - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // TF - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - // Global solution listener - rclcpp::Subscription::SharedPtr global_solution_subscriber_; - - // Local planning request action server - rclcpp_action::Server::SharedPtr local_planning_request_server_; - - // Local solution publisher - rclcpp::Publisher::SharedPtr local_trajectory_publisher_; - rclcpp::Publisher::SharedPtr local_solution_publisher_; - - // Local constraint solver plugin loader - std::unique_ptr> local_constraint_solver_plugin_loader_; - - // Local constrain solver instance to compute a local solution each iteration - std::shared_ptr local_constraint_solver_instance_; - - // Trajectory operator plugin - std::unique_ptr> trajectory_operator_loader_; - - // Trajectory_operator instance handle trajectory matching and blending - std::shared_ptr trajectory_operator_instance_; - - // This thread is used for long-running callbacks. It's a member so they do not go out of scope. - std::thread long_callback_thread_; - - // A unique callback group, to avoid mixing callbacks with other action servers - rclcpp::CallbackGroup::SharedPtr cb_group_; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp new file mode 100644 index 0000000000..164cd231a7 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp @@ -0,0 +1,177 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: A local planner component node that is customizable through plugins that implement the local planning + problem solver algorithm and the trajectory matching and blending. + */ + +#pragma once + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +// Forward declaration of parameter class allows users to implement custom parameters +namespace local_planner_parameters +{ +MOVEIT_STRUCT_FORWARD(Params); +} +namespace moveit::hybrid_planning +{ +/// Internal local planner states +/// TODO(sjahr) Use lifecycle node? +enum class LocalPlannerState : int8_t +{ + ABORT = -1, + ERROR = 0, + UNCONFIGURED = 1, + AWAIT_GLOBAL_TRAJECTORY = 2, + LOCAL_PLANNING_ACTIVE = 3 +}; + +/** + * Class LocalPlannerComponent - ROS 2 component node that implements a local planner. + */ +class LocalPlannerComponent +{ +public: + /** \brief Constructor */ + LocalPlannerComponent(const rclcpp::NodeOptions& options); + + /** \brief Destructor */ + ~LocalPlannerComponent() + { + // Join the thread used for long-running callbacks + if (long_callback_thread_.joinable()) + { + long_callback_thread_.join(); + } + } + + /** + * Initialize and start planning scene monitor to listen to the planning scene topic. + * Load trajectory_operator and constraint solver plugin. + * Initialize ROS 2 interfaces + * @return true if scene monitor and plugins are successfully initialized + */ + bool initialize(); + + /** + * Handle the planners current job based on the internal state each iteration when the planner is started. + */ + void executeIteration(); + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/foxy/api/rclcpp_components/register__node__macro_8hpp.html + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT + { + return node_->get_node_base_interface(); // NOLINT + } + +private: + /** \brief Reset internal data members including state_ = LocalPlannerState::AWAIT_GLOBAL_TRAJECTORY */ + void reset(); + + std::shared_ptr node_; + + // Planner configuration + std::shared_ptr config_; + + // Current planner state. Must be thread-safe + std::atomic state_; + + // Timer to periodically call executeIteration() + rclcpp::TimerBase::SharedPtr timer_; + + // Latest action goal handle + std::shared_ptr> local_planning_goal_handle_; + + // Local planner feedback + std::shared_ptr local_planner_feedback_; + + // Planning scene monitor to get the current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // TF + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Global solution listener + rclcpp::Subscription::SharedPtr global_solution_subscriber_; + + // Local planning request action server + rclcpp_action::Server::SharedPtr local_planning_request_server_; + + // Local solution publisher + rclcpp::Publisher::SharedPtr local_trajectory_publisher_; + rclcpp::Publisher::SharedPtr local_solution_publisher_; + + // Local constraint solver plugin loader + std::unique_ptr> local_constraint_solver_plugin_loader_; + + // Local constrain solver instance to compute a local solution each iteration + std::shared_ptr local_constraint_solver_instance_; + + // Trajectory operator plugin + std::unique_ptr> trajectory_operator_loader_; + + // Trajectory_operator instance handle trajectory matching and blending + std::shared_ptr trajectory_operator_instance_; + + // This thread is used for long-running callbacks. It's a member so they do not go out of scope. + std::thread long_callback_thread_; + + // A unique callback group, to avoid mixing callbacks with other action servers + rclcpp::CallbackGroup::SharedPtr cb_group_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h index 7c3fc6b124..e1a0441812 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,79 +49,5 @@ */ #pragma once - -#include -#include - -#include -#include - -#include - -#include -#include - -namespace moveit::hybrid_planning -{ -/** - * Class TrajectoryOperatorInterface - Base class for a trajectory operator. The operator's task is manage the local - * planner's global reference trajectory. This includes trajectory matching based on the current state to identify the - * current planning problem and blending of new global trajectory updates into the currently processed reference - * trajectory. - */ -class TrajectoryOperatorInterface -{ -public: - TrajectoryOperatorInterface() = default; - TrajectoryOperatorInterface(const TrajectoryOperatorInterface&) = default; - TrajectoryOperatorInterface(TrajectoryOperatorInterface&&) = default; - TrajectoryOperatorInterface& operator=(const TrajectoryOperatorInterface&) = default; - TrajectoryOperatorInterface& operator=(TrajectoryOperatorInterface&&) = default; - virtual ~TrajectoryOperatorInterface() = default; - /** - * Initialize trajectory operator - * @param node Node handle to access parameters - * @param robot_model Robot model - * @param group_name Name of the joint group the trajectory uses - * @return True if initialization was successful - */ - virtual bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, - const std::string& group_name) = 0; - - /** - * Add a new reference trajectory segment to the vector of global trajectory segments to process - * @param new_trajectory New reference trajectory segment to add - * @return True if segment was successfully added - */ - virtual moveit_msgs::action::LocalPlanner::Feedback - addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) = 0; - - /** - * Return the current local constraints based on the newest robot state - * @param current_state Current RobotState - * @return Current local constraints that define the local planning goal - */ - virtual moveit_msgs::action::LocalPlanner::Feedback - getLocalTrajectory(const moveit::core::RobotState& current_state, - robot_trajectory::RobotTrajectory& local_trajectory) = 0; - - /** - * Return the processing status of the reference trajectory's execution based on a user defined - * metric. - * @param current_state Current RobotState - * @return A value between 0.0 (start) to 1.0 (completion). - */ - virtual double getTrajectoryProgress(const moveit::core::RobotState& current_state) = 0; - - /** - * Reset trajectory operator to some user-defined initial state - * @return True if reset was successful - */ - virtual bool reset() = 0; - -protected: - // Reference trajectory to be processed - robot_trajectory::RobotTrajectoryPtr reference_trajectory_; - std::string group_; -}; -} // namespace moveit::hybrid_planning +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.hpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.hpp new file mode 100644 index 0000000000..40f5ccf6e0 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.hpp @@ -0,0 +1,115 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Defines an interface for a trajectory operator plugin implementation for the local planner component node. + */ + +#pragma once + +#include +#include + +#include +#include + +#include + +#include +#include + +namespace moveit::hybrid_planning +{ +/** + * Class TrajectoryOperatorInterface - Base class for a trajectory operator. The operator's task is manage the local + * planner's global reference trajectory. This includes trajectory matching based on the current state to identify the + * current planning problem and blending of new global trajectory updates into the currently processed reference + * trajectory. + */ +class TrajectoryOperatorInterface +{ +public: + TrajectoryOperatorInterface() = default; + TrajectoryOperatorInterface(const TrajectoryOperatorInterface&) = default; + TrajectoryOperatorInterface(TrajectoryOperatorInterface&&) = default; + TrajectoryOperatorInterface& operator=(const TrajectoryOperatorInterface&) = default; + TrajectoryOperatorInterface& operator=(TrajectoryOperatorInterface&&) = default; + virtual ~TrajectoryOperatorInterface() = default; + /** + * Initialize trajectory operator + * @param node Node handle to access parameters + * @param robot_model Robot model + * @param group_name Name of the joint group the trajectory uses + * @return True if initialization was successful + */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const std::string& group_name) = 0; + + /** + * Add a new reference trajectory segment to the vector of global trajectory segments to process + * @param new_trajectory New reference trajectory segment to add + * @return True if segment was successfully added + */ + virtual moveit_msgs::action::LocalPlanner::Feedback + addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) = 0; + + /** + * Return the current local constraints based on the newest robot state + * @param current_state Current RobotState + * @return Current local constraints that define the local planning goal + */ + virtual moveit_msgs::action::LocalPlanner::Feedback + getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) = 0; + + /** + * Return the processing status of the reference trajectory's execution based on a user defined + * metric. + * @param current_state Current RobotState + * @return A value between 0.0 (start) to 1.0 (completion). + */ + virtual double getTrajectoryProgress(const moveit::core::RobotState& current_state) = 0; + + /** + * Reset trajectory operator to some user-defined initial state + * @return True if reset was successful + */ + virtual bool reset() = 0; + +protected: + // Reference trajectory to be processed + robot_trajectory::RobotTrajectoryPtr reference_trajectory_; + std::string group_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index ab4bb28c66..f9914f2ee9 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -32,13 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include -#include -#include +#include +#include -#include +#include #include diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h index ed73e0221d..9de076fd83 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,32 +50,6 @@ is updated to the next global trajectory waypoint. Global trajectory updates simply replace the reference trajectory. */ -#include -#include - -namespace moveit::hybrid_planning -{ -class SimpleSampler : public TrajectoryOperatorInterface -{ -public: - SimpleSampler() = default; - ~SimpleSampler() override = default; - - bool initialize([[maybe_unused]] const rclcpp::Node::SharedPtr& node, - const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name) override; - moveit_msgs::action::LocalPlanner::Feedback - addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) override; - moveit_msgs::action::LocalPlanner::Feedback - getLocalTrajectory(const moveit::core::RobotState& current_state, - robot_trajectory::RobotTrajectory& local_trajectory) override; - double getTrajectoryProgress([[maybe_unused]] const moveit::core::RobotState& current_state) override; - bool reset() override; - -private: - std::size_t - next_waypoint_index_; // Indicates which reference trajectory waypoint is the current local goal constrained - moveit_msgs::action::LocalPlanner::Feedback feedback_; // Empty feedback - trajectory_processing::TimeOptimalTrajectoryGeneration time_parametrization_; - const moveit::core::JointModelGroup* joint_group_; -}; -} // namespace moveit::hybrid_planning +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.hpp b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.hpp new file mode 100644 index 0000000000..4581a62ce9 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.hpp @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: Simple trajectory operator that samples the next global trajectory waypoint as local goal constraint + based on the current robot state. When the waypoint is reached the index that marks the current local goal constraint + is updated to the next global trajectory waypoint. Global trajectory updates simply replace the reference trajectory. + */ + +#pragma once + +#include +#include + +namespace moveit::hybrid_planning +{ +class SimpleSampler : public TrajectoryOperatorInterface +{ +public: + SimpleSampler() = default; + ~SimpleSampler() override = default; + + bool initialize([[maybe_unused]] const rclcpp::Node::SharedPtr& node, + const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name) override; + moveit_msgs::action::LocalPlanner::Feedback + addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) override; + moveit_msgs::action::LocalPlanner::Feedback + getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) override; + double getTrajectoryProgress([[maybe_unused]] const moveit::core::RobotState& current_state) override; + bool reset() override; + +private: + std::size_t + next_waypoint_index_; // Indicates which reference trajectory waypoint is the current local goal constrained + moveit_msgs::action::LocalPlanner::Feedback feedback_; // Empty feedback + trajectory_processing::TimeOptimalTrajectoryGeneration time_parametrization_; + const moveit::core::JointModelGroup* joint_group_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp index ae3038ace1..c1ed50ab02 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include -#include +#include namespace moveit::hybrid_planning { diff --git a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp index 9a6beb355e..97399f272a 100644 --- a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp +++ b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp @@ -37,12 +37,12 @@ */ #include -#include -#include -#include +#include +#include +#include #include #include -#include +#include #include #include diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h index 53af52c223..bbc1d20b99 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,37 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -namespace move_group -{ -static const std::string PLANNER_SERVICE_NAME = - "plan_kinematic_path"; // name of the advertised service (within the ~ namespace) -static const std::string EXECUTE_ACTION_NAME = "execute_trajectory"; // name of 'execute' action -static const std::string QUERY_PLANNERS_SERVICE_NAME = - "query_planner_interface"; // name of the advertised query planners service -static const std::string GET_PLANNER_PARAMS_SERVICE_NAME = - "get_planner_params"; // service name to retrieve planner parameters -static const std::string SET_PLANNER_PARAMS_SERVICE_NAME = - "set_planner_params"; // service name to set planner parameters -static const std::string MOVE_ACTION = "move_action"; // name of 'move' action -static const std::string IK_SERVICE_NAME = "compute_ik"; // name of ik service -static const std::string FK_SERVICE_NAME = "compute_fk"; // name of fk service -static const std::string STATE_VALIDITY_SERVICE_NAME = - "check_state_validity"; // name of the service that validates states -static const std::string CARTESIAN_PATH_SERVICE_NAME = - "compute_cartesian_path"; // name of the service that computes cartesian paths -static const std::string GET_PLANNING_SCENE_SERVICE_NAME = - "get_planning_scene"; // name of the service that can be used to query the planning scene -static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = - "apply_planning_scene"; // name of the service that applies a given planning scene -static const std::string CLEAR_OCTOMAP_SERVICE_NAME = - "clear_octomap"; // name of the service that can be used to clear the octomap -static const std::string GET_URDF_SERVICE_NAME = - "get_urdf"; // name of the service that can be used to request the urdf of a planning group -static const std::string SAVE_GEOMETRY_TO_FILE_SERVICE_NAME = - "save_geometry_to_file"; // name of the service that can be used to save CollisionObjects in a PlanningScene to a file -static const std::string LOAD_GEOMETRY_FROM_FILE_SERVICE_NAME = - "load_geometry_from_file"; // name of the service that can be used to load CollisionObjects to a PlanningScene from a file -} // namespace move_group +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp b/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp new file mode 100644 index 0000000000..53af52c223 --- /dev/null +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp @@ -0,0 +1,71 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +namespace move_group +{ +static const std::string PLANNER_SERVICE_NAME = + "plan_kinematic_path"; // name of the advertised service (within the ~ namespace) +static const std::string EXECUTE_ACTION_NAME = "execute_trajectory"; // name of 'execute' action +static const std::string QUERY_PLANNERS_SERVICE_NAME = + "query_planner_interface"; // name of the advertised query planners service +static const std::string GET_PLANNER_PARAMS_SERVICE_NAME = + "get_planner_params"; // service name to retrieve planner parameters +static const std::string SET_PLANNER_PARAMS_SERVICE_NAME = + "set_planner_params"; // service name to set planner parameters +static const std::string MOVE_ACTION = "move_action"; // name of 'move' action +static const std::string IK_SERVICE_NAME = "compute_ik"; // name of ik service +static const std::string FK_SERVICE_NAME = "compute_fk"; // name of fk service +static const std::string STATE_VALIDITY_SERVICE_NAME = + "check_state_validity"; // name of the service that validates states +static const std::string CARTESIAN_PATH_SERVICE_NAME = + "compute_cartesian_path"; // name of the service that computes cartesian paths +static const std::string GET_PLANNING_SCENE_SERVICE_NAME = + "get_planning_scene"; // name of the service that can be used to query the planning scene +static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = + "apply_planning_scene"; // name of the service that applies a given planning scene +static const std::string CLEAR_OCTOMAP_SERVICE_NAME = + "clear_octomap"; // name of the service that can be used to clear the octomap +static const std::string GET_URDF_SERVICE_NAME = + "get_urdf"; // name of the service that can be used to request the urdf of a planning group +static const std::string SAVE_GEOMETRY_TO_FILE_SERVICE_NAME = + "save_geometry_to_file"; // name of the service that can be used to save CollisionObjects in a PlanningScene to a file +static const std::string LOAD_GEOMETRY_FROM_FILE_SERVICE_NAME = + "load_geometry_from_file"; // name of the service that can be used to load CollisionObjects to a PlanningScene from a file +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h index c29d5ee858..836721939e 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,68 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -namespace move_group -{ -enum MoveGroupState -{ - IDLE, - PLANNING, - MONITOR, - LOOK -}; - -MOVEIT_CLASS_FORWARD(MoveGroupCapability); // Defines MoveGroupCapabilityPtr, ConstPtr, WeakPtr... etc - -class MoveGroupCapability -{ -public: - explicit MoveGroupCapability(const std::string& capability_name) : capability_name_(capability_name) - { - } - - virtual ~MoveGroupCapability() - { - } - - void setContext(const MoveGroupContextPtr& context); - - virtual void initialize() = 0; - - const std::string& getName() const - { - return capability_name_; - } - -protected: - std::string getActionResultString(const moveit_msgs::msg::MoveItErrorCodes& error_code, bool planned_trajectory_empty, - bool plan_only); - std::string stateToStr(MoveGroupState state) const; - - void convertToMsg(const std::vector& trajectory, - moveit_msgs::msg::RobotState& first_state_msg, - std::vector& trajectory_msg) const; - void convertToMsg(const robot_trajectory::RobotTrajectoryPtr& trajectory, - moveit_msgs::msg::RobotState& first_state_msg, - moveit_msgs::msg::RobotTrajectory& trajectory_msg) const; - void convertToMsg(const std::vector& trajectory, - moveit_msgs::msg::RobotState& first_state_msg, - moveit_msgs::msg::RobotTrajectory& trajectory_msg) const; - - planning_interface::MotionPlanRequest - clearRequestStartState(const planning_interface::MotionPlanRequest& request) const; - moveit_msgs::msg::PlanningScene clearSceneRobotState(const moveit_msgs::msg::PlanningScene& scene) const; - bool performTransform(geometry_msgs::msg::PoseStamped& pose_msg, const std::string& target_frame) const; - - planning_pipeline::PlanningPipelinePtr resolvePlanningPipeline(const std::string& pipeline_id) const; - - std::string capability_name_; - MoveGroupContextPtr context_; -}; -} // namespace move_group +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_capability.hpp b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.hpp new file mode 100644 index 0000000000..642b71af83 --- /dev/null +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_capability.hpp @@ -0,0 +1,102 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace move_group +{ +enum MoveGroupState +{ + IDLE, + PLANNING, + MONITOR, + LOOK +}; + +MOVEIT_CLASS_FORWARD(MoveGroupCapability); // Defines MoveGroupCapabilityPtr, ConstPtr, WeakPtr... etc + +class MoveGroupCapability +{ +public: + explicit MoveGroupCapability(const std::string& capability_name) : capability_name_(capability_name) + { + } + + virtual ~MoveGroupCapability() + { + } + + void setContext(const MoveGroupContextPtr& context); + + virtual void initialize() = 0; + + const std::string& getName() const + { + return capability_name_; + } + +protected: + std::string getActionResultString(const moveit_msgs::msg::MoveItErrorCodes& error_code, bool planned_trajectory_empty, + bool plan_only); + std::string stateToStr(MoveGroupState state) const; + + void convertToMsg(const std::vector& trajectory, + moveit_msgs::msg::RobotState& first_state_msg, + std::vector& trajectory_msg) const; + void convertToMsg(const robot_trajectory::RobotTrajectoryPtr& trajectory, + moveit_msgs::msg::RobotState& first_state_msg, + moveit_msgs::msg::RobotTrajectory& trajectory_msg) const; + void convertToMsg(const std::vector& trajectory, + moveit_msgs::msg::RobotState& first_state_msg, + moveit_msgs::msg::RobotTrajectory& trajectory_msg) const; + + planning_interface::MotionPlanRequest + clearRequestStartState(const planning_interface::MotionPlanRequest& request) const; + moveit_msgs::msg::PlanningScene clearSceneRobotState(const moveit_msgs::msg::PlanningScene& scene) const; + bool performTransform(geometry_msgs::msg::PoseStamped& pose_msg, const std::string& target_frame) const; + + planning_pipeline::PlanningPipelinePtr resolvePlanningPipeline(const std::string& pipeline_id) const; + + std::string capability_name_; + MoveGroupContextPtr context_; +}; +} // namespace move_group diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h index acd57575d4..83be3652e0 100644 --- a/moveit_ros/move_group/include/moveit/move_group/move_group_context.h +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,54 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace moveit_cpp -{ -MOVEIT_CLASS_FORWARD(MoveItCpp); -} - -namespace planning_scene_monitor -{ -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc -} - -namespace planning_pipeline -{ -MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc -} - -namespace plan_execution -{ -MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc -} // namespace plan_execution - -namespace trajectory_execution_manager -{ -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc -} - -namespace move_group -{ -MOVEIT_STRUCT_FORWARD(MoveGroupContext); - -struct MoveGroupContext -{ - MoveGroupContext(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, - bool allow_trajectory_execution = false, bool debug = false); - ~MoveGroupContext(); - - bool status() const; - - moveit_cpp::MoveItCppPtr moveit_cpp_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; - planning_pipeline::PlanningPipelinePtr planning_pipeline_; - plan_execution::PlanExecutionPtr plan_execution_; - bool allow_trajectory_execution_; - bool debug_; -}; -} // namespace move_group +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/move_group/include/moveit/move_group/move_group_context.hpp b/moveit_ros/move_group/include/moveit/move_group/move_group_context.hpp new file mode 100644 index 0000000000..676ad3ec07 --- /dev/null +++ b/moveit_ros/move_group/include/moveit/move_group/move_group_context.hpp @@ -0,0 +1,88 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +namespace moveit_cpp +{ +MOVEIT_CLASS_FORWARD(MoveItCpp); +} + +namespace planning_scene_monitor +{ +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc +} + +namespace planning_pipeline +{ +MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc +} + +namespace plan_execution +{ +MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc +} // namespace plan_execution + +namespace trajectory_execution_manager +{ +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc +} + +namespace move_group +{ +MOVEIT_STRUCT_FORWARD(MoveGroupContext); + +struct MoveGroupContext +{ + MoveGroupContext(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, + bool allow_trajectory_execution = false, bool debug = false); + ~MoveGroupContext(); + + bool status() const; + + moveit_cpp::MoveItCppPtr moveit_cpp_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + planning_pipeline::PlanningPipelinePtr planning_pipeline_; + plan_execution::PlanExecutionPtr plan_execution_; + bool allow_trajectory_execution_; + bool debug_; +}; +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp index ec3725694e..2a26fed0ce 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp @@ -34,9 +34,9 @@ /* Author: Michael Goerner */ -#include "apply_planning_scene_service_capability.h" -#include -#include +#include "apply_planning_scene_service_capability.hpp" +#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.hpp index d48dbaa131..d220c88ae0 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 34af2a8094..0bf79bb987 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -34,18 +34,18 @@ /* Author: Ioan Sucan */ -#include "cartesian_path_service_capability.h" -#include -#include -#include -#include +#include "cartesian_path_service_capability.hpp" +#include +#include +#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include +#include #include using moveit::getLogger; diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.hpp index f0fe447cd7..9b2819009e 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp index a62b8b5ad5..376ab2a8ca 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp @@ -34,9 +34,9 @@ /* Author: David Hershberger */ -#include "clear_octomap_service_capability.h" -#include -#include +#include "clear_octomap_service_capability.hpp" +#include +#include #include namespace diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.hpp index 05f0c3ff94..04a68f70da 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 8d78f2ac41..ff7959e439 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -34,13 +34,13 @@ /* Author: Kentaro Wada */ -#include "execute_trajectory_action_capability.h" +#include "execute_trajectory_action_capability.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.hpp similarity index 98% rename from moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h rename to moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.hpp index ff4ebd36ea..174e041f16 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.hpp @@ -40,7 +40,7 @@ #pragma once -#include +#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp index fffdbf52f9..7bb701fa2a 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp @@ -34,10 +34,10 @@ /* Author: Sebastian Jahr */ -#include "get_group_urdf_capability.h" +#include "get_group_urdf_capability.hpp" -#include -#include +#include +#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h rename to moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.hpp index 05fb6b10ff..08cd2eeaa6 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.hpp @@ -37,7 +37,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp index 09aac4eed2..110914defe 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include "get_planning_scene_service_capability.h" -#include +#include "get_planning_scene_service_capability.hpp" +#include namespace move_group { diff --git a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.hpp index 6b056296b4..a109de1e2a 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_planning_scene_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index c2cd031305..2120ee1d20 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -34,12 +34,12 @@ /* Author: Ioan Sucan */ -#include "kinematics_service_capability.h" -#include -#include -#include +#include "kinematics_service_capability.hpp" +#include +#include +#include #include -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.hpp similarity index 98% rename from moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.hpp index 0df58ea057..2d8118478a 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp index 93a7b51520..ba4b0bfedc 100644 --- a/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp @@ -34,10 +34,10 @@ /* Author: Bilal Gill */ -#include "load_geometry_from_file_service_capability.h" +#include "load_geometry_from_file_service_capability.hpp" -#include -#include +#include +#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.h b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.hpp index 095aba5465..15d366d1cb 100644 --- a/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index ba7e77bfae..bc00199f27 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -34,15 +34,15 @@ /* Author: Ioan Sucan */ -#include "move_action_capability.h" - -#include -#include -#include -#include -#include -#include -#include +#include "move_action_capability.hpp" + +#include +#include +#include +#include +#include +#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h b/moveit_ros/move_group/src/default_capabilities/move_action_capability.hpp similarity index 98% rename from moveit_ros/move_group/src/default_capabilities/move_action_capability.h rename to moveit_ros/move_group/src/default_capabilities/move_action_capability.hpp index cccae63f9c..4306130dfc 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index 0a5018d2c3..b5450e4fac 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp @@ -34,11 +34,11 @@ /* Author: Ioan Sucan */ -#include "plan_service_capability.h" +#include "plan_service_capability.hpp" -#include -#include -#include +#include +#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/plan_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/plan_service_capability.hpp index fc99b87e53..089cc20dab 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 9ea896bdc9..0bce586f3a 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan, Robert Haschke */ -#include "query_planners_service_capability.h" -#include -#include -#include +#include "query_planners_service_capability.hpp" +#include +#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.hpp similarity index 98% rename from moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.hpp index 8fa02ea1ef..7842a20648 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp index 0053459bce..1c1ee225ea 100644 --- a/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp @@ -34,10 +34,10 @@ /* Author: Bilal Gill */ -#include "save_geometry_to_file_service_capability.h" +#include "save_geometry_to_file_service_capability.hpp" -#include -#include +#include +#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.h b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.hpp index 30fa21c75b..5ef3ecc4ab 100644 --- a/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp index 0e48096cb9..295bc4af5a 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp @@ -34,12 +34,12 @@ /* Author: Ioan Sucan */ -#include "state_validation_service_capability.h" -#include -#include -#include -#include -#include +#include "state_validation_service_capability.hpp" +#include +#include +#include +#include +#include namespace move_group { diff --git a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h rename to moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.hpp index 1efaf8caa9..2daf30eb11 100644 --- a/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 8ed2af48c1..f6035e52ca 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -34,14 +34,14 @@ /* Author: Jonas Tietz */ -#include "tf_publisher_capability.h" -#include -#include -#include +#include "tf_publisher_capability.hpp" +#include +#include +#include #include #include -#include -#include +#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.hpp similarity index 97% rename from moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h rename to moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.hpp index 7c01174408..97643b6c92 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace move_group diff --git a/moveit_ros/move_group/src/list_capabilities.cpp b/moveit_ros/move_group/src/list_capabilities.cpp index 78ff926dd5..01ba7e6082 100644 --- a/moveit_ros/move_group/src/list_capabilities.cpp +++ b/moveit_ros/move_group/src/list_capabilities.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index bb9629d191..2389100756 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -34,14 +34,14 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include -#include -#include +#include +#include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index 3a33da9393..f0f6f70b85 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index 2d11ab6563..b0d4d2ed41 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -34,11 +34,11 @@ /* Author: Ioan Sucan */ -#include +#include -#include -#include -#include +#include +#include +#include #include namespace move_group diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.h new file mode 100644 index 0000000000..4a0844ec61 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.h @@ -0,0 +1,56 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ +/* + * Title : collision_monitor.hpp + * Project : moveit_servo + * Created : 06/08/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description: Monitors the planning scene for collision and publishes the velocity scaling. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp index a68260bd8d..9752e5feb6 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp @@ -42,8 +42,8 @@ #pragma once #include -#include -#include +#include +#include namespace moveit_servo { diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h new file mode 100644 index 0000000000..08f0a9bcf4 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -0,0 +1,56 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : servo.hpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The core servoing logic. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index d946bdc833..d0331c650d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -45,9 +45,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h new file mode 100644 index 0000000000..9887d0742f --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h @@ -0,0 +1,56 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : servo_node.hpp + * Project : moveit_servo + * Created : 01/07/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The ROS API for Servo + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.h b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.h new file mode 100644 index 0000000000..c42f449d41 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.h @@ -0,0 +1,56 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : command.hpp + * Project : moveit_servo + * Created : 06/04/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The methods that compute the required change in joint angles for various input types. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index 0e7f07d914..bf9c432527 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -42,8 +42,8 @@ #pragma once #include -#include -#include +#include +#include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.h b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.h new file mode 100644 index 0000000000..1933c2d101 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.h @@ -0,0 +1,56 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : utils.hpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + * + * Description : The utility functions used by MoveIt Servo. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp index 060a113f56..6d44c18298 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -43,9 +43,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.h b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.h new file mode 100644 index 0000000000..18d364ed57 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.h @@ -0,0 +1,56 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 HOLDER 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. + *******************************************************************************/ + +/* Title : datatypes.hpp + * Project : moveit_servo + * Created : 06/05/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + * + * Description : The custom datatypes used by Moveit Servo. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index b8a318a9b6..660efe3d7d 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -40,6 +40,8 @@ * Description : Resources used by servo c++ integration tests */ +#pragma once + #include #include #include diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index 26f78362cc..2e0662bb1f 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -39,6 +39,8 @@ Created : 07/23/2023 */ +#pragma once + #include #include #include diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp index d84fc503bd..6173c4fa3d 100644 --- a/moveit_ros/moveit_servo/tests/test_utils.cpp +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include namespace diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h index 8b34da6d8a..e510834c2a 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,301 +47,5 @@ /* Author: Ioan Sucan, Jon Binney */ #pragma once - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace occupancy_map_monitor -{ -class OccupancyMapMonitor -{ -public: - /** - * @brief This class describes parameters that are normally provided through ROS Parameters. - */ - struct Parameters - { - double map_resolution; - std::string map_frame; - std::vector> sensor_plugins; - }; - - /** - * @brief This class contains the rcl interfaces for easier testing - */ - class MiddlewareHandle - { - public: - using SaveMapServiceCallback = std::function request_header, - const std::shared_ptr request, - std::shared_ptr response)>; - using LoadMapServiceCallback = std::function request_header, - const std::shared_ptr request, - std::shared_ptr response)>; - - /** - * @brief Destroys the object. Needed because this is pure virtual interface. - */ - virtual ~MiddlewareHandle() = default; - - /** - * @brief Gets the parameters struct. - * - * @return The parameters. - */ - virtual Parameters getParameters() const = 0; - - /** - * @brief Loads an occupancy map updater based on string. - * - * @param[in] sensor_plugin The sensor plugin string. - * - * @return The occupancy map updater pointer. - */ - virtual OccupancyMapUpdaterPtr loadOccupancyMapUpdater(const std::string& sensor_plugin) = 0; - - /** - * @brief Initializes the occupancy map updater. Needed because of interface to OccupancyMapUpdater - * - * @param[in] occupancy_map_updater The occupancy map updater - */ - virtual void initializeOccupancyMapUpdater(OccupancyMapUpdaterPtr occupancy_map_updater) = 0; - - /** - * @brief Creates a save map service. - * - * @param[in] callback The callback - */ - virtual void createSaveMapService(SaveMapServiceCallback callback) = 0; - - /** - * @brief Creates a load map service. - * - * @param[in] callback The callback - */ - virtual void createLoadMapService(LoadMapServiceCallback callback) = 0; - }; - - /** - * @brief Occupancy map monitor constructor with the MiddlewareHandle - * - * @param[in] middleware_handle The rcl interface - * @param[in] tf_buffer The tf buffer - */ - OccupancyMapMonitor(std::unique_ptr middleware_handle, - const std::shared_ptr& tf_buffer); - - /** - * @brief Occupancy map monitor constructor with Node - * - * @param[in] node The node - * @param[in] tf_buffer The tf buffer - * @param[in] map_frame The map frame - * @param[in] map_resolution The map resolution - */ - OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, const std::shared_ptr& tf_buffer, - const std::string& map_frame = "", double map_resolution = 0.0); - - /** - * @brief Occupancy map monitor constructor with Node - * - * @param[in] node The node - * @param[in] map_resolution The map resolution - */ - OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, double map_resolution = 0.0); - - /** - * @brief Destroys the object. - */ - ~OccupancyMapMonitor(); - - /** - * @brief start the monitor (will begin updating the octomap - */ - void startMonitor(); - - /** - * @brief Stops the monitor, this also stops the updaters. - */ - void stopMonitor(); - - /** @brief Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing using this - * pointer. The value of this pointer stays the same throughout the existence of the monitor instance. */ - const collision_detection::OccMapTreePtr& getOcTreePtr() - { - return tree_; - } - - /** @brief Get a const pointer to the underlying octree for this monitor. Lock the - * tree before reading this pointer */ - const collision_detection::OccMapTreeConstPtr& getOcTreePtr() const - { - return tree_const_; - } - - /** - * @brief Gets the map frame (this is set either by the constor or a parameter). - * - * @return The map frame. - */ - const std::string& getMapFrame() const - { - return parameters_.map_frame; - } - - /** - * @brief Sets the map frame. - * - * @param[in] frame The frame - */ - void setMapFrame(const std::string& frame); - - /** - * @brief Gets the map resolution. - * - * @return The map resolution. - */ - double getMapResolution() const - { - return parameters_.map_resolution; - } - - /** - * @brief Gets the tf client. - * - * @return The tf client. - */ - const std::shared_ptr& getTFClient() const - { - return tf_buffer_; - } - - /** - * @brief Adds an OccupancyMapUpdater to be monitored. - * - * @param[in] updater The OccupancyMapUpdater - */ - void addUpdater(const OccupancyMapUpdaterPtr& updater); - - /** - * @brief Add this shape to the set of shapes to be filtered out from the octomap - * - * @param[in] shape The shape to be exclueded from the updaters. - * - * @return The shape handle. Can be used to forget the shape later. - */ - ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape); - - /** - * \brief Forget about this shape handle and the shapes it corresponds to - * - * @param[in] handle The handle to forget. - */ - void forgetShape(ShapeHandle handle); - - /** - * @brief Set the callback to trigger when updates to the maintained octomap are received - * - * @param[in] update_callback The update callback function - */ - void setUpdateCallback(const std::function& update_callback) - { - tree_->setUpdateCallback(update_callback); - } - - /** - * @brief Sets the transform cache callback. - * - * @param[in] transform_cache_callback The transform cache callback - */ - void setTransformCacheCallback(const TransformCacheProvider& transform_cache_callback); - - /** - * @brief Set the debug flag on the updaters. - * - * @param[in] flag The flag - */ - void publishDebugInformation(bool flag); - - /** - * @brief Determines if active. - * - * @return True if active, False otherwise. - */ - bool isActive() const - { - return active_; - } - -private: - /** - * @brief Save the current octree to a binary file - * - * @param[in] request_header The request header - * @param[in] request The request - * @param[in] response The response - * - * @return True on success, False otherwise. - */ - bool saveMapCallback(const std::shared_ptr& request_header, - const std::shared_ptr& request, - const std::shared_ptr& response); - - /** - * @brief Load octree from a binary file (gets rid of current octree data) - * - * @param[in] request_header The request header - * @param[in] request The request - * @param[in] response The response - * - * @return True on success, False otherwise. - */ - bool loadMapCallback(const std::shared_ptr& request_header, - const std::shared_ptr& request, - const std::shared_ptr& response); - - /** - * @brief Gets the shape transform cache. - * - * @param[in] index The index - * @param[in] target_frame The target frame - * @param[in] target_time The target time - * @param[out] cache The cache - * - * @return True on success, False otherwise. - */ - bool getShapeTransformCache(std::size_t index, const std::string& target_frame, const rclcpp::Time& target_time, - ShapeTransformCache& cache) const; - - std::unique_ptr middleware_handle_; /*!< The abstract interface to ros */ - std::shared_ptr tf_buffer_; /*!< TF buffer */ - Parameters parameters_; - std::mutex parameters_lock_; /*!< Mutex for synchronizing access to parameters */ - - collision_detection::OccMapTreePtr tree_; /*!< Oct map tree */ - collision_detection::OccMapTreeConstPtr tree_const_; /*!< Shared pointer to a const oct map tree */ - - std::vector map_updaters_; /*!< The Occupancy map updaters */ - std::vector> mesh_handles_; /*!< The mesh handles */ - TransformCacheProvider transform_cache_callback_; /*!< Callback for the transform cache */ - bool debug_info_; /*!< Enable/disable debug output */ - - std::size_t mesh_handle_count_; /*!< Count of mesh handles */ - - bool active_; /*!< True when actively monitoring updaters */ - - rclcpp::Logger logger_; -}; -} // namespace occupancy_map_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp new file mode 100644 index 0000000000..7f58505143 --- /dev/null +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp @@ -0,0 +1,335 @@ +/********************************************************************* + * 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, Jon Binney */ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace occupancy_map_monitor +{ +class OccupancyMapMonitor +{ +public: + /** + * @brief This class describes parameters that are normally provided through ROS Parameters. + */ + struct Parameters + { + double map_resolution; + std::string map_frame; + std::vector> sensor_plugins; + }; + + /** + * @brief This class contains the rcl interfaces for easier testing + */ + class MiddlewareHandle + { + public: + using SaveMapServiceCallback = std::function request_header, + const std::shared_ptr request, + std::shared_ptr response)>; + using LoadMapServiceCallback = std::function request_header, + const std::shared_ptr request, + std::shared_ptr response)>; + + /** + * @brief Destroys the object. Needed because this is pure virtual interface. + */ + virtual ~MiddlewareHandle() = default; + + /** + * @brief Gets the parameters struct. + * + * @return The parameters. + */ + virtual Parameters getParameters() const = 0; + + /** + * @brief Loads an occupancy map updater based on string. + * + * @param[in] sensor_plugin The sensor plugin string. + * + * @return The occupancy map updater pointer. + */ + virtual OccupancyMapUpdaterPtr loadOccupancyMapUpdater(const std::string& sensor_plugin) = 0; + + /** + * @brief Initializes the occupancy map updater. Needed because of interface to OccupancyMapUpdater + * + * @param[in] occupancy_map_updater The occupancy map updater + */ + virtual void initializeOccupancyMapUpdater(OccupancyMapUpdaterPtr occupancy_map_updater) = 0; + + /** + * @brief Creates a save map service. + * + * @param[in] callback The callback + */ + virtual void createSaveMapService(SaveMapServiceCallback callback) = 0; + + /** + * @brief Creates a load map service. + * + * @param[in] callback The callback + */ + virtual void createLoadMapService(LoadMapServiceCallback callback) = 0; + }; + + /** + * @brief Occupancy map monitor constructor with the MiddlewareHandle + * + * @param[in] middleware_handle The rcl interface + * @param[in] tf_buffer The tf buffer + */ + OccupancyMapMonitor(std::unique_ptr middleware_handle, + const std::shared_ptr& tf_buffer); + + /** + * @brief Occupancy map monitor constructor with Node + * + * @param[in] node The node + * @param[in] tf_buffer The tf buffer + * @param[in] map_frame The map frame + * @param[in] map_resolution The map resolution + */ + OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, const std::shared_ptr& tf_buffer, + const std::string& map_frame = "", double map_resolution = 0.0); + + /** + * @brief Occupancy map monitor constructor with Node + * + * @param[in] node The node + * @param[in] map_resolution The map resolution + */ + OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, double map_resolution = 0.0); + + /** + * @brief Destroys the object. + */ + ~OccupancyMapMonitor(); + + /** + * @brief start the monitor (will begin updating the octomap + */ + void startMonitor(); + + /** + * @brief Stops the monitor, this also stops the updaters. + */ + void stopMonitor(); + + /** @brief Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing using this + * pointer. The value of this pointer stays the same throughout the existence of the monitor instance. */ + const collision_detection::OccMapTreePtr& getOcTreePtr() + { + return tree_; + } + + /** @brief Get a const pointer to the underlying octree for this monitor. Lock the + * tree before reading this pointer */ + const collision_detection::OccMapTreeConstPtr& getOcTreePtr() const + { + return tree_const_; + } + + /** + * @brief Gets the map frame (this is set either by the constor or a parameter). + * + * @return The map frame. + */ + const std::string& getMapFrame() const + { + return parameters_.map_frame; + } + + /** + * @brief Sets the map frame. + * + * @param[in] frame The frame + */ + void setMapFrame(const std::string& frame); + + /** + * @brief Gets the map resolution. + * + * @return The map resolution. + */ + double getMapResolution() const + { + return parameters_.map_resolution; + } + + /** + * @brief Gets the tf client. + * + * @return The tf client. + */ + const std::shared_ptr& getTFClient() const + { + return tf_buffer_; + } + + /** + * @brief Adds an OccupancyMapUpdater to be monitored. + * + * @param[in] updater The OccupancyMapUpdater + */ + void addUpdater(const OccupancyMapUpdaterPtr& updater); + + /** + * @brief Add this shape to the set of shapes to be filtered out from the octomap + * + * @param[in] shape The shape to be exclueded from the updaters. + * + * @return The shape handle. Can be used to forget the shape later. + */ + ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape); + + /** + * \brief Forget about this shape handle and the shapes it corresponds to + * + * @param[in] handle The handle to forget. + */ + void forgetShape(ShapeHandle handle); + + /** + * @brief Set the callback to trigger when updates to the maintained octomap are received + * + * @param[in] update_callback The update callback function + */ + void setUpdateCallback(const std::function& update_callback) + { + tree_->setUpdateCallback(update_callback); + } + + /** + * @brief Sets the transform cache callback. + * + * @param[in] transform_cache_callback The transform cache callback + */ + void setTransformCacheCallback(const TransformCacheProvider& transform_cache_callback); + + /** + * @brief Set the debug flag on the updaters. + * + * @param[in] flag The flag + */ + void publishDebugInformation(bool flag); + + /** + * @brief Determines if active. + * + * @return True if active, False otherwise. + */ + bool isActive() const + { + return active_; + } + +private: + /** + * @brief Save the current octree to a binary file + * + * @param[in] request_header The request header + * @param[in] request The request + * @param[in] response The response + * + * @return True on success, False otherwise. + */ + bool saveMapCallback(const std::shared_ptr& request_header, + const std::shared_ptr& request, + const std::shared_ptr& response); + + /** + * @brief Load octree from a binary file (gets rid of current octree data) + * + * @param[in] request_header The request header + * @param[in] request The request + * @param[in] response The response + * + * @return True on success, False otherwise. + */ + bool loadMapCallback(const std::shared_ptr& request_header, + const std::shared_ptr& request, + const std::shared_ptr& response); + + /** + * @brief Gets the shape transform cache. + * + * @param[in] index The index + * @param[in] target_frame The target frame + * @param[in] target_time The target time + * @param[out] cache The cache + * + * @return True on success, False otherwise. + */ + bool getShapeTransformCache(std::size_t index, const std::string& target_frame, const rclcpp::Time& target_time, + ShapeTransformCache& cache) const; + + std::unique_ptr middleware_handle_; /*!< The abstract interface to ros */ + std::shared_ptr tf_buffer_; /*!< TF buffer */ + Parameters parameters_; + std::mutex parameters_lock_; /*!< Mutex for synchronizing access to parameters */ + + collision_detection::OccMapTreePtr tree_; /*!< Oct map tree */ + collision_detection::OccMapTreeConstPtr tree_const_; /*!< Shared pointer to a const oct map tree */ + + std::vector map_updaters_; /*!< The Occupancy map updaters */ + std::vector> mesh_handles_; /*!< The mesh handles */ + TransformCacheProvider transform_cache_callback_; /*!< Callback for the transform cache */ + bool debug_info_; /*!< Enable/disable debug output */ + + std::size_t mesh_handle_count_; /*!< Count of mesh handles */ + + bool active_; /*!< True when actively monitoring updaters */ + + rclcpp::Logger logger_; +}; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.h new file mode 100644 index 0000000000..524be9f63c --- /dev/null +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik, 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 PickNik 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: Tyler Weaver */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp index d73e7c43a7..aacfc8e03e 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor_middleware_handle.hpp @@ -36,8 +36,8 @@ #pragma once -#include -#include +#include +#include #include #include diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h index 429000b63a..d85ae969f0 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,85 +47,5 @@ /* Author: Ioan Sucan, Jon Binney */ #pragma once - -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include - -namespace occupancy_map_monitor -{ -using ShapeHandle = unsigned int; -using ShapeTransformCache = std::map, - Eigen::aligned_allocator > >; -using TransformCacheProvider = std::function; - -class OccupancyMapMonitor; - -MOVEIT_CLASS_FORWARD(OccupancyMapUpdater); // Defines OccupancyMapUpdaterPtr, ConstPtr, WeakPtr... etc - -/** \brief Base class for classes which update the occupancy map. - */ -class OccupancyMapUpdater -{ -public: - OccupancyMapUpdater(const std::string& type); - virtual ~OccupancyMapUpdater(); - - /** \brief This is the first function to be called after construction */ - void setMonitor(OccupancyMapMonitor* monitor); - - /** @brief Set updater params using struct that comes from parsing a yaml string. This must be called after - * setMonitor() */ - virtual bool setParams(const std::string& name_space) = 0; - - /** @brief Do any necessary setup (subscribe to ros topics, etc.). This call assumes setMonitor() and setParams() have - * been previously called. */ - virtual bool initialize(const rclcpp::Node::SharedPtr& node) = 0; - - virtual void start() = 0; - - virtual void stop() = 0; - - virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) = 0; - - virtual void forgetShape(ShapeHandle handle) = 0; - - const std::string& getType() const - { - return type_; - } - - void setTransformCacheCallback(const TransformCacheProvider& transform_callback) - { - transform_provider_callback_ = transform_callback; - } - - void publishDebugInformation(bool flag) - { - debug_info_ = flag; - } - -protected: - OccupancyMapMonitor* monitor_; - std::string type_; - collision_detection::OccMapTreePtr tree_; - TransformCacheProvider transform_provider_callback_; - ShapeTransformCache transform_cache_; - bool debug_info_; - - bool updateTransformCache(const std::string& target_frame, const rclcpp::Time& target_time); - - // TODO rework this function - // static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, double* value); - // static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, unsigned int* value); -}; -} // namespace occupancy_map_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.hpp b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.hpp new file mode 100644 index 0000000000..fa29cd1ace --- /dev/null +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.hpp @@ -0,0 +1,119 @@ +/********************************************************************* + * 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, Jon Binney */ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +namespace occupancy_map_monitor +{ +using ShapeHandle = unsigned int; +using ShapeTransformCache = std::map, + Eigen::aligned_allocator > >; +using TransformCacheProvider = std::function; + +class OccupancyMapMonitor; + +MOVEIT_CLASS_FORWARD(OccupancyMapUpdater); // Defines OccupancyMapUpdaterPtr, ConstPtr, WeakPtr... etc + +/** \brief Base class for classes which update the occupancy map. + */ +class OccupancyMapUpdater +{ +public: + OccupancyMapUpdater(const std::string& type); + virtual ~OccupancyMapUpdater(); + + /** \brief This is the first function to be called after construction */ + void setMonitor(OccupancyMapMonitor* monitor); + + /** @brief Set updater params using struct that comes from parsing a yaml string. This must be called after + * setMonitor() */ + virtual bool setParams(const std::string& name_space) = 0; + + /** @brief Do any necessary setup (subscribe to ros topics, etc.). This call assumes setMonitor() and setParams() have + * been previously called. */ + virtual bool initialize(const rclcpp::Node::SharedPtr& node) = 0; + + virtual void start() = 0; + + virtual void stop() = 0; + + virtual ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) = 0; + + virtual void forgetShape(ShapeHandle handle) = 0; + + const std::string& getType() const + { + return type_; + } + + void setTransformCacheCallback(const TransformCacheProvider& transform_callback) + { + transform_provider_callback_ = transform_callback; + } + + void publishDebugInformation(bool flag) + { + debug_info_ = flag; + } + +protected: + OccupancyMapMonitor* monitor_; + std::string type_; + collision_detection::OccMapTreePtr tree_; + TransformCacheProvider transform_provider_callback_; + ShapeTransformCache transform_cache_; + bool debug_info_; + + bool updateTransformCache(const std::string& target_frame, const rclcpp::Time& target_time); + + // TODO rework this function + // static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, double* value); + // static void readXmlParam(XmlRpc::XmlRpcValue& params, const std::string& param_name, unsigned int* value); +}; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index abe0819917..e8943aa02b 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Jon Binney */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp index dd2eec7f22..4e870ca9f8 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp @@ -35,7 +35,7 @@ /* Author: Tyler Weaver */ #include -#include +#include #include #include diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index 1d09c9d604..7cd0dbb9bc 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -34,7 +34,7 @@ /* Author: Jon Binney, Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp index 95d98a2ae3..7520e9fccd 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Jon Binney */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp b/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp index 46a6653ea5..207480e531 100644 --- a/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp +++ b/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp @@ -34,7 +34,7 @@ /* Author: Tyler Weaver */ -#include +#include #include #include diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h index d54b7f475a..9f1b55d5b9 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,77 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace occupancy_map_monitor -{ -class DepthImageOctomapUpdater : public OccupancyMapUpdater -{ -public: - DepthImageOctomapUpdater(); - ~DepthImageOctomapUpdater() override; - - bool setParams(const std::string& name_space) override; - bool initialize(const rclcpp::Node::SharedPtr& node) override; - void start() override; - void stop() override; - ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) override; - void forgetShape(ShapeHandle handle) override; - -private: - void depthImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg); - bool getShapeTransform(mesh_filter::MeshHandle h, Eigen::Isometry3d& transform) const; - - rclcpp::Node::SharedPtr node_; - std::shared_ptr tf_buffer_; - std::unique_ptr input_depth_transport_; - std::unique_ptr model_depth_transport_; - std::unique_ptr filtered_depth_transport_; - std::unique_ptr filtered_label_transport_; - - image_transport::CameraSubscriber sub_depth_image_; - image_transport::CameraPublisher pub_model_depth_image_; - image_transport::CameraPublisher pub_filtered_depth_image_; - image_transport::CameraPublisher pub_filtered_label_image_; - - // Initialize clock type to RCL_ROS_TIME to prevent exception about time sources mismatch - rclcpp::Time last_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - - std::string filtered_cloud_topic_; - std::string ns_; - std::string sensor_type_; - std::string image_topic_; - std::size_t queue_size_; - double near_clipping_plane_distance_; - double far_clipping_plane_distance_; - double shadow_threshold_; - double padding_scale_; - double padding_offset_; - double max_update_rate_; - unsigned int skip_vertical_pixels_; - unsigned int skip_horizontal_pixels_; - - unsigned int image_callback_count_; - double average_callback_dt_; - unsigned int good_tf_; - unsigned int failed_tf_; - - std::unique_ptr > mesh_filter_; - std::unique_ptr free_space_updater_; - - std::vector x_cache_, y_cache_; - double inv_fx_, inv_fy_, K0_, K2_, K4_, K5_; - std::vector filtered_labels_; - rclcpp::Time last_depth_callback_start_; - rclcpp::Logger logger_; -}; -} // namespace occupancy_map_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp new file mode 100644 index 0000000000..13d22bcfc9 --- /dev/null +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace occupancy_map_monitor +{ +class DepthImageOctomapUpdater : public OccupancyMapUpdater +{ +public: + DepthImageOctomapUpdater(); + ~DepthImageOctomapUpdater() override; + + bool setParams(const std::string& name_space) override; + bool initialize(const rclcpp::Node::SharedPtr& node) override; + void start() override; + void stop() override; + ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) override; + void forgetShape(ShapeHandle handle) override; + +private: + void depthImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg); + bool getShapeTransform(mesh_filter::MeshHandle h, Eigen::Isometry3d& transform) const; + + rclcpp::Node::SharedPtr node_; + std::shared_ptr tf_buffer_; + std::unique_ptr input_depth_transport_; + std::unique_ptr model_depth_transport_; + std::unique_ptr filtered_depth_transport_; + std::unique_ptr filtered_label_transport_; + + image_transport::CameraSubscriber sub_depth_image_; + image_transport::CameraPublisher pub_model_depth_image_; + image_transport::CameraPublisher pub_filtered_depth_image_; + image_transport::CameraPublisher pub_filtered_label_image_; + + // Initialize clock type to RCL_ROS_TIME to prevent exception about time sources mismatch + rclcpp::Time last_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + std::string filtered_cloud_topic_; + std::string ns_; + std::string sensor_type_; + std::string image_topic_; + std::size_t queue_size_; + double near_clipping_plane_distance_; + double far_clipping_plane_distance_; + double shadow_threshold_; + double padding_scale_; + double padding_offset_; + double max_update_rate_; + unsigned int skip_vertical_pixels_; + unsigned int skip_horizontal_pixels_; + + unsigned int image_callback_count_; + double average_callback_dt_; + unsigned int good_tf_; + unsigned int failed_tf_; + + std::unique_ptr > mesh_filter_; + std::unique_ptr free_space_updater_; + + std::vector x_cache_, y_cache_; + double inv_fx_, inv_fy_, K0_, K2_, K4_, K5_; + std::vector filtered_labels_; + rclcpp::Time last_depth_callback_start_; + rclcpp::Logger logger_; +}; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index b9cd185d67..6912877fad 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Suat Gedikli */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp index a32b72a23e..ad529c16ad 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/updater_plugin.cpp @@ -35,6 +35,6 @@ /* Author: Ioan Sucan */ #include -#include +#include CLASS_LOADER_REGISTER_CLASS(occupancy_map_monitor::DepthImageOctomapUpdater, occupancy_map_monitor::OccupancyMapUpdater) diff --git a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h index cea8f3064b..80061c02e2 100644 --- a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h +++ b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,60 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace occupancy_map_monitor -{ -class LazyFreeSpaceUpdater -{ -public: - LazyFreeSpaceUpdater(const collision_detection::OccMapTreePtr& tree, unsigned int max_batch_size = 10); - ~LazyFreeSpaceUpdater(); - - void pushLazyUpdate(octomap::KeySet* occupied_cells, octomap::KeySet* model_cells, - const octomap::point3d& sensor_origin); - -private: -#ifdef __APPLE__ - typedef std::unordered_map OcTreeKeyCountMap; -#elif __cplusplus >= 201103L - typedef std::unordered_map OcTreeKeyCountMap; -#else - typedef std::tr1::unordered_map OcTreeKeyCountMap; -#endif - - void pushBatchToProcess(OcTreeKeyCountMap* occupied_cells, octomap::KeySet* model_cells, - const octomap::point3d& sensor_origin); - - void lazyUpdateThread(); - void processThread(); - - collision_detection::OccMapTreePtr tree_; - bool running_; - std::size_t max_batch_size_; - double max_sensor_delta_; - - std::deque occupied_cells_sets_; - std::deque model_cells_sets_; - std::deque sensor_origins_; - std::condition_variable update_condition_; - std::mutex update_cell_sets_lock_; - - OcTreeKeyCountMap* process_occupied_cells_set_; - octomap::KeySet* process_model_cells_set_; - octomap::point3d process_sensor_origin_; - std::condition_variable process_condition_; - std::mutex cell_process_lock_; - - std::thread update_thread_; - std::thread process_thread_; - - rclcpp::Logger logger_; -}; -} // namespace occupancy_map_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.hpp b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.hpp new file mode 100644 index 0000000000..0aa4276b54 --- /dev/null +++ b/moveit_ros/perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.hpp @@ -0,0 +1,94 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace occupancy_map_monitor +{ +class LazyFreeSpaceUpdater +{ +public: + LazyFreeSpaceUpdater(const collision_detection::OccMapTreePtr& tree, unsigned int max_batch_size = 10); + ~LazyFreeSpaceUpdater(); + + void pushLazyUpdate(octomap::KeySet* occupied_cells, octomap::KeySet* model_cells, + const octomap::point3d& sensor_origin); + +private: +#ifdef __APPLE__ + typedef std::unordered_map OcTreeKeyCountMap; +#elif __cplusplus >= 201103L + typedef std::unordered_map OcTreeKeyCountMap; +#else + typedef std::tr1::unordered_map OcTreeKeyCountMap; +#endif + + void pushBatchToProcess(OcTreeKeyCountMap* occupied_cells, octomap::KeySet* model_cells, + const octomap::point3d& sensor_origin); + + void lazyUpdateThread(); + void processThread(); + + collision_detection::OccMapTreePtr tree_; + bool running_; + std::size_t max_batch_size_; + double max_sensor_delta_; + + std::deque occupied_cells_sets_; + std::deque model_cells_sets_; + std::deque sensor_origins_; + std::condition_variable update_condition_; + std::mutex update_cell_sets_lock_; + + OcTreeKeyCountMap* process_occupied_cells_set_; + octomap::KeySet* process_model_cells_set_; + octomap::point3d process_sensor_origin_; + std::condition_variable process_condition_; + std::mutex cell_process_lock_; + + std::thread update_thread_; + std::thread process_thread_; + + rclcpp::Logger logger_; +}; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index fff1edf040..9f092f4181 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h index f67e0869a9..2ea6b35849 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,96 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace mesh_filter -{ -/** - * \brief Nodelet for filtering meshes from depth images. e.g. meshes of the robot or any attached object where - * a transformation can be provided for. - * \author Suat Gedikli (gedikli@willowgarage.com) - */ -class DepthSelfFiltering : public nodelet::Nodelet -{ -public: - /** \brief Nodelet init callback*/ - void onInit() override; - -private: - ~DepthSelfFiltering() override; - - /** - * \brief adding the meshes to a given mesh filter object. - * \param[in,out] mesh_filter mesh filter object that gets meshes from the robot description added to - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void addMeshes(mesh_filter::MeshFilter& mesh_filter); - - /** - * \brief main filtering routine - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void filter(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - - /** - * \brief Callback for connection/deconnection of listener - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void connectCb(); - - /** - * \brief Callback for subscribed depth images - * \author Suat Gedikli (gedikli@willowgarage.com) - * @param depth_msg depth image - * @param info_msg camera information containing parameters frame, etc. - */ - void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - -private: - // member variables to handle ros messages - std::shared_ptr input_depth_transport_; - std::shared_ptr filtered_label_transport_; - std::shared_ptr filtered_depth_transport_; - std::shared_ptr model_depth_transport_; - std::shared_ptr model_label_transport_; - image_transport::CameraSubscriber sub_depth_image_; - image_transport::CameraPublisher pub_filtered_depth_image_; - image_transport::CameraPublisher pub_filtered_label_image_; - image_transport::CameraPublisher pub_model_depth_image_; - image_transport::CameraPublisher pub_model_label_image_; - - /** \brief required to avoid listener registration before we are all set*/ - std::mutex connect_mutex_; - TransformProvider transform_provider_; - - std::shared_ptr filtered_depth_ptr_; - std::shared_ptr filtered_label_ptr_; - std::shared_ptr model_depth_ptr_; - std::shared_ptr model_label_ptr_; - /** \brief distance of near clipping plane*/ - double near_clipping_plane_distance_; - - /** \brief distance of far clipping plane*/ - double far_clipping_plane_distance_; - - /** \brief threshold that indicates a pixel to be in shadow, rather than being filtered out */ - double shadow_threshold_; - - /** \brief the coefficient for the square component of padding function in 1/m*/ - double padding_scale_; - - /** \brief the coefficient for the linear component of the padding function*/ - double padding_offset_; - - /** mesh filter object*/ - MeshFilter::Ptr mesh_filter_; -}; - -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.hpp new file mode 100644 index 0000000000..1b531d2311 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.hpp @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace mesh_filter +{ +/** + * \brief Nodelet for filtering meshes from depth images. e.g. meshes of the robot or any attached object where + * a transformation can be provided for. + * \author Suat Gedikli (gedikli@willowgarage.com) + */ +class DepthSelfFiltering : public nodelet::Nodelet +{ +public: + /** \brief Nodelet init callback*/ + void onInit() override; + +private: + ~DepthSelfFiltering() override; + + /** + * \brief adding the meshes to a given mesh filter object. + * \param[in,out] mesh_filter mesh filter object that gets meshes from the robot description added to + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void addMeshes(mesh_filter::MeshFilter& mesh_filter); + + /** + * \brief main filtering routine + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void filter(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); + + /** + * \brief Callback for connection/deconnection of listener + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void connectCb(); + + /** + * \brief Callback for subscribed depth images + * \author Suat Gedikli (gedikli@willowgarage.com) + * @param depth_msg depth image + * @param info_msg camera information containing parameters frame, etc. + */ + void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); + +private: + // member variables to handle ros messages + std::shared_ptr input_depth_transport_; + std::shared_ptr filtered_label_transport_; + std::shared_ptr filtered_depth_transport_; + std::shared_ptr model_depth_transport_; + std::shared_ptr model_label_transport_; + image_transport::CameraSubscriber sub_depth_image_; + image_transport::CameraPublisher pub_filtered_depth_image_; + image_transport::CameraPublisher pub_filtered_label_image_; + image_transport::CameraPublisher pub_model_depth_image_; + image_transport::CameraPublisher pub_model_label_image_; + + /** \brief required to avoid listener registration before we are all set*/ + std::mutex connect_mutex_; + TransformProvider transform_provider_; + + std::shared_ptr filtered_depth_ptr_; + std::shared_ptr filtered_label_ptr_; + std::shared_ptr model_depth_ptr_; + std::shared_ptr model_label_ptr_; + /** \brief distance of near clipping plane*/ + double near_clipping_plane_distance_; + + /** \brief distance of far clipping plane*/ + double far_clipping_plane_distance_; + + /** \brief threshold that indicates a pixel to be in shadow, rather than being filtered out */ + double shadow_threshold_; + + /** \brief the coefficient for the square component of padding function in 1/m*/ + double padding_scale_; + + /** \brief the coefficient for the linear component of the padding function*/ + double padding_offset_; + + /** mesh filter object*/ + MeshFilter::Ptr mesh_filter_; +}; + +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h index c2bddecc84..eff3b19d8b 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,106 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include - -namespace mesh_filter -{ -MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc - -/** - * \brief This class is used to execute functions within the thread that holds the OpenGL context. - */ - -class Job -{ -public: - Job() : done_(false) - { - } - virtual ~Job() = default; - - inline void wait() const; - virtual void execute() = 0; - inline void cancel(); - inline bool isDone() const; - -protected: - bool done_; - mutable std::condition_variable condition_; - mutable std::mutex mutex_; -}; - -void Job::wait() const -{ - std::unique_lock lock(mutex_); - while (!done_) - condition_.wait(lock); -} - -void Job::cancel() -{ - std::unique_lock lock(mutex_); - done_ = true; - condition_.notify_all(); -} - -bool Job::isDone() const -{ - return done_; -} - -template -class FilterJob : public Job -{ -public: - FilterJob(const std::function& exec) : Job(), exec_(exec) - { - } - void execute() override; - const ReturnType& getResult() const; - -private: - std::function exec_; - ReturnType result_; -}; - -template -void FilterJob::execute() -{ - std::unique_lock lock(mutex_); - if (!done_) // not canceled ! - result_ = exec_(); - - done_ = true; - condition_.notify_all(); -} - -template -const ReturnType& FilterJob::getResult() const -{ - wait(); - return result_; -} - -template <> -class FilterJob : public Job -{ -public: - FilterJob(const std::function& exec) : Job(), exec_(exec) - { - } - void execute() override - { - std::unique_lock lock(mutex_); - if (!done_) // not canceled ! - exec_(); - - done_ = true; - condition_.notify_all(); - } - -private: - std::function exec_; -}; -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.hpp new file mode 100644 index 0000000000..8cfe5bdeb8 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/filter_job.hpp @@ -0,0 +1,140 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include + +namespace mesh_filter +{ +MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc + +/** + * \brief This class is used to execute functions within the thread that holds the OpenGL context. + */ + +class Job +{ +public: + Job() : done_(false) + { + } + virtual ~Job() = default; + + inline void wait() const; + virtual void execute() = 0; + inline void cancel(); + inline bool isDone() const; + +protected: + bool done_; + mutable std::condition_variable condition_; + mutable std::mutex mutex_; +}; + +void Job::wait() const +{ + std::unique_lock lock(mutex_); + while (!done_) + condition_.wait(lock); +} + +void Job::cancel() +{ + std::unique_lock lock(mutex_); + done_ = true; + condition_.notify_all(); +} + +bool Job::isDone() const +{ + return done_; +} + +template +class FilterJob : public Job +{ +public: + FilterJob(const std::function& exec) : Job(), exec_(exec) + { + } + void execute() override; + const ReturnType& getResult() const; + +private: + std::function exec_; + ReturnType result_; +}; + +template +void FilterJob::execute() +{ + std::unique_lock lock(mutex_); + if (!done_) // not canceled ! + result_ = exec_(); + + done_ = true; + condition_.notify_all(); +} + +template +const ReturnType& FilterJob::getResult() const +{ + wait(); + return result_; +} + +template <> +class FilterJob : public Job +{ +public: + FilterJob(const std::function& exec) : Job(), exec_(exec) + { + } + void execute() override + { + std::unique_lock lock(mutex_); + if (!done_) // not canceled ! + exec_(); + + done_ = true; + condition_.notify_all(); + } + +private: + std::function exec_; +}; +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h index 52bfa8b033..5859bb5cbc 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,51 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include // for Isometry3d -#include -#ifdef __APPLE__ -#include -#else -#include -#endif - -namespace shapes -{ -class Mesh; -} - -namespace mesh_filter -{ -/** - * \brief GLMesh represents a mesh from geometric_shapes for rendering in GL frame buffers - * \author Suat Gedikli (gedikli@willowgarage.com) - */ -class GLMesh -{ -public: - /** - * \brief Constructs a GLMesh object for given mesh and label - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] mesh - * \param[in] mesh_label - */ - GLMesh(const shapes::Mesh& mesh, unsigned int mesh_label); - - /** \brief Destructor*/ - ~GLMesh(); - /** - * \brief renders the mesh in current OpenGL frame buffer (context) - * \param[in] transform the modelview transformation describing the pose of the mesh in camera coordinate frame - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void render(const Eigen::Isometry3d& transform) const; - -private: - /** \brief the OpenGL mesh represented as a OpenGL list */ - GLuint list_; - - /** \brief label of current mesh*/ - unsigned int mesh_label_; -}; -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.hpp new file mode 100644 index 0000000000..52bfa8b033 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_mesh.hpp @@ -0,0 +1,85 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include // for Isometry3d +#include +#ifdef __APPLE__ +#include +#else +#include +#endif + +namespace shapes +{ +class Mesh; +} + +namespace mesh_filter +{ +/** + * \brief GLMesh represents a mesh from geometric_shapes for rendering in GL frame buffers + * \author Suat Gedikli (gedikli@willowgarage.com) + */ +class GLMesh +{ +public: + /** + * \brief Constructs a GLMesh object for given mesh and label + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] mesh + * \param[in] mesh_label + */ + GLMesh(const shapes::Mesh& mesh, unsigned int mesh_label); + + /** \brief Destructor*/ + ~GLMesh(); + /** + * \brief renders the mesh in current OpenGL frame buffer (context) + * \param[in] transform the modelview transformation describing the pose of the mesh in camera coordinate frame + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void render(const Eigen::Isometry3d& transform) const; + +private: + /** \brief the OpenGL mesh represented as a OpenGL list */ + GLuint list_; + + /** \brief label of current mesh*/ + unsigned int mesh_label_; +}; +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 64b8dadfca..eafb2cf27a 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,275 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include -#ifdef __APPLE__ -#include -#else -#include -#endif -#include -#include -#include -#include - -#undef near -#undef far - -namespace mesh_filter -{ -MOVEIT_CLASS_FORWARD(GLRenderer); // Defines GLRendererPtr, ConstPtr, WeakPtr... etc - -/** \brief Abstracts the OpenGL frame buffer objects, and provides an interface to render meshes, and retrieve the color - * and depth ap from opengl. - * \author Suat Gedikli (gedikli@willowgarage.com) - */ -class GLRenderer -{ -public: - /** - * \brief constructs the frame buffer object in a new OpenGL context. - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] width the width of the frame buffers - * \param[in] height height of the framebuffers - * \param[in] near distance of the near clipping plane in meters - * \param[in] far distance of the far clipping plane in meters - */ - GLRenderer(unsigned width, unsigned height, double near = 0.1, double far = 10.0); - - /** \brief destructor, destroys frame buffer objects and OpenGL context*/ - ~GLRenderer(); - - /** - * \brief initializes the frame buffers for rendering and or manipulating - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void begin() const; - - /** - * \brief finalizes the frame buffers after rendering and/or manipulating - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void end() const; - - /** - * \brief executes a OpenGL list - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param [in] list the handle of the OpenGL list to be executed - */ - void callList(GLuint list) const; - - /** - * \brief retrieves the color buffer from OpenGL - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[out] buffer pointer to memory where the color values need to be stored - */ - void getColorBuffer(unsigned char* buffer) const; - - /** - * \brief retrieves the depth buffer from OpenGL - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[out] buffer pointer to memory where the depth values need to be stored - */ - void getDepthBuffer(float* buffer) const; - - /** - * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context. - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] vertex_filename path to vertex shader source code. Can set to "" (empty string) if no vertex shader is - * used. - * \param[in] fragment_filename path to fragemnt shader source code. Can be set to "" if no fragment shader is used. - * \return the programID - */ - GLuint setShadersFromFile(const std::string& vertex_filename, const std::string& fragment_filename); - - /** - * \brief loads, compiles, links and adds GLSL shaders from string to the current OpenGL context. - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] vertex_shader source code of the vertex shader. Can be "" if no vertex shader is used. - * \param[in] fragment_shader source code of the fragment shader. Can be "" if no fragment shader is used. - * \return programID - */ - GLuint setShadersFromString(const std::string& vertex_shader, const std::string& fragment_shader); - - /** - * \brief set the camera parameters - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] fx focal length in x-direction - * \param[in] fy focal length in y-direction - * \param[in] cx x component of principal point - * \param[in] cy y component of principal point - */ - void setCameraParameters(double fx, double fy, double cx, double cy); - - /** - * \brief sets the near and far clipping plane distances in meters - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] near distance of the near clipping plane in meters - * \param[in] far distance of the far clipping plane in meters - */ - void setClippingRange(double near, double far); - - /** - * \brief returns the distance of the near clipping plane in meters - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return distance of near clipping plane in meters - */ - const double& getNearClippingDistance() const; - - /** - * \brief returns the distance of the far clipping plane in meters - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return distance of the far clipping plane in meters - */ - const double& getFarClippingDistance() const; - - /** - * \brief returns the width of the frame buffer objectsin pixels - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return width of frame buffer in pixels - */ - unsigned getWidth() const; - - /** - * \brief returns the height of the frame buffer objects in pixels - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return height of frame buffer in pixels - */ - unsigned getHeight() const; - - /** - * \brief set the size of frame buffers - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] width width of frame buffer in pixels - * \param[in] height height of frame buffer in pixels - */ - void setBufferSize(unsigned width, unsigned height); - - /** - * \returns the current programID - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return current porgamID. 0 if no shaders are used. - */ - const GLuint& getProgramID() const; - - /** - * \brief returns the handle of the depth buffer as an OpenGL texture object - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return handle of the OpenGL texture object for the depth buffer - */ - GLuint getDepthTexture() const; - - /** - * \brief returns the handle of the color buffer as an OpenGL texture object - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return handle of the OpenGL texture object for the color buffer - */ - GLuint getColorTexture() const; - -private: - /** - * \brief sets the OpenGL camera parameters - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void setCameraParameters() const; - - /** - * \brief reads shader source code from file to a string - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] filename path to file containing the shader source code - * \param[out] source string to be filled with the shader source code - */ - void readShaderCodeFromFile(const std::string& filename, std::string& source) const; - - /** - * \brief Compiles, Links and adds the GLSL shaders from strings containing the source codes - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] vertex_source string containing the vertex shader source code - * \param[in] fragment_source string containing the fragment shader source code - * \return programID - */ - GLuint loadShaders(const std::string& vertex_source, const std::string& fragment_source) const; - - /** - * \brief create a OpenGL shader object from the shader source code - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] shaderID type of shader to be created (e.g. GL_VERTEX_SHADER or GL_FRAGMENT_SHADER) - * \param[in] source the source code of the shader to be created - * \return handle to the shader object - */ - GLuint createShader(GLuint shaderID, const std::string& source) const; - - /** - * \brief initializes the frame buffer objects - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void initFrameBuffers(); - - /** - * \brief deletes the frame buffer objects - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void deleteFrameBuffers(); - - /** - * \brief create the OpenGL context if required. Only on context is created for each thread - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - static void createGLContext(); - - /** - * \brief deletes OpenGL context for the current thread - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - static void deleteGLContext(); - - /** \brief width of frame buffer objects in pixels*/ - unsigned width_; - - /** \brief height of frame buffer objects in pixels*/ - unsigned height_; - - /** \brief handle to frame buffer object*/ - GLuint fbo_id_; - - /** \brief handle to render buffer object*/ - GLuint rbo_id_; - - /** \brief handle to color buffer*/ - GLuint rgb_id_; - - /** \brief handle to depth buffer*/ - GLuint depth_id_; - - /** \brief handle to program that is currently used*/ - GLuint program_; - - /** \brief distance of near clipping plane in meters*/ - double near_; - - /** \brief distance of far clipping plane in meters*/ - double far_; - - /** \brief focal length in x-direction of camera in pixels*/ - float fx_; - - /** \brief focal length in y-direction of camera in pixels*/ - float fy_; - - /** \brief x-coordinate of principal point of camera in pixels*/ - float cx_; - - /** \brief y-coordinate of principal point of camera in pixels*/ - float cy_; - - /** \brief map from thread id to OpenGL context */ - static std::map > s_context; - - /* \brief lock for context map */ - static std::mutex s_context_lock; - - static bool s_glut_initialized; -}; -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.hpp new file mode 100644 index 0000000000..def2a56f86 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.hpp @@ -0,0 +1,309 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include +#ifdef __APPLE__ +#include +#else +#include +#endif +#include +#include +#include +#include + +#undef near +#undef far + +namespace mesh_filter +{ +MOVEIT_CLASS_FORWARD(GLRenderer); // Defines GLRendererPtr, ConstPtr, WeakPtr... etc + +/** \brief Abstracts the OpenGL frame buffer objects, and provides an interface to render meshes, and retrieve the color + * and depth ap from opengl. + * \author Suat Gedikli (gedikli@willowgarage.com) + */ +class GLRenderer +{ +public: + /** + * \brief constructs the frame buffer object in a new OpenGL context. + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] width the width of the frame buffers + * \param[in] height height of the framebuffers + * \param[in] near distance of the near clipping plane in meters + * \param[in] far distance of the far clipping plane in meters + */ + GLRenderer(unsigned width, unsigned height, double near = 0.1, double far = 10.0); + + /** \brief destructor, destroys frame buffer objects and OpenGL context*/ + ~GLRenderer(); + + /** + * \brief initializes the frame buffers for rendering and or manipulating + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void begin() const; + + /** + * \brief finalizes the frame buffers after rendering and/or manipulating + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void end() const; + + /** + * \brief executes a OpenGL list + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param [in] list the handle of the OpenGL list to be executed + */ + void callList(GLuint list) const; + + /** + * \brief retrieves the color buffer from OpenGL + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[out] buffer pointer to memory where the color values need to be stored + */ + void getColorBuffer(unsigned char* buffer) const; + + /** + * \brief retrieves the depth buffer from OpenGL + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[out] buffer pointer to memory where the depth values need to be stored + */ + void getDepthBuffer(float* buffer) const; + + /** + * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context. + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] vertex_filename path to vertex shader source code. Can set to "" (empty string) if no vertex shader is + * used. + * \param[in] fragment_filename path to fragemnt shader source code. Can be set to "" if no fragment shader is used. + * \return the programID + */ + GLuint setShadersFromFile(const std::string& vertex_filename, const std::string& fragment_filename); + + /** + * \brief loads, compiles, links and adds GLSL shaders from string to the current OpenGL context. + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] vertex_shader source code of the vertex shader. Can be "" if no vertex shader is used. + * \param[in] fragment_shader source code of the fragment shader. Can be "" if no fragment shader is used. + * \return programID + */ + GLuint setShadersFromString(const std::string& vertex_shader, const std::string& fragment_shader); + + /** + * \brief set the camera parameters + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] fx focal length in x-direction + * \param[in] fy focal length in y-direction + * \param[in] cx x component of principal point + * \param[in] cy y component of principal point + */ + void setCameraParameters(double fx, double fy, double cx, double cy); + + /** + * \brief sets the near and far clipping plane distances in meters + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] near distance of the near clipping plane in meters + * \param[in] far distance of the far clipping plane in meters + */ + void setClippingRange(double near, double far); + + /** + * \brief returns the distance of the near clipping plane in meters + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return distance of near clipping plane in meters + */ + const double& getNearClippingDistance() const; + + /** + * \brief returns the distance of the far clipping plane in meters + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return distance of the far clipping plane in meters + */ + const double& getFarClippingDistance() const; + + /** + * \brief returns the width of the frame buffer objectsin pixels + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return width of frame buffer in pixels + */ + unsigned getWidth() const; + + /** + * \brief returns the height of the frame buffer objects in pixels + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return height of frame buffer in pixels + */ + unsigned getHeight() const; + + /** + * \brief set the size of frame buffers + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] width width of frame buffer in pixels + * \param[in] height height of frame buffer in pixels + */ + void setBufferSize(unsigned width, unsigned height); + + /** + * \returns the current programID + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return current porgamID. 0 if no shaders are used. + */ + const GLuint& getProgramID() const; + + /** + * \brief returns the handle of the depth buffer as an OpenGL texture object + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return handle of the OpenGL texture object for the depth buffer + */ + GLuint getDepthTexture() const; + + /** + * \brief returns the handle of the color buffer as an OpenGL texture object + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return handle of the OpenGL texture object for the color buffer + */ + GLuint getColorTexture() const; + +private: + /** + * \brief sets the OpenGL camera parameters + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void setCameraParameters() const; + + /** + * \brief reads shader source code from file to a string + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] filename path to file containing the shader source code + * \param[out] source string to be filled with the shader source code + */ + void readShaderCodeFromFile(const std::string& filename, std::string& source) const; + + /** + * \brief Compiles, Links and adds the GLSL shaders from strings containing the source codes + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] vertex_source string containing the vertex shader source code + * \param[in] fragment_source string containing the fragment shader source code + * \return programID + */ + GLuint loadShaders(const std::string& vertex_source, const std::string& fragment_source) const; + + /** + * \brief create a OpenGL shader object from the shader source code + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] shaderID type of shader to be created (e.g. GL_VERTEX_SHADER or GL_FRAGMENT_SHADER) + * \param[in] source the source code of the shader to be created + * \return handle to the shader object + */ + GLuint createShader(GLuint shaderID, const std::string& source) const; + + /** + * \brief initializes the frame buffer objects + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void initFrameBuffers(); + + /** + * \brief deletes the frame buffer objects + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void deleteFrameBuffers(); + + /** + * \brief create the OpenGL context if required. Only on context is created for each thread + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + static void createGLContext(); + + /** + * \brief deletes OpenGL context for the current thread + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + static void deleteGLContext(); + + /** \brief width of frame buffer objects in pixels*/ + unsigned width_; + + /** \brief height of frame buffer objects in pixels*/ + unsigned height_; + + /** \brief handle to frame buffer object*/ + GLuint fbo_id_; + + /** \brief handle to render buffer object*/ + GLuint rbo_id_; + + /** \brief handle to color buffer*/ + GLuint rgb_id_; + + /** \brief handle to depth buffer*/ + GLuint depth_id_; + + /** \brief handle to program that is currently used*/ + GLuint program_; + + /** \brief distance of near clipping plane in meters*/ + double near_; + + /** \brief distance of far clipping plane in meters*/ + double far_; + + /** \brief focal length in x-direction of camera in pixels*/ + float fx_; + + /** \brief focal length in y-direction of camera in pixels*/ + float fy_; + + /** \brief x-coordinate of principal point of camera in pixels*/ + float cx_; + + /** \brief y-coordinate of principal point of camera in pixels*/ + float cy_; + + /** \brief map from thread id to OpenGL context */ + static std::map > s_context; + + /* \brief lock for context map */ + static std::mutex s_context_lock; + + static bool s_glut_initialized; +}; +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h index abe6bb27b9..d663157e8a 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,77 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include -#include -#include - -// forward declarations -namespace shapes -{ -class Mesh; -} - -namespace mesh_filter -{ -class GLMesh; - -/** - * \brief MeshFilter filters out points that belong to given meshes in depth-images - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - -template -class MeshFilter : public MeshFilterBase -{ -public: - MOVEIT_DECLARE_PTR_MEMBER(MeshFilter); - - /** - * \brief Constructor - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] transform_callback Callback function that is called for each mesh to obtain the current transformation. - * \note the callback expects the mesh handle but no time stamp. Its the users responsibility to return the correct - * transformation. - */ - MeshFilter(const TransformCallback& transform_callback = TransformCallback(), - const typename SensorType::Parameters& sensor_parameters = typename SensorType::Parameters()); - - /** - * \brief returns the Sensor Parameters - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return reference of the parameters object of the used Sensor - */ - typename SensorType::Parameters& parameters(); - - /** - * \brief returns the Sensor Parameters - * \author Suat Gedikli (gedikli@willowgarage.com) - * \return const reference of the parameters object of the used Sensor - */ - const typename SensorType::Parameters& parameters() const; -}; - -template -MeshFilter::MeshFilter(const TransformCallback& transform_callback, - const typename SensorType::Parameters& sensor_parameters) - : MeshFilterBase(transform_callback, sensor_parameters, SensorType::RENDER_VERTEX_SHADER_SOURCE, - SensorType::RENDER_FRAGMENT_SHADER_SOURCE, SensorType::FILTER_VERTEX_SHADER_SOURCE, - SensorType::FILTER_FRAGMENT_SHADER_SOURCE) -{ -} - -template -typename SensorType::Parameters& MeshFilter::parameters() -{ - return static_cast(*sensor_parameters_); -} - -template -const typename SensorType::Parameters& MeshFilter::parameters() const -{ - return static_cast(*sensor_parameters_); -} - -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.hpp new file mode 100644 index 0000000000..37448b09c7 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter.hpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include +#include +#include + +// forward declarations +namespace shapes +{ +class Mesh; +} + +namespace mesh_filter +{ +class GLMesh; + +/** + * \brief MeshFilter filters out points that belong to given meshes in depth-images + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + +template +class MeshFilter : public MeshFilterBase +{ +public: + MOVEIT_DECLARE_PTR_MEMBER(MeshFilter); + + /** + * \brief Constructor + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] transform_callback Callback function that is called for each mesh to obtain the current transformation. + * \note the callback expects the mesh handle but no time stamp. Its the users responsibility to return the correct + * transformation. + */ + MeshFilter(const TransformCallback& transform_callback = TransformCallback(), + const typename SensorType::Parameters& sensor_parameters = typename SensorType::Parameters()); + + /** + * \brief returns the Sensor Parameters + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return reference of the parameters object of the used Sensor + */ + typename SensorType::Parameters& parameters(); + + /** + * \brief returns the Sensor Parameters + * \author Suat Gedikli (gedikli@willowgarage.com) + * \return const reference of the parameters object of the used Sensor + */ + const typename SensorType::Parameters& parameters() const; +}; + +template +MeshFilter::MeshFilter(const TransformCallback& transform_callback, + const typename SensorType::Parameters& sensor_parameters) + : MeshFilterBase(transform_callback, sensor_parameters, SensorType::RENDER_VERTEX_SHADER_SOURCE, + SensorType::RENDER_FRAGMENT_SHADER_SOURCE, SensorType::FILTER_VERTEX_SHADER_SOURCE, + SensorType::FILTER_FRAGMENT_SHADER_SOURCE) +{ +} + +template +typename SensorType::Parameters& MeshFilter::parameters() +{ + return static_cast(*sensor_parameters_); +} + +template +const typename SensorType::Parameters& MeshFilter::parameters() const +{ + return static_cast(*sensor_parameters_); +} + +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 879a9d87bc..2ae3774d69 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,262 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include -#include -#include -#include // for Isometry3d -#include -#include -#include -#include - -// forward declarations -namespace shapes -{ -class Mesh; -} - -namespace mesh_filter -{ -MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(GLMesh); // Defines GLMeshPtr, ConstPtr, WeakPtr... etc - -typedef unsigned int MeshHandle; -typedef uint32_t LabelType; - -class MeshFilterBase -{ - // inner types and typedefs -public: - typedef std::function TransformCallback; - // \todo @suat: to avoid a few comparisons, it would be much nicer if background = 14 and shadow = 15 (near/far clip - // can be anything below that) - // this would allow me to do a single comparison instead of 3, in the code i write - enum - { - BACKGROUND = 0, - SHADOW = 1, - NEAR_CLIP = 2, - FAR_CLIP = 3, - FIRST_LABEL = 16 - }; - -public: - /** - * \brief Constructor - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] transform_callback Callback function that is called for each mesh to obtain the current transformation. - * \note the callback expects the mesh handle but no time stamp. Its the users responsibility to return the correct - * transformation. - */ - MeshFilterBase(const TransformCallback& transform_callback, const SensorModel::Parameters& sensor_parameters, - const std::string& render_vertex_shader = "", const std::string& render_fragment_shader = "", - const std::string& filter_vertex_shader = "", const std::string& filter_fragment_shader = ""); - - /** \brief Destructor */ - ~MeshFilterBase(); - - /** - * \brief adds a mesh to the filter object. - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] mesh the mesh to be added - * \return handle to the mesh. This handle is used in the transform callback function to identify the mesh and - * retrieve the correct transformation. - */ - MeshHandle addMesh(const shapes::Mesh& mesh); - - /** - * \brief removes a mesh given by its handle - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] mesh_handle the handle of the mesh to be removed. - */ - void removeMesh(MeshHandle mesh_handle); - - /** - * \brief label/remove pixels from input depth-image - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] sensor_data pointer to the input depth image from sensor readings. - * \todo what is type? - */ - void filter(const void* sensor_data, GLushort type, bool wait = false) const; - - /** - * \brief retrieves the labels of the input data - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[out] labels pointer to buffer to be filled with labels - * \note labels are corresponding 1-1 to the mesh handles. 0 and 1 are reserved indicating either background (0) or - * shadow (1) - * The upper 8bit of a label is filled with the user given flag (see addMesh) - */ - void getFilteredLabels(LabelType* labels) const; - - /** - * \brief retrieves the filtered depth values - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[out] depth pointer to buffer to be filled with depth values. - */ - void getFilteredDepth(float* depth) const; - - /** - * \brief retrieves the labels of the rendered model - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[out] labels pointer to buffer to be filled with labels - * \note labels are corresponding 1-1 to the mesh handles. 0 and 1 are reserved indicating either background (0) or - * shadow (1) - * The upper 8bit of a label is filled with the user given flag (see addMesh) - * \todo How is this data different from the filtered labels? - */ - void getModelLabels(LabelType* labels) const; - - /** - * \brief retrieves the depth values of the rendered model - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[out] depth pointer to buffer to be filled with depth values. - */ - void getModelDepth(float* depth) const; - - /** - * \brief set the shadow threshold. points that are further away than the rendered model are filtered out. - * Except they are further away than this threshold. Then these points are kept, but its label is set to - * 1 indicating that it is in the shadow of the model - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] threshold shadow threshold in meters - */ - void setShadowThreshold(float threshold); - - /** - * \brief set the callback for retrieving transformations for each mesh. - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] transform_callback the callback - */ - void setTransformCallback(const TransformCallback& transform_callback); - - /** - * \brief set the scale component of padding used to multiply with sensor-specific padding coefficients to get final - * coefficients. - * \param[in] scale the scale value - */ - void setPaddingScale(float scale); - - /** - * \brief set the offset component of padding. This value is added to the scaled sensor-specific constant component. - * \param[in] offset the offset value - */ - void setPaddingOffset(float offset); - -protected: - /** - * \brief initializes OpenGL related things as well as renderers - */ - void initialize(const std::string& render_vertex_shader, const std::string& render_fragment_shader, - const std::string& filter_vertex_shader, const std::string& filter_fragment_shader); - - /** - * \brief cleaning up - */ - void deInitialize(); - - /** - * \brief filtering thread - */ - void run(const std::string& render_vertex_shader, const std::string& render_fragment_shader, - const std::string& filter_vertex_shader, const std::string& filter_fragment_shader); - - /** - * \brief the filter method that does the magic - * \param[in] sensor_data pointer to the buffer containing the depth readings - * \param[in] encoding the representation of the depth readings in the buffer - */ - void doFilter(const void* sensor_data, const int encoding) const; - - /** - * \brief used within a Job to allow the main thread adding meshes - * \param[in] handle the handle of the mesh that is predetermined and passed - * \param[in] cmesh the mesh to be added to the corresponding handle - */ - void addMeshHelper(MeshHandle handle, const shapes::Mesh& cmesh); - - /** - * \brief used within a Job to allow the main thread removing meshes - * \param[in] handle the handle of the mesh to be removed - */ - bool removeMeshHelper(MeshHandle handle); - - /** - * \brief add a Job for the main thread that needs to be executed there - * \param[in] job the job object that has the function o be executed - */ - void addJob(const JobPtr& job) const; - - /** - * \brief sets the size of the frame buffers - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] width width of frame buffers in pixels - * \param[in] height height of frame buffers in pixels - */ - void setSize(unsigned int width, unsigned int height); - - /** \brief storage for meshed to be filtered */ - std::map meshes_; - - /** \brief the parameters of the used sensor model*/ - SensorModel::ParametersPtr sensor_parameters_; - - /** \brief next handle to be used for next mesh that is added*/ - MeshHandle next_handle_; - - /** \brief Handle values below this are all taken (this variable is used for more efficient computation of - * next_label_) */ - MeshHandle min_handle_; - - /** \brief the filtering thread that also holds the OpenGL context*/ - std::thread filter_thread_; - - /** \brief condition variable to notify the filtering thread if a new image arrived*/ - mutable std::condition_variable jobs_condition_; - - /** \brief mutex required for synchronization of condition states*/ - mutable std::mutex jobs_mutex_; - - /** \brief OpenGL job queue that need to be processed by the worker thread*/ - mutable std::queue jobs_queue_; - - /** \brief mutex for synchronization of updating filtered meshes */ - mutable std::mutex meshes_mutex_; - - /** \brief mutex for synchronization of setting/calling transform_callback_ */ - mutable std::mutex transform_callback_mutex_; - - /** \brief indicates whether the filtering loop should stop*/ - bool stop_; - - /** \brief first pass renderer for rendering the mesh*/ - GLRendererPtr mesh_renderer_; - - /** \brief second pass renderer for filtering the results of first pass*/ - GLRendererPtr depth_filter_; - - /** \brief canvas element (screen-filling quad) for second pass*/ - GLuint canvas_; - - /** \brief handle depth texture from sensor data*/ - GLuint sensor_depth_texture_; - - /** \brief handle to GLSL location of shadow threshold*/ - GLuint shadow_threshold_location_; - - /** \brief callback function for retrieving the mesh transformations*/ - TransformCallback transform_callback_; - - /** \brief padding scale*/ - float padding_scale_; - - /** \brief padding offset*/ - float padding_offset_; - - /** \brief threshold for shadowed pixels vs. filtered pixels*/ - float shadow_threshold_; -}; -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.hpp new file mode 100644 index 0000000000..c6aa33bfb1 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.hpp @@ -0,0 +1,296 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include +#include +#include +#include // for Isometry3d +#include +#include +#include +#include + +// forward declarations +namespace shapes +{ +class Mesh; +} + +namespace mesh_filter +{ +MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(GLMesh); // Defines GLMeshPtr, ConstPtr, WeakPtr... etc + +typedef unsigned int MeshHandle; +typedef uint32_t LabelType; + +class MeshFilterBase +{ + // inner types and typedefs +public: + typedef std::function TransformCallback; + // \todo @suat: to avoid a few comparisons, it would be much nicer if background = 14 and shadow = 15 (near/far clip + // can be anything below that) + // this would allow me to do a single comparison instead of 3, in the code i write + enum + { + BACKGROUND = 0, + SHADOW = 1, + NEAR_CLIP = 2, + FAR_CLIP = 3, + FIRST_LABEL = 16 + }; + +public: + /** + * \brief Constructor + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] transform_callback Callback function that is called for each mesh to obtain the current transformation. + * \note the callback expects the mesh handle but no time stamp. Its the users responsibility to return the correct + * transformation. + */ + MeshFilterBase(const TransformCallback& transform_callback, const SensorModel::Parameters& sensor_parameters, + const std::string& render_vertex_shader = "", const std::string& render_fragment_shader = "", + const std::string& filter_vertex_shader = "", const std::string& filter_fragment_shader = ""); + + /** \brief Destructor */ + ~MeshFilterBase(); + + /** + * \brief adds a mesh to the filter object. + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] mesh the mesh to be added + * \return handle to the mesh. This handle is used in the transform callback function to identify the mesh and + * retrieve the correct transformation. + */ + MeshHandle addMesh(const shapes::Mesh& mesh); + + /** + * \brief removes a mesh given by its handle + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] mesh_handle the handle of the mesh to be removed. + */ + void removeMesh(MeshHandle mesh_handle); + + /** + * \brief label/remove pixels from input depth-image + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] sensor_data pointer to the input depth image from sensor readings. + * \todo what is type? + */ + void filter(const void* sensor_data, GLushort type, bool wait = false) const; + + /** + * \brief retrieves the labels of the input data + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[out] labels pointer to buffer to be filled with labels + * \note labels are corresponding 1-1 to the mesh handles. 0 and 1 are reserved indicating either background (0) or + * shadow (1) + * The upper 8bit of a label is filled with the user given flag (see addMesh) + */ + void getFilteredLabels(LabelType* labels) const; + + /** + * \brief retrieves the filtered depth values + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[out] depth pointer to buffer to be filled with depth values. + */ + void getFilteredDepth(float* depth) const; + + /** + * \brief retrieves the labels of the rendered model + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[out] labels pointer to buffer to be filled with labels + * \note labels are corresponding 1-1 to the mesh handles. 0 and 1 are reserved indicating either background (0) or + * shadow (1) + * The upper 8bit of a label is filled with the user given flag (see addMesh) + * \todo How is this data different from the filtered labels? + */ + void getModelLabels(LabelType* labels) const; + + /** + * \brief retrieves the depth values of the rendered model + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[out] depth pointer to buffer to be filled with depth values. + */ + void getModelDepth(float* depth) const; + + /** + * \brief set the shadow threshold. points that are further away than the rendered model are filtered out. + * Except they are further away than this threshold. Then these points are kept, but its label is set to + * 1 indicating that it is in the shadow of the model + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] threshold shadow threshold in meters + */ + void setShadowThreshold(float threshold); + + /** + * \brief set the callback for retrieving transformations for each mesh. + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] transform_callback the callback + */ + void setTransformCallback(const TransformCallback& transform_callback); + + /** + * \brief set the scale component of padding used to multiply with sensor-specific padding coefficients to get final + * coefficients. + * \param[in] scale the scale value + */ + void setPaddingScale(float scale); + + /** + * \brief set the offset component of padding. This value is added to the scaled sensor-specific constant component. + * \param[in] offset the offset value + */ + void setPaddingOffset(float offset); + +protected: + /** + * \brief initializes OpenGL related things as well as renderers + */ + void initialize(const std::string& render_vertex_shader, const std::string& render_fragment_shader, + const std::string& filter_vertex_shader, const std::string& filter_fragment_shader); + + /** + * \brief cleaning up + */ + void deInitialize(); + + /** + * \brief filtering thread + */ + void run(const std::string& render_vertex_shader, const std::string& render_fragment_shader, + const std::string& filter_vertex_shader, const std::string& filter_fragment_shader); + + /** + * \brief the filter method that does the magic + * \param[in] sensor_data pointer to the buffer containing the depth readings + * \param[in] encoding the representation of the depth readings in the buffer + */ + void doFilter(const void* sensor_data, const int encoding) const; + + /** + * \brief used within a Job to allow the main thread adding meshes + * \param[in] handle the handle of the mesh that is predetermined and passed + * \param[in] cmesh the mesh to be added to the corresponding handle + */ + void addMeshHelper(MeshHandle handle, const shapes::Mesh& cmesh); + + /** + * \brief used within a Job to allow the main thread removing meshes + * \param[in] handle the handle of the mesh to be removed + */ + bool removeMeshHelper(MeshHandle handle); + + /** + * \brief add a Job for the main thread that needs to be executed there + * \param[in] job the job object that has the function o be executed + */ + void addJob(const JobPtr& job) const; + + /** + * \brief sets the size of the frame buffers + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] width width of frame buffers in pixels + * \param[in] height height of frame buffers in pixels + */ + void setSize(unsigned int width, unsigned int height); + + /** \brief storage for meshed to be filtered */ + std::map meshes_; + + /** \brief the parameters of the used sensor model*/ + SensorModel::ParametersPtr sensor_parameters_; + + /** \brief next handle to be used for next mesh that is added*/ + MeshHandle next_handle_; + + /** \brief Handle values below this are all taken (this variable is used for more efficient computation of + * next_label_) */ + MeshHandle min_handle_; + + /** \brief the filtering thread that also holds the OpenGL context*/ + std::thread filter_thread_; + + /** \brief condition variable to notify the filtering thread if a new image arrived*/ + mutable std::condition_variable jobs_condition_; + + /** \brief mutex required for synchronization of condition states*/ + mutable std::mutex jobs_mutex_; + + /** \brief OpenGL job queue that need to be processed by the worker thread*/ + mutable std::queue jobs_queue_; + + /** \brief mutex for synchronization of updating filtered meshes */ + mutable std::mutex meshes_mutex_; + + /** \brief mutex for synchronization of setting/calling transform_callback_ */ + mutable std::mutex transform_callback_mutex_; + + /** \brief indicates whether the filtering loop should stop*/ + bool stop_; + + /** \brief first pass renderer for rendering the mesh*/ + GLRendererPtr mesh_renderer_; + + /** \brief second pass renderer for filtering the results of first pass*/ + GLRendererPtr depth_filter_; + + /** \brief canvas element (screen-filling quad) for second pass*/ + GLuint canvas_; + + /** \brief handle depth texture from sensor data*/ + GLuint sensor_depth_texture_; + + /** \brief handle to GLSL location of shadow threshold*/ + GLuint shadow_threshold_location_; + + /** \brief callback function for retrieving the mesh transformations*/ + TransformCallback transform_callback_; + + /** \brief padding scale*/ + float padding_scale_; + + /** \brief padding offset*/ + float padding_offset_; + + /** \brief threshold for shadowed pixels vs. filtered pixels*/ + float shadow_threshold_; +}; +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 607fb663c0..2964c99bd6 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,137 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include // for Vector3f - -namespace mesh_filter -{ -// forward declarations -class GLRenderer; - -/** - * \brief Abstract Interface defining a sensor model for mesh filtering - * \author Suat Gedikli - */ -class SensorModel -{ -public: - MOVEIT_CLASS_FORWARD(Parameters); // Defines ParametersPtr, ConstPtr, WeakPtr... etc - - /** - * \brief Abstract Interface defining Sensor Parameters. - * \author Suat Gedikli - */ - class Parameters - { - public: - /** - * \brief Constructor taking core parameters that are required for all sensors - * \param width width of the image generated by this kind of sensor - * \param height height of the image generated by this kind of sensors - * \param near_clipping_plane_distance distance of the near clipping plane in meters - * \param far_clipping_plane_distance distance of the far clipping plane in meters - */ - Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance); - - /** \brief virtual destructor*/ - virtual ~Parameters(); - - /** - * \brief method that sets required parameters for the renderer. - * Each sensor usually has its own shaders with specific parameters depending on sensor parameters. - * This method is called within MeshFilter before any rendering/filtering is done to set any changed - * sensor parameters in the shader code. - * \param renderer the renderer that needs to be updated - */ - virtual void setRenderParameters(GLRenderer& renderer) const = 0; - - /** - * \brief sets the specific Filter Renderer parameters - * \param renderer renderer the renderer that needs to be updated - */ - virtual void setFilterParameters(GLRenderer& renderer) const = 0; - - /** - * \brief polymorphic clone method - * \return clones object as base class - */ - virtual Parameters* clone() const = 0; - - /** - * \brief returns sensor dependent padding coefficients - * \return returns sensor dependent padding coefficients - */ - virtual const Eigen::Vector3f& getPaddingCoefficients() const = 0; - - /** - * \brief transforms depth values from rendered model to metric depth values - * \param[in,out] depth pointer to floating point depth buffer - */ - virtual void transformModelDepthToMetricDepth(float* depth) const; - - /** - * \brief transforms depth values from filtered depth to metric depth values - * \param[in,out] depth pointer to floating point depth buffer - */ - virtual void transformFilteredDepthToMetricDepth(float* depth) const; - - /** - * \brief sets the image size - * \param[in] width with of depth map - * \param[in] height height of depth map - */ - void setImageSize(unsigned width, unsigned height); - - /** - * \brief sets the clipping range - * \param[in] near distance of near clipping plane - * \param[in] far distance of far clipping plane - */ - void setDepthRange(float near, float far); - - /** - * \brief returns the width of depth maps - * \return width of the depth map - */ - unsigned getWidth() const; - - /** - * \brief returns the height of depth maps - * \return height of the depth map - */ - unsigned getHeight() const; - - /** - * \brief returns distance to the near clipping plane - * \return distance to near clipping plane - */ - float getNearClippingPlaneDistance() const; - - /** - * \brief returns the distance to the far clipping plane - * \return distance to far clipping plane - */ - float getFarClippingPlaneDistance() const; - - protected: - /** \brief width of depth maps generated by the sensor*/ - unsigned width_; - - /** \brief height of depth maps generated by the sensor*/ - unsigned height_; - - /** \brief distance of far clipping plane*/ - float far_clipping_plane_distance_; - - /** \brief distance of near clipping plane*/ - float near_clipping_plane_distance_; - }; - - /** - * \brief virtual destructor - */ - virtual ~SensorModel(); -}; -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.hpp new file mode 100644 index 0000000000..d28b989435 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.hpp @@ -0,0 +1,171 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include // for Vector3f + +namespace mesh_filter +{ +// forward declarations +class GLRenderer; + +/** + * \brief Abstract Interface defining a sensor model for mesh filtering + * \author Suat Gedikli + */ +class SensorModel +{ +public: + MOVEIT_CLASS_FORWARD(Parameters); // Defines ParametersPtr, ConstPtr, WeakPtr... etc + + /** + * \brief Abstract Interface defining Sensor Parameters. + * \author Suat Gedikli + */ + class Parameters + { + public: + /** + * \brief Constructor taking core parameters that are required for all sensors + * \param width width of the image generated by this kind of sensor + * \param height height of the image generated by this kind of sensors + * \param near_clipping_plane_distance distance of the near clipping plane in meters + * \param far_clipping_plane_distance distance of the far clipping plane in meters + */ + Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance); + + /** \brief virtual destructor*/ + virtual ~Parameters(); + + /** + * \brief method that sets required parameters for the renderer. + * Each sensor usually has its own shaders with specific parameters depending on sensor parameters. + * This method is called within MeshFilter before any rendering/filtering is done to set any changed + * sensor parameters in the shader code. + * \param renderer the renderer that needs to be updated + */ + virtual void setRenderParameters(GLRenderer& renderer) const = 0; + + /** + * \brief sets the specific Filter Renderer parameters + * \param renderer renderer the renderer that needs to be updated + */ + virtual void setFilterParameters(GLRenderer& renderer) const = 0; + + /** + * \brief polymorphic clone method + * \return clones object as base class + */ + virtual Parameters* clone() const = 0; + + /** + * \brief returns sensor dependent padding coefficients + * \return returns sensor dependent padding coefficients + */ + virtual const Eigen::Vector3f& getPaddingCoefficients() const = 0; + + /** + * \brief transforms depth values from rendered model to metric depth values + * \param[in,out] depth pointer to floating point depth buffer + */ + virtual void transformModelDepthToMetricDepth(float* depth) const; + + /** + * \brief transforms depth values from filtered depth to metric depth values + * \param[in,out] depth pointer to floating point depth buffer + */ + virtual void transformFilteredDepthToMetricDepth(float* depth) const; + + /** + * \brief sets the image size + * \param[in] width with of depth map + * \param[in] height height of depth map + */ + void setImageSize(unsigned width, unsigned height); + + /** + * \brief sets the clipping range + * \param[in] near distance of near clipping plane + * \param[in] far distance of far clipping plane + */ + void setDepthRange(float near, float far); + + /** + * \brief returns the width of depth maps + * \return width of the depth map + */ + unsigned getWidth() const; + + /** + * \brief returns the height of depth maps + * \return height of the depth map + */ + unsigned getHeight() const; + + /** + * \brief returns distance to the near clipping plane + * \return distance to near clipping plane + */ + float getNearClippingPlaneDistance() const; + + /** + * \brief returns the distance to the far clipping plane + * \return distance to far clipping plane + */ + float getFarClippingPlaneDistance() const; + + protected: + /** \brief width of depth maps generated by the sensor*/ + unsigned width_; + + /** \brief height of depth maps generated by the sensor*/ + unsigned height_; + + /** \brief distance of far clipping plane*/ + float far_clipping_plane_distance_; + + /** \brief distance of near clipping plane*/ + float near_clipping_plane_distance_; + }; + + /** + * \brief virtual destructor + */ + virtual ~SensorModel(); +}; +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h index 033dd67e30..3cf7f4553a 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,133 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include - -#include - -namespace mesh_filter -{ -/** - * \brief Model for Disparity based devices. E.g stereo camera systems or OpenNI compatible devices - * \author Suat Gedikli - */ -class MOVEIT_MESH_FILTER_EXPORT StereoCameraModel : public SensorModel -{ -public: - /** - * \brief Parameters for Stereo-like devices - * \author Suat Gedikli - */ - class Parameters : public SensorModel::Parameters - { - public: - /** - * \brief Constructor - * \param[in] width width of generated depth maps from this device - * \param[in] height height of generated depth maps from this device - * \param[in] near_clipping_plane_distance distance of near clipping plane - * \param[in] far_clipping_plane_distance distance of far clipping plene - * \param[in] fx focal length in x-direction - * \param[in] fy focal length in y-direction - * \param[in] cx x component of principal point - * \param[in] cy y component of principal point - * \param[in] base_line the distance in meters used to determine disparity values - * \param[in] disparity_resolution resolution/quantization of disparity values in pixels - */ - Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance, - float fx, float fy, float cx, float cy, float base_line, float disparity_resolution); - /** \brief Descturctor*/ - ~Parameters() override; - - /** - * \brief polymorphic clone method - * \return deep copied Parameters of type StereoCameraModel::Parameters - */ - SensorModel::Parameters* clone() const override; - - /** - * \brief set the shader parameters required for the model rendering - * \param[in] renderer the renderer that holds the rendering shader. - */ - void setRenderParameters(GLRenderer& renderer) const override; - - /** - * \brief set the shader parameters required for the mesh filtering - * @param[in] renderer the renderer that holds the filtering shader - */ - void setFilterParameters(GLRenderer& renderer) const override; - - /** - * \brief sets the camera parameters of the pinhole camera where the disparities were obtained. Usually the left - * camera - * \param[in] fx focal length in x-direction - * \param[in] fy focal length in y-direction - * \param[in] cx x component of principal point - * \param[in] cy y component of principal point - */ - void setCameraParameters(float fx, float fy, float cx, float cy); - - /** - * \brief sets the base line = distance of the two projective devices (camera, projector-camera) - * \param[in] base_line the distance in meters - */ - void setBaseline(float base_line); - - /** - * \brief the quantization of disparity values in pixels. Usually 1/16th or 1/8th for OpenNI compatible devices - * \param disparity_resolution - */ - void setDisparityResolution(float disparity_resolution); - - /** - * \brief returns the coefficients that are required for obtaining the padding for meshes - * \return the padding coefficients - */ - const Eigen::Vector3f& getPaddingCoefficients() const override; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - private: - /** \brief focal length in x-direction*/ - float fx_; - - /** \brief focal length in y-direction*/ - float fy_; - - /** \brief x component of principal point*/ - float cx_; - - /** \brief y component of principal point*/ - float cy_; - - /** \brief distance of the two projective devices that are used to determine the disparities*/ - float base_line_; - - /** \brief resolution/quantization of disparity values*/ - float disparity_resolution_; - - /** - * \brief padding coefficients - * \note absolute padding in meters = coeff[0] * z^2 + coeff[1] * z + coeff[2] - */ - const Eigen::Vector3f padding_coefficients_; - }; - - /** \brief predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) */ - static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; // NOLINT(readability-identifier-naming) - - /** \brief source code of the vertex shader used to render the meshes*/ - static const std::string RENDER_VERTEX_SHADER_SOURCE; - - /** \brief source code of the fragment shader used to render the meshes*/ - static const std::string RENDER_FRAGMENT_SHADER_SOURCE; - - /** \brief source code of the vertex shader used to filter the depth map*/ - static const std::string FILTER_VERTEX_SHADER_SOURCE; - - /** \brief source code of the fragment shader used to filter the depth map*/ - static const std::string FILTER_FRAGMENT_SHADER_SOURCE; -}; -} // namespace mesh_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.hpp new file mode 100644 index 0000000000..cea79d81e7 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/stereo_camera_model.hpp @@ -0,0 +1,167 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include + +#include + +namespace mesh_filter +{ +/** + * \brief Model for Disparity based devices. E.g stereo camera systems or OpenNI compatible devices + * \author Suat Gedikli + */ +class MOVEIT_MESH_FILTER_EXPORT StereoCameraModel : public SensorModel +{ +public: + /** + * \brief Parameters for Stereo-like devices + * \author Suat Gedikli + */ + class Parameters : public SensorModel::Parameters + { + public: + /** + * \brief Constructor + * \param[in] width width of generated depth maps from this device + * \param[in] height height of generated depth maps from this device + * \param[in] near_clipping_plane_distance distance of near clipping plane + * \param[in] far_clipping_plane_distance distance of far clipping plene + * \param[in] fx focal length in x-direction + * \param[in] fy focal length in y-direction + * \param[in] cx x component of principal point + * \param[in] cy y component of principal point + * \param[in] base_line the distance in meters used to determine disparity values + * \param[in] disparity_resolution resolution/quantization of disparity values in pixels + */ + Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance, + float fx, float fy, float cx, float cy, float base_line, float disparity_resolution); + /** \brief Descturctor*/ + ~Parameters() override; + + /** + * \brief polymorphic clone method + * \return deep copied Parameters of type StereoCameraModel::Parameters + */ + SensorModel::Parameters* clone() const override; + + /** + * \brief set the shader parameters required for the model rendering + * \param[in] renderer the renderer that holds the rendering shader. + */ + void setRenderParameters(GLRenderer& renderer) const override; + + /** + * \brief set the shader parameters required for the mesh filtering + * @param[in] renderer the renderer that holds the filtering shader + */ + void setFilterParameters(GLRenderer& renderer) const override; + + /** + * \brief sets the camera parameters of the pinhole camera where the disparities were obtained. Usually the left + * camera + * \param[in] fx focal length in x-direction + * \param[in] fy focal length in y-direction + * \param[in] cx x component of principal point + * \param[in] cy y component of principal point + */ + void setCameraParameters(float fx, float fy, float cx, float cy); + + /** + * \brief sets the base line = distance of the two projective devices (camera, projector-camera) + * \param[in] base_line the distance in meters + */ + void setBaseline(float base_line); + + /** + * \brief the quantization of disparity values in pixels. Usually 1/16th or 1/8th for OpenNI compatible devices + * \param disparity_resolution + */ + void setDisparityResolution(float disparity_resolution); + + /** + * \brief returns the coefficients that are required for obtaining the padding for meshes + * \return the padding coefficients + */ + const Eigen::Vector3f& getPaddingCoefficients() const override; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + /** \brief focal length in x-direction*/ + float fx_; + + /** \brief focal length in y-direction*/ + float fy_; + + /** \brief x component of principal point*/ + float cx_; + + /** \brief y component of principal point*/ + float cy_; + + /** \brief distance of the two projective devices that are used to determine the disparities*/ + float base_line_; + + /** \brief resolution/quantization of disparity values*/ + float disparity_resolution_; + + /** + * \brief padding coefficients + * \note absolute padding in meters = coeff[0] * z^2 + coeff[1] * z + coeff[2] + */ + const Eigen::Vector3f padding_coefficients_; + }; + + /** \brief predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion) */ + static const StereoCameraModel::Parameters& REGISTERED_PSDK_PARAMS; // NOLINT(readability-identifier-naming) + + /** \brief source code of the vertex shader used to render the meshes*/ + static const std::string RENDER_VERTEX_SHADER_SOURCE; + + /** \brief source code of the fragment shader used to render the meshes*/ + static const std::string RENDER_FRAGMENT_SHADER_SOURCE; + + /** \brief source code of the vertex shader used to filter the depth map*/ + static const std::string FILTER_VERTEX_SHADER_SOURCE; + + /** \brief source code of the fragment shader used to filter the depth map*/ + static const std::string FILTER_FRAGMENT_SHADER_SOURCE; +}; +} // namespace mesh_filter diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h index 9db4d91bc7..3a481ef73e 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,132 +47,5 @@ /* Author: Suat Gedikli */ #pragma once - -#include -#include -#include -#include -#include - -namespace tf2_ros -{ -class TransformListener; -class Buffer; -} // namespace tf2_ros - -/** - * \brief Class that caches and updates transformations for given frames. - * \author Suat Gedikli (gedikli@willowgarage.com) - */ -class TransformProvider -{ -public: - /** - * \brief Constructor - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] update_rate update rate in Hz - */ - TransformProvider(double update_rate = 30.); - - /** \brief Destructor */ - ~TransformProvider(); - - /** - * \brief returns the current transformation of a mesh given by its handle - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] handle handle of the mesh - * \param[out] transform pose of the mesh in camera coordinate system - * \return true if transform available, false otherwise - */ - bool getTransform(mesh_filter::MeshHandle handle, Eigen::Isometry3d& transform) const; - - /** - * \brief registers a mesh with its handle - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] handle handle of the mesh - * \param[in] name frame_id_ of the mesh - */ - void addHandle(mesh_filter::MeshHandle handle, const std::string& name); - - /** - * \brief sets the camera frame id. The returned transformations are in respect to this coordinate frame - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param frame frame id of parent/camera coordinate system. - */ - void setFrame(const std::string& frame); - - /** - * \brief starts the updating process. Done in a separate thread - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void start(); - - /** - * \brief stops the update process/thread. - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void stop(); - - /** - * \brief sets the update rate in Hz. This should be slow enough to reduce the system load but fast enough to get - * up-to-date transformations. For PSDK compatible devices this value should be around 30 Hz. - * \author Suat Gedikli (gedikli@willowgarage.com) - * \param[in] update_rate update rate in Hz - */ - void setUpdateRate(double update_rate); - -private: - /** - * \brief this method is called periodically by the dedicated thread and updates all the transformations of the - * registered frames. - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void updateTransforms(); - - MOVEIT_CLASS_FORWARD(TransformContext); // Defines TransformContextPtr, ConstPtr, WeakPtr... etc - - /** - * \brief Context Object for registered frames - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - class TransformContext - { - public: - TransformContext(const std::string& name) : frame_id_(name) - { - transformation_.matrix().setZero(); - } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - std::string frame_id_; - Eigen::Isometry3d transformation_; - std::mutex mutex_; - }; - - /** - * \brief The entry point of the dedicated thread that updates the transformations periodically. - * \author Suat Gedikli (gedikli@willowgarage.com) - */ - void run(); - - /** \brief mapping between the mesh handle and its context*/ - std::map handle2context_; - - /** \brief TransformListener used to listen and update transformations*/ - std::shared_ptr tf_listener_; - std::shared_ptr tf_buffer_; - - /** \brief SceneMonitor used to get current states*/ - planning_scene_monitor::PlanningSceneMonitorPtr psm_; - - /** \brief the camera frame id*/ - std::string frame_id_; - - /** \brief thread object*/ - std::thread thread_; - - /** \flag to leave the update loop*/ - bool stop_; - - /** \brief update rate in Hz*/ - ros::Rate update_rate_; -}; +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.hpp new file mode 100644 index 0000000000..b1d4603657 --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.hpp @@ -0,0 +1,166 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Suat Gedikli */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace tf2_ros +{ +class TransformListener; +class Buffer; +} // namespace tf2_ros + +/** + * \brief Class that caches and updates transformations for given frames. + * \author Suat Gedikli (gedikli@willowgarage.com) + */ +class TransformProvider +{ +public: + /** + * \brief Constructor + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] update_rate update rate in Hz + */ + TransformProvider(double update_rate = 30.); + + /** \brief Destructor */ + ~TransformProvider(); + + /** + * \brief returns the current transformation of a mesh given by its handle + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] handle handle of the mesh + * \param[out] transform pose of the mesh in camera coordinate system + * \return true if transform available, false otherwise + */ + bool getTransform(mesh_filter::MeshHandle handle, Eigen::Isometry3d& transform) const; + + /** + * \brief registers a mesh with its handle + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] handle handle of the mesh + * \param[in] name frame_id_ of the mesh + */ + void addHandle(mesh_filter::MeshHandle handle, const std::string& name); + + /** + * \brief sets the camera frame id. The returned transformations are in respect to this coordinate frame + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param frame frame id of parent/camera coordinate system. + */ + void setFrame(const std::string& frame); + + /** + * \brief starts the updating process. Done in a separate thread + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void start(); + + /** + * \brief stops the update process/thread. + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void stop(); + + /** + * \brief sets the update rate in Hz. This should be slow enough to reduce the system load but fast enough to get + * up-to-date transformations. For PSDK compatible devices this value should be around 30 Hz. + * \author Suat Gedikli (gedikli@willowgarage.com) + * \param[in] update_rate update rate in Hz + */ + void setUpdateRate(double update_rate); + +private: + /** + * \brief this method is called periodically by the dedicated thread and updates all the transformations of the + * registered frames. + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void updateTransforms(); + + MOVEIT_CLASS_FORWARD(TransformContext); // Defines TransformContextPtr, ConstPtr, WeakPtr... etc + + /** + * \brief Context Object for registered frames + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + class TransformContext + { + public: + TransformContext(const std::string& name) : frame_id_(name) + { + transformation_.matrix().setZero(); + } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string frame_id_; + Eigen::Isometry3d transformation_; + std::mutex mutex_; + }; + + /** + * \brief The entry point of the dedicated thread that updates the transformations periodically. + * \author Suat Gedikli (gedikli@willowgarage.com) + */ + void run(); + + /** \brief mapping between the mesh handle and its context*/ + std::map handle2context_; + + /** \brief TransformListener used to listen and update transformations*/ + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + + /** \brief SceneMonitor used to get current states*/ + planning_scene_monitor::PlanningSceneMonitorPtr psm_; + + /** \brief the camera frame id*/ + std::string frame_id_; + + /** \brief thread object*/ + std::thread thread_; + + /** \flag to leave the update loop*/ + bool stop_; + + /** \brief update rate in Hz*/ + ros::Rate update_rate_; +}; diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp index 266d8948ab..6de4cd1fdb 100644 --- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp +++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp @@ -34,14 +34,14 @@ /* Author: Suat Gedikli */ -#include -#include -#include +#include +#include +#include #include #include #include -#include -#include +#include +#include #include namespace enc = sensor_msgs::image_encodings; diff --git a/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp b/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp index 00fba30bb3..96737abbdd 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_mesh.cpp @@ -34,7 +34,7 @@ /* Author: Suat Gedikli */ -#include +#include #include #include #include diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 0dea00e55c..e528cb5b55 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -42,7 +42,7 @@ #include #endif #include -#include +#include #include #include #include diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp index 007e9e8397..06ec291625 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter.cpp @@ -34,8 +34,8 @@ /* Author: Suat Gedikli */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 6b4b551b9f..333a703192 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -34,9 +34,9 @@ /* Author: Suat Gedikli */ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 9ff5cdbf0c..a35edd3c1b 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -34,7 +34,7 @@ /* Author: Suat Gedikli */ -#include +#include #include mesh_filter::SensorModel::~SensorModel() = default; diff --git a/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp b/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp index eaa9d54118..3189986aa2 100644 --- a/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/stereo_camera_model.cpp @@ -34,8 +34,8 @@ /* Author: Suat Gedikli */ -#include -#include +#include +#include mesh_filter::StereoCameraModel::Parameters::Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, diff --git a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp index 44617c4322..a21695ed70 100644 --- a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp +++ b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp @@ -34,7 +34,7 @@ /* Author: Suat Gedikli */ -#include +#include #include #include #include diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index 08c78ec3f2..abfe43a15d 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -33,8 +33,8 @@ *********************************************************************/ #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h index d77d8dfe63..17c13dee53 100644 --- a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,97 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace point_containment_filter -{ -typedef unsigned int ShapeHandle; - -/** \brief Computing a mask for a pointcloud that states which points are inside the robot */ -class ShapeMask -{ -public: - /** \brief The possible values of a mask computed for a point */ - enum - { - INSIDE = 0, - OUTSIDE = 1, - CLIP = 2 - }; - - typedef std::function TransformCallback; - - /** \brief Construct the filter */ - ShapeMask(const TransformCallback& transform_callback = TransformCallback()); - - /** \brief Destructor to clean up */ - virtual ~ShapeMask(); - - ShapeHandle addShape(const shapes::ShapeConstPtr& shape, double scale = 1.0, double padding = 0.0); - void removeShape(ShapeHandle handle); - - void setTransformCallback(const TransformCallback& transform_callback); - - /** \brief Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE, the - point - is inside the robot. The point is outside if the mask element is OUTSIDE. - */ - void maskContainment(const sensor_msgs::msg::PointCloud2& data_in, const Eigen::Vector3d& sensor_pos, - const double min_sensor_dist, const double max_sensor_dist, std::vector& mask); - - /** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. - It is assumed the point is in the frame corresponding to the TransformCallback */ - int getMaskContainment(double x, double y, double z) const; - - /** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. - It is assumed the point is in the frame corresponding to the TransformCallback */ - int getMaskContainment(const Eigen::Vector3d& pt) const; - -protected: - struct SeeShape - { - SeeShape() - { - body = nullptr; - } - - bodies::Body* body; - ShapeHandle handle; - double volume; - }; - - struct SortBodies - { - bool operator()(const SeeShape& b1, const SeeShape& b2) const - { - if (b1.volume > b2.volume) - return true; - if (b1.volume < b2.volume) - return false; - return b1.handle < b2.handle; - } - }; - - TransformCallback transform_callback_; - - /** \brief Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration. */ - mutable std::mutex shapes_lock_; - std::set bodies_; - std::vector bspheres_; - -private: - /** \brief Free memory. */ - void freeMemory(); - - ShapeHandle next_handle_; - ShapeHandle min_handle_; - std::map::iterator> used_handles_; -}; -} // namespace point_containment_filter +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.hpp b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.hpp new file mode 100644 index 0000000000..d77d8dfe63 --- /dev/null +++ b/moveit_ros/perception/point_containment_filter/include/moveit/point_containment_filter/shape_mask.hpp @@ -0,0 +1,131 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace point_containment_filter +{ +typedef unsigned int ShapeHandle; + +/** \brief Computing a mask for a pointcloud that states which points are inside the robot */ +class ShapeMask +{ +public: + /** \brief The possible values of a mask computed for a point */ + enum + { + INSIDE = 0, + OUTSIDE = 1, + CLIP = 2 + }; + + typedef std::function TransformCallback; + + /** \brief Construct the filter */ + ShapeMask(const TransformCallback& transform_callback = TransformCallback()); + + /** \brief Destructor to clean up */ + virtual ~ShapeMask(); + + ShapeHandle addShape(const shapes::ShapeConstPtr& shape, double scale = 1.0, double padding = 0.0); + void removeShape(ShapeHandle handle); + + void setTransformCallback(const TransformCallback& transform_callback); + + /** \brief Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE, the + point + is inside the robot. The point is outside if the mask element is OUTSIDE. + */ + void maskContainment(const sensor_msgs::msg::PointCloud2& data_in, const Eigen::Vector3d& sensor_pos, + const double min_sensor_dist, const double max_sensor_dist, std::vector& mask); + + /** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. + It is assumed the point is in the frame corresponding to the TransformCallback */ + int getMaskContainment(double x, double y, double z) const; + + /** \brief Get the containment mask (INSIDE or OUTSIDE) value for an individual point. + It is assumed the point is in the frame corresponding to the TransformCallback */ + int getMaskContainment(const Eigen::Vector3d& pt) const; + +protected: + struct SeeShape + { + SeeShape() + { + body = nullptr; + } + + bodies::Body* body; + ShapeHandle handle; + double volume; + }; + + struct SortBodies + { + bool operator()(const SeeShape& b1, const SeeShape& b2) const + { + if (b1.volume > b2.volume) + return true; + if (b1.volume < b2.volume) + return false; + return b1.handle < b2.handle; + } + }; + + TransformCallback transform_callback_; + + /** \brief Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration. */ + mutable std::mutex shapes_lock_; + std::set bodies_; + std::vector bspheres_; + +private: + /** \brief Free memory. */ + void freeMemory(); + + ShapeHandle next_handle_; + ShapeHandle min_handle_; + std::map::iterator> used_handles_; +}; +} // namespace point_containment_filter diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index db931d5b54..73a532fbe4 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 79b0c0c33e..83f675ddfc 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,78 +47,5 @@ /* Author: Jon Binney, Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#if RCLCPP_VERSION_GTE(28, 3, 3) // Rolling -#include -#else -#include -#endif -#include -#include -#include - -#include - -namespace occupancy_map_monitor -{ -class PointCloudOctomapUpdater : public OccupancyMapUpdater -{ -public: - PointCloudOctomapUpdater(); - ~PointCloudOctomapUpdater() override{}; - - bool setParams(const std::string& name_space) override; - - bool initialize(const rclcpp::Node::SharedPtr& node) override; - void start() override; - void stop() override; - ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) override; - void forgetShape(ShapeHandle handle) override; - -protected: - virtual void updateMask(const sensor_msgs::msg::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin, - std::vector& mask); - -private: - bool getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const; - void cloudMsgCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud_msg); - - // TODO: Enable private node for publishing filtered point cloud - // ros::NodeHandle root_nh_; - // ros::NodeHandle private_nh_; - rclcpp::Node::SharedPtr node_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - // Initialize clock type to RCL_ROS_TIME to prevent exception about time sources mismatch - rclcpp::Time last_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - - /* params */ - std::string point_cloud_topic_; - double scale_; - double padding_; - double max_range_; - unsigned int point_subsample_; - double max_update_rate_; - std::string filtered_cloud_topic_; - std::string ns_; - rclcpp::Publisher::SharedPtr filtered_cloud_publisher_; - - message_filters::Subscriber* point_cloud_subscriber_; - tf2_ros::MessageFilter* point_cloud_filter_; - - /* used to store all cells in the map which a given ray passes through during raycasting. - we cache this here because it dynamically pre-allocates a lot of memory in its constructor */ - octomap::KeyRay key_ray_; - - std::unique_ptr shape_mask_; - std::vector mask_; - - rclcpp::Logger logger_; -}; -} // namespace occupancy_map_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp new file mode 100644 index 0000000000..e768005753 --- /dev/null +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp @@ -0,0 +1,112 @@ +/********************************************************************* + * 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: Jon Binney, Ioan Sucan */ + +#pragma once + +#include +#include +#include +#include +#if RCLCPP_VERSION_GTE(28, 3, 3) // Rolling +#include +#else +#include +#endif +#include +#include +#include + +#include + +namespace occupancy_map_monitor +{ +class PointCloudOctomapUpdater : public OccupancyMapUpdater +{ +public: + PointCloudOctomapUpdater(); + ~PointCloudOctomapUpdater() override{}; + + bool setParams(const std::string& name_space) override; + + bool initialize(const rclcpp::Node::SharedPtr& node) override; + void start() override; + void stop() override; + ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) override; + void forgetShape(ShapeHandle handle) override; + +protected: + virtual void updateMask(const sensor_msgs::msg::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin, + std::vector& mask); + +private: + bool getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const; + void cloudMsgCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud_msg); + + // TODO: Enable private node for publishing filtered point cloud + // ros::NodeHandle root_nh_; + // ros::NodeHandle private_nh_; + rclcpp::Node::SharedPtr node_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Initialize clock type to RCL_ROS_TIME to prevent exception about time sources mismatch + rclcpp::Time last_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + /* params */ + std::string point_cloud_topic_; + double scale_; + double padding_; + double max_range_; + unsigned int point_subsample_; + double max_update_rate_; + std::string filtered_cloud_topic_; + std::string ns_; + rclcpp::Publisher::SharedPtr filtered_cloud_publisher_; + + message_filters::Subscriber* point_cloud_subscriber_; + tf2_ros::MessageFilter* point_cloud_filter_; + + /* used to store all cells in the map which a given ray passes through during raycasting. + we cache this here because it dynamically pre-allocates a lot of memory in its constructor */ + octomap::KeyRay key_ray_; + + std::unique_ptr shape_mask_; + std::vector mask_; + + rclcpp::Logger logger_; +}; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/plugin_init.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/plugin_init.cpp index a586dbaae3..fc91e7d99e 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/plugin_init.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/plugin_init.cpp @@ -35,6 +35,6 @@ /* Author: Ioan Sucan */ #include -#include +#include CLASS_LOADER_REGISTER_CLASS(occupancy_map_monitor::PointCloudOctomapUpdater, occupancy_map_monitor::OccupancyMapUpdater) diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index bce0275430..c652430513 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -35,8 +35,8 @@ /* Author: Jon Binney, Ioan Sucan */ #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h index 1f56067e12..479855553d 100644 --- a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,131 +47,5 @@ /* Author: Sachin Chitta */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace shapes -{ -MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc -} - -namespace moveit -{ -namespace semantic_world -{ -MOVEIT_CLASS_FORWARD(SemanticWorld); // Defines SemanticWorldPtr, ConstPtr, WeakPtr... etc - -/** - * @brief A (simple) semantic world representation for pick and place and other tasks. - */ -class SemanticWorld -{ -public: - /** @brief The signature for a callback on receiving table messages*/ - typedef std::function TableCallbackFn; - - /** - * @brief A (simple) semantic world representation for pick and place and other tasks. - * Currently this is used only to represent tables. - */ - SemanticWorld(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningSceneConstPtr& planning_scene); - - /** - * @brief Get all the tables within a region of interest - */ - object_recognition_msgs::msg::TableArray getTablesInROI(double minx, double miny, double minz, double maxx, - double maxy, double maxz) const; - - /** - * @brief Get all the tables within a region of interest - */ - std::vector getTableNamesInROI(double minx, double miny, double minz, double maxx, double maxy, - double maxz) const; - - /** - * @brief Generate possible place poses on the table for a given object. This chooses appropriate - * values for min_distance_from_edge and for height_above_table based on the object properties. - * The assumption is that the object is represented by a mesh. - */ - std::vector - generatePlacePoses(const std::string& table_name, const shapes::ShapeConstPtr& object_shape, - const geometry_msgs::msg::Quaternion& object_orientation, double resolution, - double delta_height = 0.01, unsigned int num_heights = 2) const; - - /** - * @brief Generate possible place poses on the table for a given object. This chooses appropriate - * values for min_distance_from_edge and for height_above_table based on the object properties. - * The assumption is that the object is represented by a mesh. - */ - std::vector - generatePlacePoses(const object_recognition_msgs::msg::Table& table, const shapes::ShapeConstPtr& object_shape, - const geometry_msgs::msg::Quaternion& object_orientation, double resolution, - double delta_height = 0.01, unsigned int num_heights = 2) const; - /** - * @brief Generate possible place poses on the table. This samples locations in a grid on the table at - * the given resolution (in meters) in both X and Y directions. The locations are sampled at the - * specified height above the table (in meters) and then at subsequent additional heights (num_heights - * times) incremented by delta_height. Locations are only accepted if they are at least min_distance_from_edge - * meters from the edge of the table. - */ - std::vector generatePlacePoses(const object_recognition_msgs::msg::Table& table, - double resolution, double height_above_table, - double delta_height = 0.01, - unsigned int num_heights = 2, - double min_distance_from_edge = 0.10) const; - - void clear(); - - bool addTablesToCollisionWorld(); - - visualization_msgs::msg::MarkerArray - getPlaceLocationsMarker(const std::vector& poses) const; - - void addTableCallback(const TableCallbackFn& table_callback) - { - table_callback_ = table_callback; - } - - std::string findObjectTable(const geometry_msgs::msg::Pose& pose, double min_distance_from_edge = 0.0, - double min_vertical_offset = 0.0) const; - - bool isInsideTableContour(const geometry_msgs::msg::Pose& pose, const object_recognition_msgs::msg::Table& table, - double min_distance_from_edge = 0.0, double min_vertical_offset = 0.0) const; - -private: - shapes::Mesh* createSolidMeshFromPlanarPolygon(const shapes::Mesh& polygon, double thickness) const; - - shapes::Mesh* orientPlanarPolygon(const shapes::Mesh& polygon) const; - - void tableCallback(const object_recognition_msgs::msg::TableArray::ConstSharedPtr& msg); - - void transformTableArray(object_recognition_msgs::msg::TableArray& table_array) const; - - planning_scene::PlanningSceneConstPtr planning_scene_; - - rclcpp::Node::SharedPtr node_handle_; - - object_recognition_msgs::msg::TableArray table_array_; - - std::vector place_poses_; - - std::map current_tables_in_collision_world_; - - rclcpp::Subscription::SharedPtr table_subscriber_; - - rclcpp::Publisher::SharedPtr visualization_publisher_; - rclcpp::Publisher::SharedPtr collision_object_publisher_; - - TableCallbackFn table_callback_; - - rclcpp::Publisher::SharedPtr planning_scene_diff_publisher_; - - rclcpp::Logger logger_; -}; -} // namespace semantic_world -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.hpp b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.hpp new file mode 100644 index 0000000000..b46d17731e --- /dev/null +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.hpp @@ -0,0 +1,165 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Sachin Chitta */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace shapes +{ +MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc +} + +namespace moveit +{ +namespace semantic_world +{ +MOVEIT_CLASS_FORWARD(SemanticWorld); // Defines SemanticWorldPtr, ConstPtr, WeakPtr... etc + +/** + * @brief A (simple) semantic world representation for pick and place and other tasks. + */ +class SemanticWorld +{ +public: + /** @brief The signature for a callback on receiving table messages*/ + typedef std::function TableCallbackFn; + + /** + * @brief A (simple) semantic world representation for pick and place and other tasks. + * Currently this is used only to represent tables. + */ + SemanticWorld(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningSceneConstPtr& planning_scene); + + /** + * @brief Get all the tables within a region of interest + */ + object_recognition_msgs::msg::TableArray getTablesInROI(double minx, double miny, double minz, double maxx, + double maxy, double maxz) const; + + /** + * @brief Get all the tables within a region of interest + */ + std::vector getTableNamesInROI(double minx, double miny, double minz, double maxx, double maxy, + double maxz) const; + + /** + * @brief Generate possible place poses on the table for a given object. This chooses appropriate + * values for min_distance_from_edge and for height_above_table based on the object properties. + * The assumption is that the object is represented by a mesh. + */ + std::vector + generatePlacePoses(const std::string& table_name, const shapes::ShapeConstPtr& object_shape, + const geometry_msgs::msg::Quaternion& object_orientation, double resolution, + double delta_height = 0.01, unsigned int num_heights = 2) const; + + /** + * @brief Generate possible place poses on the table for a given object. This chooses appropriate + * values for min_distance_from_edge and for height_above_table based on the object properties. + * The assumption is that the object is represented by a mesh. + */ + std::vector + generatePlacePoses(const object_recognition_msgs::msg::Table& table, const shapes::ShapeConstPtr& object_shape, + const geometry_msgs::msg::Quaternion& object_orientation, double resolution, + double delta_height = 0.01, unsigned int num_heights = 2) const; + /** + * @brief Generate possible place poses on the table. This samples locations in a grid on the table at + * the given resolution (in meters) in both X and Y directions. The locations are sampled at the + * specified height above the table (in meters) and then at subsequent additional heights (num_heights + * times) incremented by delta_height. Locations are only accepted if they are at least min_distance_from_edge + * meters from the edge of the table. + */ + std::vector generatePlacePoses(const object_recognition_msgs::msg::Table& table, + double resolution, double height_above_table, + double delta_height = 0.01, + unsigned int num_heights = 2, + double min_distance_from_edge = 0.10) const; + + void clear(); + + bool addTablesToCollisionWorld(); + + visualization_msgs::msg::MarkerArray + getPlaceLocationsMarker(const std::vector& poses) const; + + void addTableCallback(const TableCallbackFn& table_callback) + { + table_callback_ = table_callback; + } + + std::string findObjectTable(const geometry_msgs::msg::Pose& pose, double min_distance_from_edge = 0.0, + double min_vertical_offset = 0.0) const; + + bool isInsideTableContour(const geometry_msgs::msg::Pose& pose, const object_recognition_msgs::msg::Table& table, + double min_distance_from_edge = 0.0, double min_vertical_offset = 0.0) const; + +private: + shapes::Mesh* createSolidMeshFromPlanarPolygon(const shapes::Mesh& polygon, double thickness) const; + + shapes::Mesh* orientPlanarPolygon(const shapes::Mesh& polygon) const; + + void tableCallback(const object_recognition_msgs::msg::TableArray::ConstSharedPtr& msg); + + void transformTableArray(object_recognition_msgs::msg::TableArray& table_array) const; + + planning_scene::PlanningSceneConstPtr planning_scene_; + + rclcpp::Node::SharedPtr node_handle_; + + object_recognition_msgs::msg::TableArray table_array_; + + std::vector place_poses_; + + std::map current_tables_in_collision_world_; + + rclcpp::Subscription::SharedPtr table_subscriber_; + + rclcpp::Publisher::SharedPtr visualization_publisher_; + rclcpp::Publisher::SharedPtr collision_object_publisher_; + + TableCallbackFn table_callback_; + + rclcpp::Publisher::SharedPtr planning_scene_diff_publisher_; + + rclcpp::Logger logger_; +}; +} // namespace semantic_world +} // namespace moveit diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 337630c267..fb9b659b4d 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -37,7 +37,7 @@ #include #include // MoveIt -#include +#include #include #include #include diff --git a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h index 93c67a8d5d..6092f7e330 100644 --- a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h +++ b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -33,23 +45,5 @@ *********************************************************************/ #pragma once - -#include -#include - -namespace collision_detection -{ -/** Augment CollisionPluginLoader with a method to fetch the plugin name from the ROS parameter server */ -class CollisionPluginLoader : public CollisionPluginCache -{ -public: - CollisionPluginLoader(); - - /** @brief Fetch plugin name from parameter server and activate the plugin for the given scene */ - void setupScene(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene); - -private: - rclcpp::Logger logger_; -}; - -} // namespace collision_detection +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.hpp b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.hpp new file mode 100644 index 0000000000..c4f3615961 --- /dev/null +++ b/moveit_ros/planning/collision_plugin_loader/include/moveit/collision_plugin_loader/collision_plugin_loader.hpp @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014 Fetch Robotics 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 Fetch Robotics 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. + *********************************************************************/ + +#pragma once + +#include +#include + +namespace collision_detection +{ +/** Augment CollisionPluginLoader with a method to fetch the plugin name from the ROS parameter server */ +class CollisionPluginLoader : public CollisionPluginCache +{ +public: + CollisionPluginLoader(); + + /** @brief Fetch plugin name from parameter server and activate the plugin for the given scene */ + void setupScene(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene); + +private: + rclcpp::Logger logger_; +}; + +} // namespace collision_detection diff --git a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp index ea362c574b..e91b8e93a7 100644 --- a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp +++ b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include namespace collision_detection diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h index 4aeeda5758..754bad6229 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h +++ b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,31 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include - -namespace constraint_sampler_manager_loader -{ -MOVEIT_CLASS_FORWARD( - ConstraintSamplerManagerLoader); // Defines ConstraintSamplerManagerLoaderPtr, ConstPtr, WeakPtr... etc - -class ConstraintSamplerManagerLoader -{ -public: - ConstraintSamplerManagerLoader( - const rclcpp::Node::SharedPtr& node, - const constraint_samplers::ConstraintSamplerManagerPtr& csm = constraint_samplers::ConstraintSamplerManagerPtr()); - - const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager() const - { - return constraint_sampler_manager_; - } - -private: - constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; - - MOVEIT_CLASS_FORWARD(Helper); - HelperPtr impl_; -}; -} // namespace constraint_sampler_manager_loader +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.hpp b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.hpp new file mode 100644 index 0000000000..ced71f9eda --- /dev/null +++ b/moveit_ros/planning/constraint_sampler_manager_loader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.hpp @@ -0,0 +1,65 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include + +namespace constraint_sampler_manager_loader +{ +MOVEIT_CLASS_FORWARD( + ConstraintSamplerManagerLoader); // Defines ConstraintSamplerManagerLoaderPtr, ConstPtr, WeakPtr... etc + +class ConstraintSamplerManagerLoader +{ +public: + ConstraintSamplerManagerLoader( + const rclcpp::Node::SharedPtr& node, + const constraint_samplers::ConstraintSamplerManagerPtr& csm = constraint_samplers::ConstraintSamplerManagerPtr()); + + const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager() const + { + return constraint_sampler_manager_; + } + +private: + constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_; + + MOVEIT_CLASS_FORWARD(Helper); + HelperPtr impl_; +}; +} // namespace constraint_sampler_manager_loader diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp index cd069ca821..074b397c02 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp +++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 96c645da15..2d39f11e3e 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,63 +47,5 @@ /* Author: Ioan Sucan, Dave Coleman */ #pragma once - -#include -#include -#include -#include -#include - -namespace kinematics_plugin_loader -{ -MOVEIT_CLASS_FORWARD(KinematicsPluginLoader); // Defines KinematicsPluginLoaderPtr, ConstPtr, WeakPtr... etc - -/** \brief Helper class for loading kinematics solvers */ -class KinematicsPluginLoader -{ -public: - /** \brief Load the kinematics solvers based on information on the - ROS parameter server. Take node as an argument and as optional argument the name of the - ROS parameter under which the robot description can be - found. This is passed to the kinematics solver initialization as - well as used to read the SRDF document when needed. */ - KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node, - const std::string& robot_description = "robot_description") - : node_(node) - , robot_description_(robot_description) - , logger_(moveit::getLogger("moveit.ros.kinematics_plugin_loader")) - { - } - - /** \brief Get a function pointer that allocates and initializes a kinematics solver. If not previously called, this - * function reads ROS parameters for the groups defined in the SRDF. */ - moveit::core::SolverAllocatorFn getLoaderFunction(const srdf::ModelSharedPtr& srdf_model); - - /** \brief Get the groups for which the function pointer returned by getLoaderFunction() can allocate a solver */ - const std::vector& getKnownGroups() const - { - return groups_; - } - - /** \brief Get a map from group name to default IK timeout */ - const std::map& getIKTimeout() const - { - return ik_timeout_; - } - - void status() const; - -private: - const rclcpp::Node::SharedPtr node_; - std::unordered_map> group_param_listener_; - std::unordered_map group_params_; - std::string robot_description_; - - MOVEIT_CLASS_FORWARD(KinematicsLoaderImpl); // Defines KinematicsLoaderImplPtr, ConstPtr, WeakPtr... etc - KinematicsLoaderImplPtr loader_; - - std::vector groups_; - std::map ik_timeout_; - rclcpp::Logger logger_; -}; -} // namespace kinematics_plugin_loader +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.hpp b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.hpp new file mode 100644 index 0000000000..e7494cf73c --- /dev/null +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.hpp @@ -0,0 +1,97 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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, Dave Coleman */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace kinematics_plugin_loader +{ +MOVEIT_CLASS_FORWARD(KinematicsPluginLoader); // Defines KinematicsPluginLoaderPtr, ConstPtr, WeakPtr... etc + +/** \brief Helper class for loading kinematics solvers */ +class KinematicsPluginLoader +{ +public: + /** \brief Load the kinematics solvers based on information on the + ROS parameter server. Take node as an argument and as optional argument the name of the + ROS parameter under which the robot description can be + found. This is passed to the kinematics solver initialization as + well as used to read the SRDF document when needed. */ + KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node, + const std::string& robot_description = "robot_description") + : node_(node) + , robot_description_(robot_description) + , logger_(moveit::getLogger("moveit.ros.kinematics_plugin_loader")) + { + } + + /** \brief Get a function pointer that allocates and initializes a kinematics solver. If not previously called, this + * function reads ROS parameters for the groups defined in the SRDF. */ + moveit::core::SolverAllocatorFn getLoaderFunction(const srdf::ModelSharedPtr& srdf_model); + + /** \brief Get the groups for which the function pointer returned by getLoaderFunction() can allocate a solver */ + const std::vector& getKnownGroups() const + { + return groups_; + } + + /** \brief Get a map from group name to default IK timeout */ + const std::map& getIKTimeout() const + { + return ik_timeout_; + } + + void status() const; + +private: + const rclcpp::Node::SharedPtr node_; + std::unordered_map> group_param_listener_; + std::unordered_map group_params_; + std::string robot_description_; + + MOVEIT_CLASS_FORWARD(KinematicsLoaderImpl); // Defines KinematicsLoaderImplPtr, ConstPtr, WeakPtr... etc + KinematicsLoaderImplPtr loader_; + + std::vector groups_; + std::map ik_timeout_; + rclcpp::Logger logger_; +}; +} // namespace kinematics_plugin_loader diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index db723955d5..ead7aa044a 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Dave Coleman */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 15251ccdc9..144ddaad6c 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,158 +50,5 @@ */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace moveit_cpp -{ -MOVEIT_CLASS_FORWARD(MoveItCpp); // Defines MoveItCppPtr, ConstPtr, WeakPtr... etc - -class MoveItCpp -{ -public: - /// Specification of options to use when constructing the MoveItCpp class - struct PlanningSceneMonitorOptions - { - PlanningSceneMonitorOptions() - : joint_state_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC) - , attached_collision_object_topic( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC) - , monitored_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC) - , publish_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC) - { - } - - void load(const rclcpp::Node::SharedPtr& node) - { - const std::string ns = "planning_scene_monitor_options"; - node->get_parameter_or(ns + ".name", name, std::string("planning_scene_monitor")); - node->get_parameter_or(ns + ".robot_description", robot_description, std::string("robot_description")); - node->get_parameter_or(ns + ".wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); - } - std::string name; - std::string robot_description; - const std::string joint_state_topic; - const std::string attached_collision_object_topic; - const std::string monitored_planning_scene_topic; - const std::string publish_planning_scene_topic; - double wait_for_initial_state_timeout; - }; - - /// struct contains the the variables used for loading the planning pipeline - struct PlanningPipelineOptions - { - void load(const rclcpp::Node::SharedPtr& node) - { - const std::string ns = "planning_pipelines."; - node->get_parameter(ns + "pipeline_names", pipeline_names); - node->get_parameter(ns + "namespace", parent_namespace); - } - std::vector pipeline_names; - std::string parent_namespace; - }; - - /// Parameter container for initializing MoveItCpp - struct Options - { - Options(const rclcpp::Node::SharedPtr& node) - { - planning_scene_monitor_options.load(node); - planning_pipeline_options.load(node); - } - - PlanningSceneMonitorOptions planning_scene_monitor_options; - PlanningPipelineOptions planning_pipeline_options; - }; - - /** \brief Constructor */ - MoveItCpp(const rclcpp::Node::SharedPtr& node); - MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options); - - /** - * @brief This class owns unique resources (e.g. action clients, threads) and its not very - * meaningful to copy. Pass by references, move it, or simply create multiple instances where - * required. - */ - MoveItCpp(const MoveItCpp&) = delete; - MoveItCpp& operator=(const MoveItCpp&) = delete; - - MoveItCpp(MoveItCpp&& other) = default; - MoveItCpp& operator=(MoveItCpp&& other) = default; - - /** \brief Destructor */ - ~MoveItCpp(); - - /** \brief Get the RobotModel object. */ - moveit::core::RobotModelConstPtr getRobotModel() const; - - /** \brief Get the ROS node this instance operates on */ - const rclcpp::Node::SharedPtr& getNode() const; - - /** \brief Get the current state queried from the current state monitor - \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ - bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds); - - /** \brief Get the current state queried from the current state monitor - \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ - moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0); - - /** \brief Get all loaded planning pipeline instances mapped to their reference names */ - const std::unordered_map& getPlanningPipelines() const; - - /** \brief Get the stored instance of the planning scene monitor */ - planning_scene_monitor::PlanningSceneMonitorConstPtr getPlanningSceneMonitor() const; - planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); - - std::shared_ptr getTFBuffer() const; - std::shared_ptr getTFBuffer(); - - /** \brief Get the stored instance of the trajectory execution manager */ - trajectory_execution_manager::TrajectoryExecutionManagerConstPtr getTrajectoryExecutionManager() const; - trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); - - /** \brief Execute a trajectory on the planning group specified by the robot's trajectory using the trajectory - * execution manager. - * \param [in] robot_trajectory Contains trajectory info as well as metadata such as a RobotModel. - * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find - * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. - * \return moveit_controller_manager::ExecutionStatus::SUCCEEDED if successful - */ - [[deprecated("MoveItCpp::execute() no longer requires a blocking parameter")]] moveit_controller_manager::ExecutionStatus - execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking, - const std::vector& controllers = std::vector()); - - moveit_controller_manager::ExecutionStatus - execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, - const std::vector& controllers = std::vector()); - - /** \brief Utility to terminate the given planning pipeline */ - bool terminatePlanningPipeline(const std::string& pipeline_name); - -private: - // Core properties and instances - rclcpp::Node::SharedPtr node_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Planning - std::unordered_map planning_pipelines_; - std::unordered_map> groups_algorithms_map_; - - // Execution - trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; - - rclcpp::Logger logger_; - - /** \brief Initialize and setup the planning scene monitor */ - bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options); - - /** \brief Initialize and setup the planning pipelines */ - bool loadPlanningPipelines(const PlanningPipelineOptions& options); -}; -} // namespace moveit_cpp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp new file mode 100644 index 0000000000..eeaf4ab415 --- /dev/null +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp @@ -0,0 +1,195 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik 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 PickNik Inc. 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: Henning Kayser + Desc: A high level interface that does not require the use of ROS Actions, Services, and Messages to access the + core MoveIt functionality +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_cpp +{ +MOVEIT_CLASS_FORWARD(MoveItCpp); // Defines MoveItCppPtr, ConstPtr, WeakPtr... etc + +class MoveItCpp +{ +public: + /// Specification of options to use when constructing the MoveItCpp class + struct PlanningSceneMonitorOptions + { + PlanningSceneMonitorOptions() + : joint_state_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC) + , attached_collision_object_topic( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC) + , monitored_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC) + , publish_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC) + { + } + + void load(const rclcpp::Node::SharedPtr& node) + { + const std::string ns = "planning_scene_monitor_options"; + node->get_parameter_or(ns + ".name", name, std::string("planning_scene_monitor")); + node->get_parameter_or(ns + ".robot_description", robot_description, std::string("robot_description")); + node->get_parameter_or(ns + ".wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); + } + std::string name; + std::string robot_description; + const std::string joint_state_topic; + const std::string attached_collision_object_topic; + const std::string monitored_planning_scene_topic; + const std::string publish_planning_scene_topic; + double wait_for_initial_state_timeout; + }; + + /// struct contains the the variables used for loading the planning pipeline + struct PlanningPipelineOptions + { + void load(const rclcpp::Node::SharedPtr& node) + { + const std::string ns = "planning_pipelines."; + node->get_parameter(ns + "pipeline_names", pipeline_names); + node->get_parameter(ns + "namespace", parent_namespace); + } + std::vector pipeline_names; + std::string parent_namespace; + }; + + /// Parameter container for initializing MoveItCpp + struct Options + { + Options(const rclcpp::Node::SharedPtr& node) + { + planning_scene_monitor_options.load(node); + planning_pipeline_options.load(node); + } + + PlanningSceneMonitorOptions planning_scene_monitor_options; + PlanningPipelineOptions planning_pipeline_options; + }; + + /** \brief Constructor */ + MoveItCpp(const rclcpp::Node::SharedPtr& node); + MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; + + MoveItCpp(MoveItCpp&& other) = default; + MoveItCpp& operator=(MoveItCpp&& other) = default; + + /** \brief Destructor */ + ~MoveItCpp(); + + /** \brief Get the RobotModel object. */ + moveit::core::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node this instance operates on */ + const rclcpp::Node::SharedPtr& getNode() const; + + /** \brief Get the current state queried from the current state monitor + \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ + bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds); + + /** \brief Get the current state queried from the current state monitor + \param wait_seconds the time in seconds for the state monitor to wait for a robot state. */ + moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0); + + /** \brief Get all loaded planning pipeline instances mapped to their reference names */ + const std::unordered_map& getPlanningPipelines() const; + + /** \brief Get the stored instance of the planning scene monitor */ + planning_scene_monitor::PlanningSceneMonitorConstPtr getPlanningSceneMonitor() const; + planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); + + std::shared_ptr getTFBuffer() const; + std::shared_ptr getTFBuffer(); + + /** \brief Get the stored instance of the trajectory execution manager */ + trajectory_execution_manager::TrajectoryExecutionManagerConstPtr getTrajectoryExecutionManager() const; + trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); + + /** \brief Execute a trajectory on the planning group specified by the robot's trajectory using the trajectory + * execution manager. + * \param [in] robot_trajectory Contains trajectory info as well as metadata such as a RobotModel. + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit_controller_manager::ExecutionStatus::SUCCEEDED if successful + */ + [[deprecated("MoveItCpp::execute() no longer requires a blocking parameter")]] moveit_controller_manager::ExecutionStatus + execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking, + const std::vector& controllers = std::vector()); + + moveit_controller_manager::ExecutionStatus + execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + const std::vector& controllers = std::vector()); + + /** \brief Utility to terminate the given planning pipeline */ + bool terminatePlanningPipeline(const std::string& pipeline_name); + +private: + // Core properties and instances + rclcpp::Node::SharedPtr node_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Planning + std::unordered_map planning_pipelines_; + std::unordered_map> groups_algorithms_map_; + + // Execution + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + + rclcpp::Logger logger_; + + /** \brief Initialize and setup the planning scene monitor */ + bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options); + + /** \brief Initialize and setup the planning pipelines */ + bool loadPlanningPipelines(const PlanningPipelineOptions& options); +}; +} // namespace moveit_cpp diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 88668e1f01..ac6928f664 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,218 +48,5 @@ Desc: API for planning and execution capabilities of a JointModelGroup */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace moveit_cpp -{ -MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc - -class PlanningComponent -{ -public: - /// Planner parameters provided with the MotionPlanRequest - struct PlanRequestParameters - { - std::string planner_id; - std::string planning_pipeline; - int planning_attempts; - double planning_time; - double max_velocity_scaling_factor; - double max_acceleration_scaling_factor; - - template - void declareOrGetParam(const rclcpp::Node::SharedPtr& node, const std::string& param_name, T& output_value, - T default_value) - { - // Try to get parameter or use default - if (!node->get_parameter_or(param_name, output_value, default_value)) - { - RCLCPP_WARN(node->get_logger(), - "Parameter \'%s\' not found in config use default value instead, check parameter type and " - "namespace in YAML file", - (param_name).c_str()); - } - } - - void load(const rclcpp::Node::SharedPtr& node, const std::string& param_namespace = "") - { - // Set namespace - std::string ns = "plan_request_params."; - if (!param_namespace.empty()) - { - ns = param_namespace + ".plan_request_params."; - } - - // Declare parameters - declareOrGetParam(node, ns + "planner_id", planner_id, std::string("")); - declareOrGetParam(node, ns + "planning_pipeline", planning_pipeline, std::string("")); - declareOrGetParam(node, ns + "planning_time", planning_time, 1.0); - declareOrGetParam(node, ns + "planning_attempts", planning_attempts, 5); - declareOrGetParam(node, ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); - declareOrGetParam(node, ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); - } - }; - - /// Planner parameters provided with the MotionPlanRequest - struct MultiPipelinePlanRequestParameters - { - /** Constructor, load MultiPipelinePlanRequestParameters as defined in the node's ROS parameters - * \param [in] node Node access the ROS parameters - * \param [in] planning_pipeline_names A vector with the names of the pipelines that should be used in parallel - */ - MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node, - const std::vector& planning_pipeline_names) - { - plan_request_parameter_vector.reserve(planning_pipeline_names.size()); - - for (const auto& planning_pipeline_name : planning_pipeline_names) - { - PlanRequestParameters parameters; - parameters.load(node, planning_pipeline_name); - plan_request_parameter_vector.push_back(parameters); - } - } - - // Additional constructor to create an empty MultiPipelinePlanRequestParameters instance - MultiPipelinePlanRequestParameters() - { - } - - // Plan request parameters for the individual planning pipelines which run concurrently - std::vector plan_request_parameter_vector; - }; - - /** \brief Constructor */ - PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node); - PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp); - - /** - * @brief This class owns unique resources (e.g. action clients, threads) and its not very - * meaningful to copy. Pass by references, move it, or simply create multiple instances where - * required. - */ - PlanningComponent(const PlanningComponent&) = delete; - PlanningComponent& operator=(const PlanningComponent&) = delete; - - PlanningComponent(PlanningComponent&& other) = delete; - PlanningComponent& operator=(PlanningComponent&& other) = delete; - - /** \brief Destructor */ - ~PlanningComponent() = default; - - /** \brief Get the name of the planning group */ - const std::string& getPlanningGroupName() const; - - /** \brief Get the names of the named robot states available as targets */ - const std::vector getNamedTargetStates(); - - /** \brief Get the joint values for targets specified by name */ - std::map getNamedTargetStateValues(const std::string& name); - - /** \brief Specify the workspace bounding box. - The box is specified in the planning frame (i.e. relative to the robot root link start position). - This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the - robot relative to the world. */ - void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); - - /** \brief Remove the workspace bounding box from planning */ - void unsetWorkspace(); - - /** \brief Get the considered start state if defined, otherwise get the current state */ - moveit::core::RobotStatePtr getStartState(); - - /** \brief Set the robot state that should be considered as start state for planning */ - bool setStartState(const moveit::core::RobotState& start_state); - /** \brief Set the named robot state that should be considered as start state for planning */ - bool setStartState(const std::string& named_state); - - /** \brief Set the start state for planning to be the current state of the robot */ - void setStartStateToCurrentState(); - - /** \brief Set the goal constraints used for planning */ - bool setGoal(const std::vector& goal_constraints); - /** \brief Set the goal constraints generated from a target state */ - bool setGoal(const moveit::core::RobotState& goal_state); - /** \brief Set the goal constraints generated from target pose and robot link */ - bool setGoal(const geometry_msgs::msg::PoseStamped& goal_pose, const std::string& link_name); - /** \brief Set the goal constraints generated from a named target state */ - bool setGoal(const std::string& named_target); - - /** \brief Set the path constraints generated from a moveit msg Constraints */ - bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints); - - /** \brief Set the trajectory constraints generated from a moveit msg Constraints */ - bool setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& trajectory_constraints); - - /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using - * default parameters. */ - planning_interface::MotionPlanResponse plan(); - /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the - * provided PlanRequestParameters. */ - planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, - planning_scene::PlanningScenePtr planning_scene = nullptr); - - /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the - * provided PlanRequestParameters. This defaults to taking the full planning time (null stopping_criterion_callback) - * and finding the shortest solution in joint space. */ - planning_interface::MotionPlanResponse - plan(const MultiPipelinePlanRequestParameters& parameters, - const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = - &moveit::planning_pipeline_interfaces::getShortestSolution, - const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr, - planning_scene::PlanningScenePtr planning_scene = nullptr); - - /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates - * after the execution is complete. The execution can be run in background by setting blocking to false. */ - [[deprecated("Use MoveItCpp::execute()")]] bool execute(bool /*blocking */) - { - return false; - }; - - /** \brief Utility function to get a MotionPlanRequest from PlanRequestParameters and the internal state of the - * PlanningComponent instance */ - ::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters); - - /** \brief Utility function to get a Vector of MotionPlanRequest from a vector of PlanRequestParameters and the - * internal state of the PlanningComponent instance */ - std::vector<::planning_interface::MotionPlanRequest> - getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters); - -private: - // Core properties and instances - rclcpp::Node::SharedPtr node_; - MoveItCppPtr moveit_cpp_; - const std::string group_name_; - // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources - const moveit::core::JointModelGroup* joint_model_group_; - - // Planning - // The start state used in the planning motion request - moveit::core::RobotStatePtr considered_start_state_; - std::vector current_goal_constraints_; - moveit_msgs::msg::Constraints current_path_constraints_; - moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_; - moveit_msgs::msg::WorkspaceParameters workspace_parameters_; - bool workspace_parameters_set_ = false; - rclcpp::Logger logger_; - - // common properties for goals - // TODO(henningkayser): support goal tolerances - // double goal_joint_tolerance_; - // double goal_position_tolerance_; - // double goal_orientation_tolerance_; - // TODO(henningkayser): implement path/trajectory constraints - // std::unique_ptr path_constraints_; - // std::unique_ptr trajectory_constraints_; -}; -} // namespace moveit_cpp +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.hpp b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.hpp new file mode 100644 index 0000000000..b8b74367e7 --- /dev/null +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.hpp @@ -0,0 +1,253 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik 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 PickNik Inc. 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: Henning Kayser + Desc: API for planning and execution capabilities of a JointModelGroup */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_cpp +{ +MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc + +class PlanningComponent +{ +public: + /// Planner parameters provided with the MotionPlanRequest + struct PlanRequestParameters + { + std::string planner_id; + std::string planning_pipeline; + int planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + + template + void declareOrGetParam(const rclcpp::Node::SharedPtr& node, const std::string& param_name, T& output_value, + T default_value) + { + // Try to get parameter or use default + if (!node->get_parameter_or(param_name, output_value, default_value)) + { + RCLCPP_WARN(node->get_logger(), + "Parameter \'%s\' not found in config use default value instead, check parameter type and " + "namespace in YAML file", + (param_name).c_str()); + } + } + + void load(const rclcpp::Node::SharedPtr& node, const std::string& param_namespace = "") + { + // Set namespace + std::string ns = "plan_request_params."; + if (!param_namespace.empty()) + { + ns = param_namespace + ".plan_request_params."; + } + + // Declare parameters + declareOrGetParam(node, ns + "planner_id", planner_id, std::string("")); + declareOrGetParam(node, ns + "planning_pipeline", planning_pipeline, std::string("")); + declareOrGetParam(node, ns + "planning_time", planning_time, 1.0); + declareOrGetParam(node, ns + "planning_attempts", planning_attempts, 5); + declareOrGetParam(node, ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); + declareOrGetParam(node, ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); + } + }; + + /// Planner parameters provided with the MotionPlanRequest + struct MultiPipelinePlanRequestParameters + { + /** Constructor, load MultiPipelinePlanRequestParameters as defined in the node's ROS parameters + * \param [in] node Node access the ROS parameters + * \param [in] planning_pipeline_names A vector with the names of the pipelines that should be used in parallel + */ + MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node, + const std::vector& planning_pipeline_names) + { + plan_request_parameter_vector.reserve(planning_pipeline_names.size()); + + for (const auto& planning_pipeline_name : planning_pipeline_names) + { + PlanRequestParameters parameters; + parameters.load(node, planning_pipeline_name); + plan_request_parameter_vector.push_back(parameters); + } + } + + // Additional constructor to create an empty MultiPipelinePlanRequestParameters instance + MultiPipelinePlanRequestParameters() + { + } + + // Plan request parameters for the individual planning pipelines which run concurrently + std::vector plan_request_parameter_vector; + }; + + /** \brief Constructor */ + PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node); + PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + PlanningComponent(const PlanningComponent&) = delete; + PlanningComponent& operator=(const PlanningComponent&) = delete; + + PlanningComponent(PlanningComponent&& other) = delete; + PlanningComponent& operator=(PlanningComponent&& other) = delete; + + /** \brief Destructor */ + ~PlanningComponent() = default; + + /** \brief Get the name of the planning group */ + const std::string& getPlanningGroupName() const; + + /** \brief Get the names of the named robot states available as targets */ + const std::vector getNamedTargetStates(); + + /** \brief Get the joint values for targets specified by name */ + std::map getNamedTargetStateValues(const std::string& name); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief Remove the workspace bounding box from planning */ + void unsetWorkspace(); + + /** \brief Get the considered start state if defined, otherwise get the current state */ + moveit::core::RobotStatePtr getStartState(); + + /** \brief Set the robot state that should be considered as start state for planning */ + bool setStartState(const moveit::core::RobotState& start_state); + /** \brief Set the named robot state that should be considered as start state for planning */ + bool setStartState(const std::string& named_state); + + /** \brief Set the start state for planning to be the current state of the robot */ + void setStartStateToCurrentState(); + + /** \brief Set the goal constraints used for planning */ + bool setGoal(const std::vector& goal_constraints); + /** \brief Set the goal constraints generated from a target state */ + bool setGoal(const moveit::core::RobotState& goal_state); + /** \brief Set the goal constraints generated from target pose and robot link */ + bool setGoal(const geometry_msgs::msg::PoseStamped& goal_pose, const std::string& link_name); + /** \brief Set the goal constraints generated from a named target state */ + bool setGoal(const std::string& named_target); + + /** \brief Set the path constraints generated from a moveit msg Constraints */ + bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints); + + /** \brief Set the trajectory constraints generated from a moveit msg Constraints */ + bool setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& trajectory_constraints); + + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using + * default parameters. */ + planning_interface::MotionPlanResponse plan(); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the + * provided PlanRequestParameters. */ + planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, + planning_scene::PlanningScenePtr planning_scene = nullptr); + + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the + * provided PlanRequestParameters. This defaults to taking the full planning time (null stopping_criterion_callback) + * and finding the shortest solution in joint space. */ + planning_interface::MotionPlanResponse + plan(const MultiPipelinePlanRequestParameters& parameters, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = + &moveit::planning_pipeline_interfaces::getShortestSolution, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr, + planning_scene::PlanningScenePtr planning_scene = nullptr); + + /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates + * after the execution is complete. The execution can be run in background by setting blocking to false. */ + [[deprecated("Use MoveItCpp::execute()")]] bool execute(bool /*blocking */) + { + return false; + }; + + /** \brief Utility function to get a MotionPlanRequest from PlanRequestParameters and the internal state of the + * PlanningComponent instance */ + ::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters); + + /** \brief Utility function to get a Vector of MotionPlanRequest from a vector of PlanRequestParameters and the + * internal state of the PlanningComponent instance */ + std::vector<::planning_interface::MotionPlanRequest> + getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters); + +private: + // Core properties and instances + rclcpp::Node::SharedPtr node_; + MoveItCppPtr moveit_cpp_; + const std::string group_name_; + // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources + const moveit::core::JointModelGroup* joint_model_group_; + + // Planning + // The start state used in the planning motion request + moveit::core::RobotStatePtr considered_start_state_; + std::vector current_goal_constraints_; + moveit_msgs::msg::Constraints current_path_constraints_; + moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_; + moveit_msgs::msg::WorkspaceParameters workspace_parameters_; + bool workspace_parameters_set_ = false; + rclcpp::Logger logger_; + + // common properties for goals + // TODO(henningkayser): support goal tolerances + // double goal_joint_tolerance_; + // double goal_position_tolerance_; + // double goal_orientation_tolerance_; + // TODO(henningkayser): implement path/trajectory constraints + // std::unique_ptr path_constraints_; + // std::unique_ptr trajectory_constraints_; +}; +} // namespace moveit_cpp diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 410edc25ff..abc683745c 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -36,9 +36,9 @@ #include -#include +#include #include -#include +#include #include #include #include diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index b3494a4a50..945a979e16 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -36,11 +36,11 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_ros/planning/moveit_cpp/test/moveit_cpp_test.cpp b/moveit_ros/planning/moveit_cpp/test/moveit_cpp_test.cpp index 2191d9f112..8d49561f56 100644 --- a/moveit_ros/planning/moveit_cpp/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning/moveit_cpp/test/moveit_cpp_test.cpp @@ -43,8 +43,8 @@ #include // Main class -#include -#include +#include +#include // Msgs #include diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h index 5f2d9dd80b..999562f7e4 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,157 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include - -/** \brief This namespace includes functionality specific to the execution and monitoring of motion plans */ -namespace plan_execution -{ -MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc - -class PlanExecution -{ -public: - struct Options - { - Options() : replan(false), replan_attemps(0), replan_delay(0.0) - { - } - - /// Flag indicating whether replanning is allowed - bool replan; - - /// If replanning is allowed, this variable specifies how many replanning attempts there can be, at most, before - /// failure - unsigned int replan_attemps; - - /// The amount of time to wait in between replanning attempts (in seconds) - double replan_delay; - - /// Callback for computing motion plans. This callback must always be specified. - ExecutableMotionPlanComputationFn plan_callback; - - /// Callback for repairing motion plans. This is optional. A new plan is re-computed if repairing routines are not - /// specified. - /// To aid in the repair process, the position that the controller had reached in the execution of the previous plan - /// is also passed as argument. - /// The format is the same as what the trajectory_execution_manager::TrajectoryExecutionManager reports: a pair of - /// two integers where the first - /// one is the index of the last trajectory being executed (from the sequence of trajectories specified in the - /// ExecutableMotionPlan) and the second - /// one is the index of the closest waypoint along that trajectory. - std::function& trajectory_index)> - repair_plan_callback_; - - std::function before_plan_callback_; - std::function before_execution_callback_; - std::function done_callback_; - }; - - PlanExecution(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution); - ~PlanExecution(); - - const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const - { - return planning_scene_monitor_; - } - - const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const - { - return trajectory_execution_manager_; - } - - double getTrajectoryStateRecordingFrequency() const - { - if (trajectory_monitor_) - { - return trajectory_monitor_->getSamplingFrequency(); - } - else - { - return 0.0; - } - } - - void setTrajectoryStateRecordingFrequency(double freq) - { - if (trajectory_monitor_) - trajectory_monitor_->setSamplingFrequency(freq); - } - - void setMaxReplanAttempts(unsigned int attempts) - { - default_max_replan_attempts_ = attempts; - } - - unsigned int getMaxReplanAttempts() const - { - return default_max_replan_attempts_; - } - - void planAndExecute(ExecutableMotionPlan& plan, const Options& opt); - void planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::msg::PlanningScene& scene_diff, const Options& opt); - - /** \brief Execute and monitor a previously created \e plan. - - In case there is no \e planning_scene or \e planning_scene_monitor set in the \e plan they will be set at the - start of the method. They are then used to monitor the execution. */ - moveit_msgs::msg::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan, bool reset_preempted = true); - - void stop(); - -private: - void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt); - bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair& path_segment); - - void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); - void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus& status); - void successfulTrajectorySegmentExecution(const ExecutableMotionPlan& plan, std::size_t index); - - const rclcpp::Node::SharedPtr node_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; - planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_; - - unsigned int default_max_replan_attempts_; - - class - { - private: - std::atomic preemption_requested_{ false }; - - public: - /** \brief check *and clear* the preemption flag - - A true return value has to trigger full execution stop, as it consumes the request */ - inline bool checkAndClear() - { - return preemption_requested_.exchange(false); - } - inline void request() - { - preemption_requested_.store(true); - } - } preempt_; - - bool new_scene_update_; - - bool execution_complete_; - bool path_became_invalid_; - - rclcpp::Logger logger_; - - // class DynamicReconfigureImpl; - // DynamicReconfigureImpl* reconfigure_impl_; -}; -} // namespace plan_execution +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp new file mode 100644 index 0000000000..e5bcddb190 --- /dev/null +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp @@ -0,0 +1,191 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +/** \brief This namespace includes functionality specific to the execution and monitoring of motion plans */ +namespace plan_execution +{ +MOVEIT_CLASS_FORWARD(PlanExecution); // Defines PlanExecutionPtr, ConstPtr, WeakPtr... etc + +class PlanExecution +{ +public: + struct Options + { + Options() : replan(false), replan_attemps(0), replan_delay(0.0) + { + } + + /// Flag indicating whether replanning is allowed + bool replan; + + /// If replanning is allowed, this variable specifies how many replanning attempts there can be, at most, before + /// failure + unsigned int replan_attemps; + + /// The amount of time to wait in between replanning attempts (in seconds) + double replan_delay; + + /// Callback for computing motion plans. This callback must always be specified. + ExecutableMotionPlanComputationFn plan_callback; + + /// Callback for repairing motion plans. This is optional. A new plan is re-computed if repairing routines are not + /// specified. + /// To aid in the repair process, the position that the controller had reached in the execution of the previous plan + /// is also passed as argument. + /// The format is the same as what the trajectory_execution_manager::TrajectoryExecutionManager reports: a pair of + /// two integers where the first + /// one is the index of the last trajectory being executed (from the sequence of trajectories specified in the + /// ExecutableMotionPlan) and the second + /// one is the index of the closest waypoint along that trajectory. + std::function& trajectory_index)> + repair_plan_callback_; + + std::function before_plan_callback_; + std::function before_execution_callback_; + std::function done_callback_; + }; + + PlanExecution(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution); + ~PlanExecution(); + + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const + { + return planning_scene_monitor_; + } + + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const + { + return trajectory_execution_manager_; + } + + double getTrajectoryStateRecordingFrequency() const + { + if (trajectory_monitor_) + { + return trajectory_monitor_->getSamplingFrequency(); + } + else + { + return 0.0; + } + } + + void setTrajectoryStateRecordingFrequency(double freq) + { + if (trajectory_monitor_) + trajectory_monitor_->setSamplingFrequency(freq); + } + + void setMaxReplanAttempts(unsigned int attempts) + { + default_max_replan_attempts_ = attempts; + } + + unsigned int getMaxReplanAttempts() const + { + return default_max_replan_attempts_; + } + + void planAndExecute(ExecutableMotionPlan& plan, const Options& opt); + void planAndExecute(ExecutableMotionPlan& plan, const moveit_msgs::msg::PlanningScene& scene_diff, const Options& opt); + + /** \brief Execute and monitor a previously created \e plan. + + In case there is no \e planning_scene or \e planning_scene_monitor set in the \e plan they will be set at the + start of the method. They are then used to monitor the execution. */ + moveit_msgs::msg::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan& plan, bool reset_preempted = true); + + void stop(); + +private: + void planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt); + bool isRemainingPathValid(const ExecutableMotionPlan& plan, const std::pair& path_segment); + + void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); + void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus& status); + void successfulTrajectorySegmentExecution(const ExecutableMotionPlan& plan, std::size_t index); + + const rclcpp::Node::SharedPtr node_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_; + + unsigned int default_max_replan_attempts_; + + class + { + private: + std::atomic preemption_requested_{ false }; + + public: + /** \brief check *and clear* the preemption flag + + A true return value has to trigger full execution stop, as it consumes the request */ + inline bool checkAndClear() + { + return preemption_requested_.exchange(false); + } + inline void request() + { + preemption_requested_.store(true); + } + } preempt_; + + bool new_scene_update_; + + bool execution_complete_; + bool path_became_invalid_; + + rclcpp::Logger logger_; + + // class DynamicReconfigureImpl; + // DynamicReconfigureImpl* reconfigure_impl_; +}; +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h index 2248857584..1773e12637 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,55 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -namespace plan_execution -{ -struct ExecutableMotionPlan; - -/** \brief Representation of a trajectory that can be executed */ -struct ExecutableTrajectory -{ - ExecutableTrajectory() : trajectory_monitoring(true) - { - } - - ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description, - std::vector controller_names = {}) - : trajectory(trajectory) - , description(description) - , trajectory_monitoring(true) - , controller_name(std::move(controller_names)) - { - } - - robot_trajectory::RobotTrajectoryPtr trajectory; - std::string description; - bool trajectory_monitoring; - collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix; - std::function effect_on_success; - std::vector controller_name; -}; - -/// A generic representation on what a computed motion plan looks like -struct ExecutableMotionPlan -{ - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; - planning_scene::PlanningSceneConstPtr planning_scene; - - std::vector plan_components; - - /// The trace of the trajectory recorded during execution - robot_trajectory::RobotTrajectoryPtr executed_trajectory; - - /// An error code reflecting what went wrong (if anything) - moveit_msgs::msg::MoveItErrorCodes error_code; -}; - -/// The signature of a function that can compute a motion plan -using ExecutableMotionPlanComputationFn = std::function; -} // namespace plan_execution +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.hpp b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.hpp new file mode 100644 index 0000000000..e755e58c30 --- /dev/null +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.hpp @@ -0,0 +1,89 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace plan_execution +{ +struct ExecutableMotionPlan; + +/** \brief Representation of a trajectory that can be executed */ +struct ExecutableTrajectory +{ + ExecutableTrajectory() : trajectory_monitoring(true) + { + } + + ExecutableTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& description, + std::vector controller_names = {}) + : trajectory(trajectory) + , description(description) + , trajectory_monitoring(true) + , controller_name(std::move(controller_names)) + { + } + + robot_trajectory::RobotTrajectoryPtr trajectory; + std::string description; + bool trajectory_monitoring; + collision_detection::AllowedCollisionMatrixConstPtr allowed_collision_matrix; + std::function effect_on_success; + std::vector controller_name; +}; + +/// A generic representation on what a computed motion plan looks like +struct ExecutableMotionPlan +{ + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + planning_scene::PlanningSceneConstPtr planning_scene; + + std::vector plan_components; + + /// The trace of the trajectory recorded during execution + robot_trajectory::RobotTrajectoryPtr executed_trajectory; + + /// An error code reflecting what went wrong (if anything) + moveit_msgs::msg::MoveItErrorCodes error_code; +}; + +/// The signature of a function that can compute a motion plan +using ExecutableMotionPlanComputationFn = std::function; +} // namespace plan_execution diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 65c46322cc..49b6efd257 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -34,12 +34,12 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -50,7 +50,7 @@ #include // #include -// #include +// #include namespace plan_execution { diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp index 1aa98f230d..76f55f71e2 100644 --- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp +++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp @@ -34,15 +34,15 @@ /* Author: Jens Petit */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include static const std::string ROBOT_DESCRIPTION = "robot_description"; diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index 0118986c5c..9f8993c613 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan */ #include -#include +#include #include using namespace std::chrono_literals; diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index 93b8443fac..7c9d15eb55 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan, Sachin Chitta */ #include -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp b/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp index f868d5f622..01eed4e5d2 100644 --- a/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp +++ b/moveit_ros/planning/planning_components_tools/src/print_planning_model_info.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp b/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp index e8a8a8ec79..72a5491fb6 100644 --- a/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp +++ b/moveit_ros/planning/planning_components_tools/src/print_planning_scene_info.cpp @@ -37,7 +37,7 @@ */ // MoveIt -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp index d083fa46f6..5f4d4194a2 100644 --- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp +++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan */ #include -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index 8b5f517535..05e451622f 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 89115bbdb6..1ab8d3ff42 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -38,246 +50,5 @@ */ #pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace planning_pipeline -{ -/** - * \brief Helper function template to load a vector of plugins - * - * \tparam TPluginClass Plugin class type, PlanningRequestAdapter or PlanningResponseAdapter - * \param plugin_loader Loader to create the requested plugin - * \param plugin_vector Vector to add the loaded plugin to - * \param plugin_names Names of the plugins to be loaded - */ -template -void loadPluginVector(const std::shared_ptr& node, - const std::unique_ptr>& plugin_loader, - std::vector>& plugin_vector, - const std::vector& plugin_names, const std::string& parameter_namespace) -{ - // Try loading a plugin for each plugin name - for (const std::string& plugin_name : plugin_names) - { - RCLCPP_INFO(node->get_logger(), "Try loading adapter '%s'", plugin_name.c_str()); - std::shared_ptr adapter; - try - { - adapter = plugin_loader->createUniqueInstance(plugin_name); - } - catch (pluginlib::PluginlibException& ex) - { - RCLCPP_FATAL(node->get_logger(), "Exception while loading planning adapter plugin '%s': %s", plugin_name.c_str(), - ex.what()); - throw; - } - // If loading was successful, initialize plugin and add to vector - if (adapter) - { - adapter->initialize(node, parameter_namespace); - plugin_vector.push_back(std::move(adapter)); - RCLCPP_INFO(node->get_logger(), "Loaded adapter '%s'", plugin_name.c_str()); - } - else - { - RCLCPP_WARN(node->get_logger(), "Failed to initialize adapter '%s'. Not adding it to the chain.", - plugin_name.c_str()); - } - } -}; - -/** \brief This class facilitates loading planning plugins and planning adapter plugins. It implements functionality to - * use the loaded plugins in a motion planning pipeline consisting of PlanningRequestAdapters (Pre-processing), a - * Planner plugin and PlanningResponseAdapters (Post-processing).*/ -class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline -{ -public: - /** \brief Given a robot model, a ROS node, and a parameter namespace, initialize the planning pipeline. The planner plugin - and adapters are provided in form of ROS parameters. The order of the elements in the adapter vectors corresponds - to the order in the motion planning pipeline. - \param model The robot model for which this pipeline is initialized. - \param node The ROS node that should be used for reading parameters needed for configuration - \param parameter_namespace parameter namespace where the planner configurations are stored - */ - PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace); - - /** \brief Given a robot model, a ROS node, and a parameter namespace, initialize the planning pipeline with the given - planner plugin and adapters. The order of the elements in the adapter vectors corresponds to the order in the - motion planning pipeline. - \param model The robot model for which this pipeline is initialized. - \param node The ROS node that should be used for reading parameters needed for configuration - \param parameter_namespace Parameter namespace where the planner configurations are stored - \param planner_plugin_names Names of the planner plugins to use - \param request_adapter_plugin_names Optional vector of RequestAdapter plugin names - \param response_adapter_plugin_names Optional vector of ResponseAdapter plugin names - */ - PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, const std::vector& planner_plugin_names, - const std::vector& request_adapter_plugin_names = std::vector(), - const std::vector& response_adapter_plugin_names = std::vector()); - - /* - BEGIN BLOCK OF DEPRECATED FUNCTIONS: TODO(sjahr): Remove after 04/2024 (6 months from merge) - */ - [[deprecated("Use generatePlan or ROS parameter API instead.")]] void displayComputedMotionPlans(bool /*flag*/){}; - [[deprecated("Use generatePlan or ROS parameter API instead.")]] void publishReceivedRequests(bool /*flag*/){}; - [[deprecated("Use generatePlan or ROS parameter API instead.")]] void checkSolutionPaths(bool /*flag*/){}; - [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getDisplayComputedMotionPlans() const - { - return false; - } - [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getPublishReceivedRequests() const - { - return false; - } - [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getCheckSolutionPaths() const - { - return false; - } - [[deprecated( - "Please use getResponseAdapterPluginNames() or getRequestAdapterPluginNames().")]] const std::vector& - getAdapterPluginNames() const - { - return pipeline_parameters_.request_adapters; - } - [[deprecated("`check_solution_paths` and `display_computed_motion_plans` are deprecated. To validate the solution " - "please add the ValidatePath response adapter and to publish the path use the DisplayMotionPath " - "response adapter to your pipeline.")]] bool - generatePlan(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, - const planning_interface::MotionPlanRequest& /*req*/, planning_interface::MotionPlanResponse& /*res*/, - const bool /*publish_received_requests*/, const bool /*check_solution_paths*/, - const bool /*display_computed_motion_plans*/) const - { - return false; - } - [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const - { - return pipeline_parameters_.planning_plugins.at(0); - } - [[deprecated( - "Please use 'getPlannerManager(const std::string& planner_name)'.")]] const planning_interface::PlannerManagerPtr& - getPlannerManager() - { - return planner_map_.at(pipeline_parameters_.planning_plugins.at(0)); - } - /* - END BLOCK OF DEPRECATED FUNCTIONS - */ - - /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in - sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for - motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether - received requests should be published just before beginning processing (useful for debugging) - */ - [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - const bool publish_received_requests = false) const; - - /** \brief Request termination, if a generatePlan() function is currently computing plans */ - void terminate() const; - - /** \brief Get the names of the planning plugins used */ - [[nodiscard]] const std::vector& getPlannerPluginNames() const - { - return pipeline_parameters_.planning_plugins; - } - - /** \brief Get the names of the planning request adapter plugins used */ - [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const - { - return pipeline_parameters_.request_adapters; - } - - /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ - [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const - { - return pipeline_parameters_.response_adapters; - } - - /** \brief Get the robot model that this pipeline is using */ - [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - /** \brief Get current status of the planning pipeline */ - [[nodiscard]] bool isActive() const - { - return active_; - } - - /** \brief Return the parameter namespace as name of the planning pipeline. */ - [[nodiscard]] std::string getName() const - { - return parameter_namespace_; - } - - /** \brief Get access to planner plugin */ - const planning_interface::PlannerManagerPtr getPlannerManager(const std::string& planner_name) - { - if (planner_map_.find(planner_name) == planner_map_.end()) - { - RCLCPP_ERROR(node_->get_logger(), "Cannot retrieve planner because '%s' does not exist.", planner_name.c_str()); - return nullptr; - } - return planner_map_.at(planner_name); - } - -private: - /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance - void configure(); - - /** - * @brief Helper function to publish the planning pipeline state during the planning process - * - * @param req Current request to publish - * @param res Current pipeline result - * @param pipeline_stage Last pipeline stage that got invoked - */ - void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, const planning_interface::MotionPlanResponse& res, - const std::string& pipeline_stage) const; - - // Flag that indicates whether or not the planning pipeline is currently solving a planning problem - mutable std::atomic active_; - - // ROS node and parameters - std::shared_ptr node_; - const std::string parameter_namespace_; - planning_pipeline_parameters::Params pipeline_parameters_; - - // Planner plugin - std::unique_ptr> planner_plugin_loader_; - std::unordered_map planner_map_; - - // Plan request adapters - std::unique_ptr> request_adapter_plugin_loader_; - std::vector planning_request_adapter_vector_; - - // Plan response adapters - std::unique_ptr> response_adapter_plugin_loader_; - std::vector planning_response_adapter_vector_; - - // Robot model - moveit::core::RobotModelConstPtr robot_model_; - - /// Publish the planning pipeline progress - rclcpp::Publisher::SharedPtr progress_publisher_; - - rclcpp::Logger logger_; -}; - -MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc -} // namespace planning_pipeline +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.hpp b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.hpp new file mode 100644 index 0000000000..18abda399b --- /dev/null +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.hpp @@ -0,0 +1,283 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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, Sebastian Jahr + Description: Implementation of a MoveIt planning pipeline composed of a planner plugin and request and response + adapter plugins. +*/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning_pipeline +{ +/** + * \brief Helper function template to load a vector of plugins + * + * \tparam TPluginClass Plugin class type, PlanningRequestAdapter or PlanningResponseAdapter + * \param plugin_loader Loader to create the requested plugin + * \param plugin_vector Vector to add the loaded plugin to + * \param plugin_names Names of the plugins to be loaded + */ +template +void loadPluginVector(const std::shared_ptr& node, + const std::unique_ptr>& plugin_loader, + std::vector>& plugin_vector, + const std::vector& plugin_names, const std::string& parameter_namespace) +{ + // Try loading a plugin for each plugin name + for (const std::string& plugin_name : plugin_names) + { + RCLCPP_INFO(node->get_logger(), "Try loading adapter '%s'", plugin_name.c_str()); + std::shared_ptr adapter; + try + { + adapter = plugin_loader->createUniqueInstance(plugin_name); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_FATAL(node->get_logger(), "Exception while loading planning adapter plugin '%s': %s", plugin_name.c_str(), + ex.what()); + throw; + } + // If loading was successful, initialize plugin and add to vector + if (adapter) + { + adapter->initialize(node, parameter_namespace); + plugin_vector.push_back(std::move(adapter)); + RCLCPP_INFO(node->get_logger(), "Loaded adapter '%s'", plugin_name.c_str()); + } + else + { + RCLCPP_WARN(node->get_logger(), "Failed to initialize adapter '%s'. Not adding it to the chain.", + plugin_name.c_str()); + } + } +}; + +/** \brief This class facilitates loading planning plugins and planning adapter plugins. It implements functionality to + * use the loaded plugins in a motion planning pipeline consisting of PlanningRequestAdapters (Pre-processing), a + * Planner plugin and PlanningResponseAdapters (Post-processing).*/ +class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline +{ +public: + /** \brief Given a robot model, a ROS node, and a parameter namespace, initialize the planning pipeline. The planner plugin + and adapters are provided in form of ROS parameters. The order of the elements in the adapter vectors corresponds + to the order in the motion planning pipeline. + \param model The robot model for which this pipeline is initialized. + \param node The ROS node that should be used for reading parameters needed for configuration + \param parameter_namespace parameter namespace where the planner configurations are stored + */ + PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, + const std::string& parameter_namespace); + + /** \brief Given a robot model, a ROS node, and a parameter namespace, initialize the planning pipeline with the given + planner plugin and adapters. The order of the elements in the adapter vectors corresponds to the order in the + motion planning pipeline. + \param model The robot model for which this pipeline is initialized. + \param node The ROS node that should be used for reading parameters needed for configuration + \param parameter_namespace Parameter namespace where the planner configurations are stored + \param planner_plugin_names Names of the planner plugins to use + \param request_adapter_plugin_names Optional vector of RequestAdapter plugin names + \param response_adapter_plugin_names Optional vector of ResponseAdapter plugin names + */ + PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, + const std::string& parameter_namespace, const std::vector& planner_plugin_names, + const std::vector& request_adapter_plugin_names = std::vector(), + const std::vector& response_adapter_plugin_names = std::vector()); + + /* + BEGIN BLOCK OF DEPRECATED FUNCTIONS: TODO(sjahr): Remove after 04/2024 (6 months from merge) + */ + [[deprecated("Use generatePlan or ROS parameter API instead.")]] void displayComputedMotionPlans(bool /*flag*/){}; + [[deprecated("Use generatePlan or ROS parameter API instead.")]] void publishReceivedRequests(bool /*flag*/){}; + [[deprecated("Use generatePlan or ROS parameter API instead.")]] void checkSolutionPaths(bool /*flag*/){}; + [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getDisplayComputedMotionPlans() const + { + return false; + } + [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getPublishReceivedRequests() const + { + return false; + } + [[deprecated("Use generatePlan or ROS parameter API instead.")]] bool getCheckSolutionPaths() const + { + return false; + } + [[deprecated( + "Please use getResponseAdapterPluginNames() or getRequestAdapterPluginNames().")]] const std::vector& + getAdapterPluginNames() const + { + return pipeline_parameters_.request_adapters; + } + [[deprecated("`check_solution_paths` and `display_computed_motion_plans` are deprecated. To validate the solution " + "please add the ValidatePath response adapter and to publish the path use the DisplayMotionPath " + "response adapter to your pipeline.")]] bool + generatePlan(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& /*req*/, planning_interface::MotionPlanResponse& /*res*/, + const bool /*publish_received_requests*/, const bool /*check_solution_paths*/, + const bool /*display_computed_motion_plans*/) const + { + return false; + } + [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const + { + return pipeline_parameters_.planning_plugins.at(0); + } + [[deprecated( + "Please use 'getPlannerManager(const std::string& planner_name)'.")]] const planning_interface::PlannerManagerPtr& + getPlannerManager() + { + return planner_map_.at(pipeline_parameters_.planning_plugins.at(0)); + } + /* + END BLOCK OF DEPRECATED FUNCTIONS + */ + + /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in + sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for + motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether + received requests should be published just before beginning processing (useful for debugging) + */ + [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + const bool publish_received_requests = false) const; + + /** \brief Request termination, if a generatePlan() function is currently computing plans */ + void terminate() const; + + /** \brief Get the names of the planning plugins used */ + [[nodiscard]] const std::vector& getPlannerPluginNames() const + { + return pipeline_parameters_.planning_plugins; + } + + /** \brief Get the names of the planning request adapter plugins used */ + [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const + { + return pipeline_parameters_.request_adapters; + } + + /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ + [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const + { + return pipeline_parameters_.response_adapters; + } + + /** \brief Get the robot model that this pipeline is using */ + [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + /** \brief Get current status of the planning pipeline */ + [[nodiscard]] bool isActive() const + { + return active_; + } + + /** \brief Return the parameter namespace as name of the planning pipeline. */ + [[nodiscard]] std::string getName() const + { + return parameter_namespace_; + } + + /** \brief Get access to planner plugin */ + const planning_interface::PlannerManagerPtr getPlannerManager(const std::string& planner_name) + { + if (planner_map_.find(planner_name) == planner_map_.end()) + { + RCLCPP_ERROR(node_->get_logger(), "Cannot retrieve planner because '%s' does not exist.", planner_name.c_str()); + return nullptr; + } + return planner_map_.at(planner_name); + } + +private: + /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance + void configure(); + + /** + * @brief Helper function to publish the planning pipeline state during the planning process + * + * @param req Current request to publish + * @param res Current pipeline result + * @param pipeline_stage Last pipeline stage that got invoked + */ + void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, const planning_interface::MotionPlanResponse& res, + const std::string& pipeline_stage) const; + + // Flag that indicates whether or not the planning pipeline is currently solving a planning problem + mutable std::atomic active_; + + // ROS node and parameters + std::shared_ptr node_; + const std::string parameter_namespace_; + planning_pipeline_parameters::Params pipeline_parameters_; + + // Planner plugin + std::unique_ptr> planner_plugin_loader_; + std::unordered_map planner_map_; + + // Plan request adapters + std::unique_ptr> request_adapter_plugin_loader_; + std::vector planning_request_adapter_vector_; + + // Plan response adapters + std::unique_ptr> response_adapter_plugin_loader_; + std::vector planning_response_adapter_vector_; + + // Robot model + moveit::core::RobotModelConstPtr robot_model_; + + /// Publish the planning pipeline progress + rclcpp::Publisher::SharedPtr progress_publisher_; + + rclcpp::Logger logger_; +}; + +MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc +} // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 882ab97b13..34f30375c8 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan, Sebastian Jahr */ -#include +#include #include #include diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp index 1b5d8986ac..4a715909b3 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp @@ -35,9 +35,9 @@ /* Author: Sebastian Jahr */ #include -#include -#include -#include +#include +#include +#include #include namespace planning_pipeline_test diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp index 7100a69f09..50029d8770 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -36,8 +36,8 @@ #include -#include -#include +#include +#include namespace { diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.h b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.h new file mode 100644 index 0000000000..fb2369713f --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.h @@ -0,0 +1,52 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Desc: A thread safe container to store motion plan responses */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp index 779a7dd9d4..b1efd40c7a 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp @@ -37,8 +37,8 @@ #pragma once -#include -#include +#include +#include namespace moveit { diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.h b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.h new file mode 100644 index 0000000000..1e7fefa558 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.h @@ -0,0 +1,52 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Desc: Functions to create and use a map of PlanningPipelines to solve MotionPlanRequests */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp index 9c9760e2e4..23a62d0867 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -38,10 +38,10 @@ #pragma once #include -#include -#include -#include -#include +#include +#include +#include +#include namespace moveit { diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.h b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.h new file mode 100644 index 0000000000..bf57e5b975 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.h @@ -0,0 +1,52 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Desc: Solution selection function implementations */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.h b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.h new file mode 100644 index 0000000000..72239439d9 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.h @@ -0,0 +1,52 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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: AndyZe, Sebastian Jahr + Desc: Stopping criterion function implementations */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp index 6aed9555b2..e0b60847c1 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp @@ -37,7 +37,7 @@ * that is the case, a warning is created but the planning process is not interrupted. */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp index bde29d5e12..315084ebef 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp @@ -43,9 +43,9 @@ * outside its limits for it to be “fixable”. */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp index 3f3d22f6f2..99ccf820d7 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp @@ -36,9 +36,9 @@ * Desc: This adapter checks if the start state is in collision. */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index 55b0562b79..b6316b5ddb 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -38,8 +38,8 @@ attached object are not known to a planner. */ -#include -#include +#include +#include #include #include diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp index de0510ef3a..a21dffe61e 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp @@ -37,7 +37,7 @@ * The default workspace is a cube whose edge length is defined with a ROS 2 parameter. */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index fd6a5b4204..5c566bd75b 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -38,8 +38,8 @@ */ #include -#include -#include +#include +#include #include #include diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp index 7b8ed0b13c..b5097dce5f 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Michael Ferguson */ -#include -#include +#include +#include #include #include diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp index e5fcb1bc03..bc6fa5f605 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp @@ -36,8 +36,8 @@ Desc: Response adapter to display the motion path in RVIZ by publishing as EE pose marker array via ROS topic. */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp index c5e47cda1f..1284ba821b 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp @@ -36,11 +36,11 @@ Desc: Response adapter that checks a path for validity (collision avoidance, feasibility and constraint satisfaction) */ -#include +#include #include #include #include -#include +#include #include namespace default_planning_response_adapters diff --git a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp index 0e0c873de6..e3ed5af569 100644 --- a/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp +++ b/moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h index b1ff502d9c..ddb74381d8 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,316 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#include - -namespace planning_scene_monitor -{ -using JointStateUpdateCallback = std::function; - -/** @class CurrentStateMonitor - @brief Monitors the joint_states topic and tf to maintain the current state of the robot. */ -class CurrentStateMonitor -{ -public: - /** - * @brief Dependency injection class for testing the CurrentStateMonitor - */ - class MiddlewareHandle - { - public: - using TfCallback = std::function; - - /** - * @brief Destroys the object. - */ - virtual ~MiddlewareHandle() = default; - - /** - * @brief Get the current time - * - * @return Time object representing the time when this is called - */ - virtual rclcpp::Time now() const = 0; - - /** - * @brief Creates a joint state subscription - * - * @param[in] topic The topic - * @param[in] callback The callback - */ - virtual void createJointStateSubscription(const std::string& topic, JointStateUpdateCallback callback) = 0; - - /** - * @brief Creates a static transform message subscription - * - * @param[in] callback The callback - */ - virtual void createStaticTfSubscription(TfCallback callback) = 0; - - /** - * @brief Creates a dynamic transform message subscription - * - * @param[in] callback The callback - */ - virtual void createDynamicTfSubscription(TfCallback callback) = 0; - - /** - * @brief Reset the joint state subscription - */ - virtual void resetJointStateSubscription() = 0; - - /** - * @brief Get the joint state topic name - * - * @return The joint state topic name. - */ - virtual std::string getJointStateTopicName() const = 0; - - /** - * @brief Block for the specified amount of time. - * - * @param[in] nanoseconds The nanoseconds to sleep - * - * @return True if sleep happened as expected. - */ - virtual bool sleepFor(const std::chrono::nanoseconds& nanoseconds) const = 0; - - /** - * @brief Uses rclcpp::ok to check the context status - * - * @return Return of rclcpp::ok - */ - virtual bool ok() const = 0; - - /** - * @brief Get the static transform topic name - * - * @return The static transform topic name. - */ - virtual std::string getStaticTfTopicName() const = 0; - - /** - * @brief Get the dynamic transform topic name - * - * @return The dynamic transform topic name. - */ - virtual std::string getDynamicTfTopicName() const = 0; - - /** - * @brief Reset the static & dynamic transform subscriptions - */ - virtual void resetTfSubscriptions() = 0; - }; - - /** @brief Constructor. - * @param middleware_handle The ros middleware handle - * @param robot_model The current kinematic model to build on - * @param tf_buffer A pointer to the tf2_ros Buffer to use - * @param use_sim_time True when the time is abstracted - */ - CurrentStateMonitor(std::unique_ptr middleware_handle, - const moveit::core::RobotModelConstPtr& robot_model, - const std::shared_ptr& tf_buffer, bool use_sim_time); - - /** @brief Constructor. - * @param node A shared_ptr to a node used for subscription to joint_states_topic - * @param robot_model The current kinematic model to build on - * @param tf_buffer A pointer to the tf2_ros Buffer to use - * @param use_sim_time True when the time is abstracted - */ - CurrentStateMonitor(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, - const std::shared_ptr& tf_buffer, bool use_sim_time); - - ~CurrentStateMonitor(); - - /** @brief Start monitoring joint states on a particular topic - * @param joint_states_topic The topic name for joint states (defaults to "joint_states") - */ - void startStateMonitor(const std::string& joint_states_topic = "joint_states"); - - /** @brief Stop monitoring the "joint_states" topic - */ - void stopStateMonitor(); - - /** @brief Check if the state monitor is started */ - bool isActive() const; - - /** @brief Get the RobotModel for which we are monitoring state */ - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - /** @brief Get the name of the topic being monitored. Returns an empty string if the monitor is inactive. */ - std::string getMonitoredTopic() const; - - /** @brief Query whether we have joint state information for all DOFs in the kinematic model - * @return False if we have no joint state information for one or more of the joints - */ - inline bool haveCompleteState() const - { - return haveCompleteStateHelper(rclcpp::Time(0, 0, RCL_ROS_TIME), nullptr); - } - - /** @brief Query whether we have joint state information for all DOFs in the kinematic model - * @param oldest_allowed_update_time All joint information must be from this time or more current - * @return False if we have no joint state information for one of the joints or if our state - * information is more than \e age old*/ - inline bool haveCompleteState(const rclcpp::Time& oldest_allowed_update_time) const - { - return haveCompleteStateHelper(oldest_allowed_update_time, nullptr); - } - - /** @brief Query whether we have joint state information for all DOFs in the kinematic model - * @param age Joint information must be at most this old - * @return False if we have no joint state information for one of the joints or if our state - * information is more than \e age old - */ - inline bool haveCompleteState(const rclcpp::Duration& age) const - { - return haveCompleteStateHelper(middleware_handle_->now() - age, nullptr); - } - - /** @brief Query whether we have joint state information for all DOFs in the kinematic model - * @param missing_joints Returns the list of joints that are missing - * @return False if we have no joint state information for one or more of the joints - */ - inline bool haveCompleteState(std::vector& missing_joints) const - { - return haveCompleteStateHelper(rclcpp::Time(0, 0, RCL_ROS_TIME), &missing_joints); - } - - /** @brief Query whether we have joint state information for all DOFs in the kinematic model - * @param oldest_allowed_update_time All joint information must be from this time or more current - * @param missing_joints Returns the list of joints that are missing - * @return False if we have no joint state information for one of the joints or if our state - * information is more than \e age old*/ - inline bool haveCompleteState(const rclcpp::Time& oldest_allowed_update_time, - std::vector& missing_joints) const - { - return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints); - } - - /** @brief Query whether we have joint state information for all DOFs in the kinematic model - * @return False if we have no joint state information for one of the joints or if our state - * information is more than \e age old - */ - inline bool haveCompleteState(const rclcpp::Duration& age, std::vector& missing_joints) const - { - return haveCompleteStateHelper(middleware_handle_->now() - age, &missing_joints); - } - - /** @brief Get the current state - * @return Returns the current state */ - moveit::core::RobotStatePtr getCurrentState() const; - - /** @brief Set the state \e upd to the current state maintained by this class. */ - void setToCurrentState(moveit::core::RobotState& upd) const; - - /** @brief Get the time stamp for the current state */ - rclcpp::Time getCurrentStateTime() const; - - /** @brief Get the current state and its time stamp - * @return Returns a pair of the current state and its time stamp */ - std::pair getCurrentStateAndTime() const; - - /** @brief Get the current state values as a map from joint names to joint state values - * @return Returns the map from joint names to joint state values*/ - std::map getCurrentStateValues() const; - - /** @brief Wait for at most \e wait_time_s seconds (default 1s) for a robot state more recent than t - * @return true on success, false if up-to-date robot state wasn't received within \e wait_time_s - */ - bool waitForCurrentState(const rclcpp::Time& t = rclcpp::Clock(RCL_ROS_TIME).now(), double wait_time_s = 1.0) const; - - /** @brief Wait for at most \e wait_time_s seconds until the complete robot state is known. - @return true if the full state is known */ - bool waitForCompleteState(double wait_time_s) const; - - /** @brief Wait for at most \e wait_time_s seconds until the joint values from the group \e group are known. Return - * true if values for all joints in \e group are known */ - bool waitForCompleteState(const std::string& group, double wait_time_s) const; - - /** @brief Get the time point when the monitor was started */ - const rclcpp::Time& getMonitorStartTime() const - { - return monitor_start_time_; - } - - /** @brief Add a function that will be called whenever the joint state is updated*/ - void addUpdateCallback(const JointStateUpdateCallback& fn); - - /** @brief Clear the functions to be called when an update to the joint state is received */ - void clearUpdateCallbacks(); - - /** @brief When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, - * if the difference is less than a specified value (labeled the "allowed bounds error"). - * This value can be set using this function. - * @param error The specified value for the "allowed bounds error". The default is machine precision. */ - void setBoundsError(double error) - { - error_ = (error > 0) ? error : -error; - } - - /** @brief When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, - * if the difference is less than a specified value (labeled the "allowed bounds error"). - * @return The stored value for the "allowed bounds error" - */ - double getBoundsError() const - { - return error_; - } - - /** @brief Allow the joint_state arrays velocity and effort to be copied into the robot state - * this is useful in some but not all applications - */ - void enableCopyDynamics(bool enabled) - { - copy_dynamics_ = enabled; - } - -private: - bool haveCompleteStateHelper(const rclcpp::Time& oldest_allowed_update_time, - std::vector* missing_joints) const; - - void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state); - void updateMultiDofJoints(); - void transformCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr& msg, const bool is_static); - - std::unique_ptr middleware_handle_; - std::shared_ptr tf_buffer_; - moveit::core::RobotModelConstPtr robot_model_; - moveit::core::RobotState robot_state_; - std::map joint_time_; - bool state_monitor_started_; - bool copy_dynamics_; // Copy velocity and effort from joint_state - rclcpp::Time monitor_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - double error_; - rclcpp::Time current_state_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - - mutable std::mutex state_update_lock_; - mutable std::condition_variable state_update_condition_; - std::vector update_callbacks_; - - bool use_sim_time_; - - rclcpp::Logger logger_; -}; - -MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc -} // namespace planning_scene_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp new file mode 100644 index 0000000000..273d4b1049 --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp @@ -0,0 +1,350 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +namespace planning_scene_monitor +{ +using JointStateUpdateCallback = std::function; + +/** @class CurrentStateMonitor + @brief Monitors the joint_states topic and tf to maintain the current state of the robot. */ +class CurrentStateMonitor +{ +public: + /** + * @brief Dependency injection class for testing the CurrentStateMonitor + */ + class MiddlewareHandle + { + public: + using TfCallback = std::function; + + /** + * @brief Destroys the object. + */ + virtual ~MiddlewareHandle() = default; + + /** + * @brief Get the current time + * + * @return Time object representing the time when this is called + */ + virtual rclcpp::Time now() const = 0; + + /** + * @brief Creates a joint state subscription + * + * @param[in] topic The topic + * @param[in] callback The callback + */ + virtual void createJointStateSubscription(const std::string& topic, JointStateUpdateCallback callback) = 0; + + /** + * @brief Creates a static transform message subscription + * + * @param[in] callback The callback + */ + virtual void createStaticTfSubscription(TfCallback callback) = 0; + + /** + * @brief Creates a dynamic transform message subscription + * + * @param[in] callback The callback + */ + virtual void createDynamicTfSubscription(TfCallback callback) = 0; + + /** + * @brief Reset the joint state subscription + */ + virtual void resetJointStateSubscription() = 0; + + /** + * @brief Get the joint state topic name + * + * @return The joint state topic name. + */ + virtual std::string getJointStateTopicName() const = 0; + + /** + * @brief Block for the specified amount of time. + * + * @param[in] nanoseconds The nanoseconds to sleep + * + * @return True if sleep happened as expected. + */ + virtual bool sleepFor(const std::chrono::nanoseconds& nanoseconds) const = 0; + + /** + * @brief Uses rclcpp::ok to check the context status + * + * @return Return of rclcpp::ok + */ + virtual bool ok() const = 0; + + /** + * @brief Get the static transform topic name + * + * @return The static transform topic name. + */ + virtual std::string getStaticTfTopicName() const = 0; + + /** + * @brief Get the dynamic transform topic name + * + * @return The dynamic transform topic name. + */ + virtual std::string getDynamicTfTopicName() const = 0; + + /** + * @brief Reset the static & dynamic transform subscriptions + */ + virtual void resetTfSubscriptions() = 0; + }; + + /** @brief Constructor. + * @param middleware_handle The ros middleware handle + * @param robot_model The current kinematic model to build on + * @param tf_buffer A pointer to the tf2_ros Buffer to use + * @param use_sim_time True when the time is abstracted + */ + CurrentStateMonitor(std::unique_ptr middleware_handle, + const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tf_buffer, bool use_sim_time); + + /** @brief Constructor. + * @param node A shared_ptr to a node used for subscription to joint_states_topic + * @param robot_model The current kinematic model to build on + * @param tf_buffer A pointer to the tf2_ros Buffer to use + * @param use_sim_time True when the time is abstracted + */ + CurrentStateMonitor(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tf_buffer, bool use_sim_time); + + ~CurrentStateMonitor(); + + /** @brief Start monitoring joint states on a particular topic + * @param joint_states_topic The topic name for joint states (defaults to "joint_states") + */ + void startStateMonitor(const std::string& joint_states_topic = "joint_states"); + + /** @brief Stop monitoring the "joint_states" topic + */ + void stopStateMonitor(); + + /** @brief Check if the state monitor is started */ + bool isActive() const; + + /** @brief Get the RobotModel for which we are monitoring state */ + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + /** @brief Get the name of the topic being monitored. Returns an empty string if the monitor is inactive. */ + std::string getMonitoredTopic() const; + + /** @brief Query whether we have joint state information for all DOFs in the kinematic model + * @return False if we have no joint state information for one or more of the joints + */ + inline bool haveCompleteState() const + { + return haveCompleteStateHelper(rclcpp::Time(0, 0, RCL_ROS_TIME), nullptr); + } + + /** @brief Query whether we have joint state information for all DOFs in the kinematic model + * @param oldest_allowed_update_time All joint information must be from this time or more current + * @return False if we have no joint state information for one of the joints or if our state + * information is more than \e age old*/ + inline bool haveCompleteState(const rclcpp::Time& oldest_allowed_update_time) const + { + return haveCompleteStateHelper(oldest_allowed_update_time, nullptr); + } + + /** @brief Query whether we have joint state information for all DOFs in the kinematic model + * @param age Joint information must be at most this old + * @return False if we have no joint state information for one of the joints or if our state + * information is more than \e age old + */ + inline bool haveCompleteState(const rclcpp::Duration& age) const + { + return haveCompleteStateHelper(middleware_handle_->now() - age, nullptr); + } + + /** @brief Query whether we have joint state information for all DOFs in the kinematic model + * @param missing_joints Returns the list of joints that are missing + * @return False if we have no joint state information for one or more of the joints + */ + inline bool haveCompleteState(std::vector& missing_joints) const + { + return haveCompleteStateHelper(rclcpp::Time(0, 0, RCL_ROS_TIME), &missing_joints); + } + + /** @brief Query whether we have joint state information for all DOFs in the kinematic model + * @param oldest_allowed_update_time All joint information must be from this time or more current + * @param missing_joints Returns the list of joints that are missing + * @return False if we have no joint state information for one of the joints or if our state + * information is more than \e age old*/ + inline bool haveCompleteState(const rclcpp::Time& oldest_allowed_update_time, + std::vector& missing_joints) const + { + return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints); + } + + /** @brief Query whether we have joint state information for all DOFs in the kinematic model + * @return False if we have no joint state information for one of the joints or if our state + * information is more than \e age old + */ + inline bool haveCompleteState(const rclcpp::Duration& age, std::vector& missing_joints) const + { + return haveCompleteStateHelper(middleware_handle_->now() - age, &missing_joints); + } + + /** @brief Get the current state + * @return Returns the current state */ + moveit::core::RobotStatePtr getCurrentState() const; + + /** @brief Set the state \e upd to the current state maintained by this class. */ + void setToCurrentState(moveit::core::RobotState& upd) const; + + /** @brief Get the time stamp for the current state */ + rclcpp::Time getCurrentStateTime() const; + + /** @brief Get the current state and its time stamp + * @return Returns a pair of the current state and its time stamp */ + std::pair getCurrentStateAndTime() const; + + /** @brief Get the current state values as a map from joint names to joint state values + * @return Returns the map from joint names to joint state values*/ + std::map getCurrentStateValues() const; + + /** @brief Wait for at most \e wait_time_s seconds (default 1s) for a robot state more recent than t + * @return true on success, false if up-to-date robot state wasn't received within \e wait_time_s + */ + bool waitForCurrentState(const rclcpp::Time& t = rclcpp::Clock(RCL_ROS_TIME).now(), double wait_time_s = 1.0) const; + + /** @brief Wait for at most \e wait_time_s seconds until the complete robot state is known. + @return true if the full state is known */ + bool waitForCompleteState(double wait_time_s) const; + + /** @brief Wait for at most \e wait_time_s seconds until the joint values from the group \e group are known. Return + * true if values for all joints in \e group are known */ + bool waitForCompleteState(const std::string& group, double wait_time_s) const; + + /** @brief Get the time point when the monitor was started */ + const rclcpp::Time& getMonitorStartTime() const + { + return monitor_start_time_; + } + + /** @brief Add a function that will be called whenever the joint state is updated*/ + void addUpdateCallback(const JointStateUpdateCallback& fn); + + /** @brief Clear the functions to be called when an update to the joint state is received */ + void clearUpdateCallbacks(); + + /** @brief When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, + * if the difference is less than a specified value (labeled the "allowed bounds error"). + * This value can be set using this function. + * @param error The specified value for the "allowed bounds error". The default is machine precision. */ + void setBoundsError(double error) + { + error_ = (error > 0) ? error : -error; + } + + /** @brief When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, + * if the difference is less than a specified value (labeled the "allowed bounds error"). + * @return The stored value for the "allowed bounds error" + */ + double getBoundsError() const + { + return error_; + } + + /** @brief Allow the joint_state arrays velocity and effort to be copied into the robot state + * this is useful in some but not all applications + */ + void enableCopyDynamics(bool enabled) + { + copy_dynamics_ = enabled; + } + +private: + bool haveCompleteStateHelper(const rclcpp::Time& oldest_allowed_update_time, + std::vector* missing_joints) const; + + void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state); + void updateMultiDofJoints(); + void transformCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr& msg, const bool is_static); + + std::unique_ptr middleware_handle_; + std::shared_ptr tf_buffer_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotState robot_state_; + std::map joint_time_; + bool state_monitor_started_; + bool copy_dynamics_; // Copy velocity and effort from joint_state + rclcpp::Time monitor_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + double error_; + rclcpp::Time current_state_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + mutable std::mutex state_update_lock_; + mutable std::condition_variable state_update_condition_; + std::vector update_callbacks_; + + bool use_sim_time_; + + rclcpp::Logger logger_; +}; + +MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.h new file mode 100644 index 0000000000..11ed2aa69b --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik, 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 PickNik 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: Tyler Weaver */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp index c5f33cfc96..ee7a1d2a38 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp @@ -42,7 +42,7 @@ #include #include -#include +#include namespace planning_scene_monitor { diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index 54007cb85f..966cf41449 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,706 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace planning_scene_monitor -{ -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc - -/** - * @brief PlanningSceneMonitor - * Subscribes to the topic \e planning_scene */ -class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor -{ -public: - /** - * @brief PlanningSceneMonitor cannot be copy-constructed - */ - PlanningSceneMonitor(const PlanningSceneMonitor&) = delete; - - /** - * @brief PlanningSceneMonitor cannot be copy-assigned - */ - PlanningSceneMonitor& operator=(const PlanningSceneMonitor&) = delete; - - enum SceneUpdateType - { - /** \brief No update */ - UPDATE_NONE = 0, - - /** \brief The state in the monitored scene was updated */ - UPDATE_STATE = 1, - - /** \brief The maintained set of fixed transforms in the monitored scene was updated */ - UPDATE_TRANSFORMS = 2, - - /** \brief The geometry of the scene was updated. This includes receiving new octomaps, collision objects, attached - objects, scene geometry, etc. */ - UPDATE_GEOMETRY = 4, - - /** \brief The entire scene was updated */ - UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY - }; - - /// The name of the topic used by default for receiving joint states - static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states" - - /// The name of the topic used by default for attached collision objects - static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object" - - /// The name of the topic used by default for receiving collision objects in the world - static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object" - - /// The name of the topic used by default for receiving geometry information about a planning scene (complete - /// overwrite of world geometry) - static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world" - - /// The name of the topic used by default for receiving full planning scenes or planning scene diffs - static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene" - - /// The name of the service used by default for requesting full planning scene state - static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene" - - /// The name of the topic used by default for publishing the monitored planning scene (this is without "/" in the - /// name, so the topic is prefixed by the node name) - static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene" - - /** @brief Constructor - * @param robot_description The name of the ROS parameter that contains the URDF (in string format) - * @param name A name identifying this planning scene monitor - */ - PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, - const std::string& name = ""); - - /** @brief Constructor - * @param rml A pointer to a kinematic model loader - * @param name A name identifying this planning scene monitor - */ - PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml, - const std::string& name = ""); - - /** @brief Constructor - * @param scene The scene instance to maintain up to date with monitored information - * @param robot_description The name of the ROS parameter that contains the URDF (in string format) - * @param name A name identifying this planning scene monitor - */ - PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const std::string& robot_description, const std::string& name = ""); - - /** @brief Constructor - * @param scene The scene instance to maintain up to date with monitored information - * @param rml A pointer to a kinematic model loader - * @param name A name identifying this planning scene monitor - */ - PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const robot_model_loader::RobotModelLoaderPtr& rml, const std::string& name = ""); - - ~PlanningSceneMonitor(); - - /** \brief Get the name of this monitor */ - const std::string& getName() const - { - return monitor_name_; - } - - /** \brief Get the user kinematic model loader */ - const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const - { - return rm_loader_; - } - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - /** @brief Avoid this function! Returns an @b - * unsafe pointer to the current planning scene. - * @warning Most likely you do not want to call this function - * directly. PlanningSceneMonitor has a background thread - * which repeatedly updates and clobbers various contents - * of its internal PlanningScene instance. This function - * just returns a pointer to that dynamic internal object. - * The correct thing is usually to use a - * LockedPlanningSceneRO or LockedPlanningSceneRW, which - * locks the PlanningSceneMonitor and provides safe access - * to the PlanningScene object. - * @see LockedPlanningSceneRO - * @see LockedPlanningSceneRW. - * @return A pointer to the current planning scene.*/ - const planning_scene::PlanningScenePtr& getPlanningScene() - { - return scene_; - } - - /*! @brief Avoid this function! Returns an @b - * unsafe pointer to the current planning scene. - * @copydetails PlanningSceneMonitor::getPlanningScene() */ - const planning_scene::PlanningSceneConstPtr& getPlanningScene() const - { - return scene_const_; - } - - /** @brief Return true if the scene \e scene can be updated directly - or indirectly by this monitor. This function will return true if - the pointer of the scene is the same as the one maintained, - or if a parent of the scene is the one maintained. */ - bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const; - - /** @brief Return true if the scene \e scene can be updated directly - or indirectly by this monitor. This function will return true if - the pointer of the scene is the same as the one maintained, - or if a parent of the scene is the one maintained. */ - bool updatesScene(const planning_scene::PlanningScenePtr& scene) const; - - /** @brief Get the stored robot description - * @return An instance of the stored robot description*/ - const std::string& getRobotDescription() const - { - return robot_description_; - } - - /// Get the default robot padding - double getDefaultRobotPadding() const - { - return default_robot_padd_; - } - - /// Get the default robot scaling - double getDefaultRobotScale() const - { - return default_robot_scale_; - } - - /// Get the default object padding - double getDefaultObjectPadding() const - { - return default_object_padd_; - } - - /// Get the default attached padding - double getDefaultAttachedObjectPadding() const - { - return default_attached_padd_; - } - - /** @brief Get the instance of the TF client that was passed to the constructor of this class. */ - const std::shared_ptr& getTFClient() const - { - return tf_buffer_; - } - - /** \brief By default, the maintained planning scene does not reason about diffs. When the flag passed in is true, the - maintained - scene starts counting diffs. Future updates to the planning scene will be stored as diffs and can be retrieved as - such. Setting the flag to false restores the default behaviour. Maintaining diffs is automatically enabled when - publishing planning scenes. */ - void monitorDiffs(bool flag); - - /** \brief Start publishing the maintained planning scene. The first message set out is a complete planning scene. - Diffs are sent afterwards on updates specified by the \e event bitmask. For UPDATE_SCENE, the full scene is always - sent. */ - void startPublishingPlanningScene(SceneUpdateType event, - const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC); - - /** \brief Stop publishing the maintained planning scene. */ - void stopPublishingPlanningScene(); - - /** \brief Set the maximum frequency at which planning scenes are being published */ - void setPlanningScenePublishingFrequency(double hz); - - /** \brief Get the maximum frequency at which planning scenes are published (Hz) */ - double getPlanningScenePublishingFrequency() const - { - return publish_planning_scene_frequency_; - } - - /** @brief Get the stored instance of the stored current state monitor - * @return An instance of the stored current state monitor*/ - const CurrentStateMonitorPtr& getStateMonitor() const - { - return current_state_monitor_; - } - - CurrentStateMonitorPtr& getStateMonitorNonConst() - { - return current_state_monitor_; - } - - /** @brief Update the transforms for the frames that are not part of the kinematic model using tf. - * Examples of these frames are the "map" and "odom_combined" transforms. This function is automatically called when - * data that uses transforms is received. - * However, this function should also be called before starting a planning request, for example. - */ - void updateFrameTransforms(); - - /** @brief Start the current state monitor - @param joint_states_topic the topic to listen to for joint states - @param attached_objects_topic the topic to listen to for attached collision objects */ - void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC, - const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); - - /** @brief Stop the state monitor*/ - void stopStateMonitor(); - - /** @brief Update the scene using the monitored state. This function is automatically called when an update to the - current state is received (if startStateMonitor() has been called). - The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency(). */ - void updateSceneWithCurrentState(); - - /** @brief Update the scene using the monitored state at a specified frequency, in Hz. This function has an effect - only when updates from the CurrentStateMonitor are received at a higher frequency. - In that case, the updates are throttled down, so that they do not exceed a maximum update frequency specified - here. - @param hz the update frequency. By default this is 10Hz. */ - void setStateUpdateFrequency(double hz); - - /** @brief Get the maximum frequency (Hz) at which the current state of the planning scene is updated.*/ - double getStateUpdateFrequency() const - { - if (dt_state_update_.count() > 0.0) - { - return 1.0 / dt_state_update_.count(); - } - else - { - return 0.0; - } - } - - /** @brief Start the scene monitor (ROS topic-based) - * @param scene_topic The name of the planning scene topic - */ - void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC); - - /** @brief Request a full planning scene state using a service call - * Be careful not to use this in conjunction with providePlanningSceneService(), - * as it will create a pointless feedback loop. - * @param service_name The name of the service to use for requesting the planning scene. - * This must be a service of type moveit_msgs::srv::GetPlanningScene and is usually called - * "/get_planning_scene". - */ - bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); - - /** @brief Create an optional service for getting the complete planning scene - * This is useful for satisfying the Rviz PlanningScene display's need for a service - * without having to use a move_group node. - * Be careful not to use this in conjunction with requestPlanningSceneState(), - * as it will create a pointless feedback loop. - * @param service_name The topic to provide the service - */ - void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); - - /** @brief Stop the scene monitor*/ - void stopSceneMonitor(); - - /** @brief Start the OccupancyMapMonitor and listening for: - * - Requests to add/remove/update collision objects to/from the world - * - The collision map - * - Requests to attached/detach collision objects - * @param collision_objects_topic The topic on which to listen for collision objects - * @param planning_scene_world_topic The topic to listen to for world scene geometry - * @param load_octomap_monitor Flag to disable octomap monitor if desired - */ - void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC, - const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - const bool load_octomap_monitor = true); - - /** @brief Stop the world geometry monitor */ - void stopWorldGeometryMonitor(); - - /** @brief Add a function to be called when an update to the scene is received */ - void addUpdateCallback(const std::function& fn); - - /** @brief Clear the functions to be called when an update to the scene is received */ - void clearUpdateCallbacks(); - - /** @brief Get the topic names that the monitor is listening to */ - void getMonitoredTopics(std::vector& topics) const; - - /** \brief Return the time when the last update was made to the planning scene (by \e any monitor) */ - const rclcpp::Time& getLastUpdateTime() const - { - return last_update_time_; - } - - void publishDebugInformation(bool flag); - - /** @brief This function is called every time there is a change to the planning scene */ - void triggerSceneUpdateEvent(SceneUpdateType update_type); - - /** \brief Wait for robot state to become more recent than time t. - * - * If there is no state monitor active, there will be no scene updates. - * Hence, you can specify a timeout to wait for those updates. Default is 1s. - */ - bool waitForCurrentRobotState(const rclcpp::Time& t, double wait_time = 1.); - - void clearOctomap(); - - // Called to update the planning scene with a new message. - bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene); - - // Called to update a collision object in the planning scene. - bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg, - const std::optional& color_msg = std::nullopt); - - // Called to update an attached collision object in the planning scene. - bool processAttachedCollisionObjectMsg( - const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg); - -protected: - /** @brief Initialize the planning scene monitor - * @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated) - */ - void initialize(const planning_scene::PlanningScenePtr& scene); - - /** \brief Lock the scene for reading (multiple threads can lock for reading at the same time) */ - void lockSceneRead(); - - /** \brief Unlock the scene from reading (multiple threads can lock for reading at the same time) */ - void unlockSceneRead(); - - /** \brief Lock the scene for writing (only one thread can lock for writing and no other thread can lock for reading) - */ - void lockSceneWrite(); - - /** \brief Lock the scene from writing (only one thread can lock for writing and no other thread can lock for reading) - */ - void unlockSceneWrite(); - - /** @brief Configure the collision matrix for a particular scene */ - void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene); - - /** @brief Configure the default padding*/ - void configureDefaultPadding(); - - /** @brief Callback for a new planning scene world*/ - void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world); - - /** @brief Callback for octomap updates */ - void octomapUpdateCallback(); - - /** @brief Callback for a change for an attached object of the current state of the planning scene */ - void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached); - - /** @brief Callback for a change in the world maintained by the planning scene */ - void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object, - collision_detection::World::Action action); - - void includeRobotLinksInOctree(); - void excludeRobotLinksFromOctree(); - - void excludeWorldObjectsFromOctree(); - void includeWorldObjectsInOctree(); - void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj); - void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj); - - void excludeAttachedBodiesFromOctree(); - void includeAttachedBodiesInOctree(); - void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body); - void includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body); - - bool getShapeTransformCache(const std::string& target_frame, const rclcpp::Time& target_time, - occupancy_map_monitor::ShapeTransformCache& cache) const; - - /// The name of this scene monitor - std::string monitor_name_; - - planning_scene::PlanningScenePtr scene_; - planning_scene::PlanningSceneConstPtr scene_const_; - planning_scene::PlanningScenePtr parent_scene_; /// if diffs are monitored, this is the pointer to the parent scene - std::shared_mutex scene_update_mutex_; /// mutex for stored scene - rclcpp::Time last_update_time_; /// Last time the state was updated - rclcpp::Time last_robot_motion_time_; /// Last time the robot has moved - - std::shared_ptr node_; - - // TODO: (anasarrak) callbacks on ROS2? - // https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/ - // ros::CallbackQueue queue_; - std::shared_ptr pnode_; - std::shared_ptr private_executor_; - std::thread private_executor_thread_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - std::string robot_description_; - - /// default robot padding - double default_robot_padd_; - /// default robot scaling - double default_robot_scale_; - /// default object padding - double default_object_padd_; - /// default attached padding - double default_attached_padd_; - /// default robot link padding - std::map default_robot_link_padd_; - /// default robot link scale - std::map default_robot_link_scale_; - - // variables for planning scene publishing - rclcpp::Publisher::SharedPtr planning_scene_publisher_; - std::unique_ptr publish_planning_scene_; - double publish_planning_scene_frequency_; - SceneUpdateType publish_update_types_; - std::atomic new_scene_update_; - std::condition_variable_any new_scene_update_condition_; - - // subscribe to various sources of data - rclcpp::Subscription::SharedPtr planning_scene_subscriber_; - rclcpp::Subscription::SharedPtr planning_scene_world_subscriber_; - - rclcpp::Subscription::SharedPtr attached_collision_object_subscriber_; - rclcpp::Subscription::SharedPtr collision_object_subscriber_; - - // provide an optional service to get the full planning scene state - // this is used by MoveGroup and related application nodes - rclcpp::Service::SharedPtr get_scene_service_; - - // include a octomap monitor - std::unique_ptr octomap_monitor_; - - // include a current state monitor - CurrentStateMonitorPtr current_state_monitor_; - - typedef std::map > > - LinkShapeHandles; - using AttachedBodyShapeHandles = std::map > >; - using CollisionBodyShapeHandles = - std::map > >; - - LinkShapeHandles link_shape_handles_; - AttachedBodyShapeHandles attached_body_shape_handles_; - CollisionBodyShapeHandles collision_body_shape_handles_; - mutable std::recursive_mutex shape_handles_lock_; - - /// lock access to update_callbacks_ - std::recursive_mutex update_lock_; - std::vector > update_callbacks_; /// List of callbacks to trigger when updates - /// are received - -private: - void getUpdatedFrameTransforms(std::vector& transforms); - - // publish planning scene update diffs (runs in its own thread) - void scenePublishingThread(); - - // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes - void onStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state); - - // called by state_update_timer_ when a state update it pending - void stateUpdateTimerCallback(); - - // Callback for a new planning scene msg - void newPlanningSceneCallback(const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene); - - // Callback for requesting the full planning scene via service - void getPlanningSceneServiceCallback(const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req, - const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res); - - void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates, - bool publish_planning_scene, double publish_planning_scene_hz); - - // Lock for state_update_pending_ and dt_state_update_ - std::mutex state_pending_mutex_; - - /// True when we need to update the RobotState from current_state_monitor_ - // This field is protected by state_pending_mutex_ - volatile bool state_update_pending_; - - /// the amount of time to wait in between updates to the robot state - // This field is protected by state_pending_mutex_ - std::chrono::duration dt_state_update_; // 1hz - - /// the amount of time to wait when looking up transforms - // Setting this to a non-zero value resolves issues when the sensor data is - // arriving so fast that it is preceding the transform state. - rclcpp::Duration shape_transform_cache_lookup_wait_time_; - - /// timer for state updates. - // Check if last_state_update_ is true and if so call updateSceneWithCurrentState() - // Not safe to access from callback functions. - - rclcpp::TimerBase::SharedPtr state_update_timer_; - - /// Last time the state was updated from current_state_monitor_ - // Only access this from callback functions (and constructor) - std::chrono::system_clock::time_point last_robot_state_update_wall_time_; - - robot_model_loader::RobotModelLoaderPtr rm_loader_; - moveit::core::RobotModelConstPtr robot_model_; - - collision_detection::CollisionPluginLoader collision_loader_; - - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_; - - bool use_sim_time_; - - friend class LockedPlanningSceneRO; - friend class LockedPlanningSceneRW; - - rclcpp::Logger logger_; -}; - -/** \brief This is a convenience class for obtaining access to an - * instance of a locked PlanningScene. - * - * Instances of this class can be used almost exactly like instances - * of a PlanningScenePtr because of the typecast operator and - * "operator->" functions. Therefore you will often see code like this: - * @code - * planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); - * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); - * @endcode - - * The function "getRobotModel()" is a member of PlanningScene and not - * a member of this class. However because of the "operator->" here - * which returns a PlanningSceneConstPtr, this works. - * - * Any number of these "read_only" locks can exist at a given time. - * The intention is that users which only need to read from the - * PlanningScene will use these and will thus not interfere with each - * other. - * - * @see LockedPlanningSceneRW */ -class LockedPlanningSceneRO -{ -public: - LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor) - : planning_scene_monitor_(planning_scene_monitor) - { - initialize(true); - } - - const PlanningSceneMonitorPtr& getPlanningSceneMonitor() - { - return planning_scene_monitor_; - } - - operator bool() const - { - return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene(); - } - - operator const planning_scene::PlanningSceneConstPtr&() const - { - return static_cast(planning_scene_monitor_.get())->getPlanningScene(); - } - - const planning_scene::PlanningSceneConstPtr& operator->() const - { - return static_cast(planning_scene_monitor_.get())->getPlanningScene(); - } - -protected: - LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only) - : planning_scene_monitor_(planning_scene_monitor) - { - initialize(read_only); - } - - void initialize(bool read_only) - { - if (planning_scene_monitor_) - lock_ = std::make_shared(planning_scene_monitor_.get(), read_only); - } - - MOVEIT_STRUCT_FORWARD(SingleUnlock); - - // we use this struct so that lock/unlock are called only once - // even if the LockedPlanningScene instance is copied around - struct SingleUnlock - { - SingleUnlock(PlanningSceneMonitor* planning_scene_monitor, bool read_only) - : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only) - { - if (read_only) - { - planning_scene_monitor_->lockSceneRead(); - } - else - { - planning_scene_monitor_->lockSceneWrite(); - } - } - ~SingleUnlock() - { - if (read_only_) - { - planning_scene_monitor_->unlockSceneRead(); - } - else - { - planning_scene_monitor_->unlockSceneWrite(); - } - } - PlanningSceneMonitor* planning_scene_monitor_; - bool read_only_; - }; - - PlanningSceneMonitorPtr planning_scene_monitor_; - SingleUnlockPtr lock_; -}; - -/** \brief This is a convenience class for obtaining access to an - * instance of a locked PlanningScene. - * - * Instances of this class can be used almost exactly like instances - * of a PlanningScenePtr because of the typecast operator and - * "operator->" functions. Therefore you will often see code like this: - * @code - * planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor); - * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); - * @endcode - - * The function "getRobotModel()" is a member of PlanningScene and not - * a member of this class. However because of the "operator->" here - * which returns a PlanningScenePtr, this works. - * - * Only one of these "ReadWrite" locks can exist at a given time. The - * intention is that users which need to write to the PlanningScene - * will use these, preventing other writers and readers from locking - * the same PlanningScene at the same time. - * - * @see LockedPlanningSceneRO */ -class LockedPlanningSceneRW : public LockedPlanningSceneRO -{ -public: - LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor) - : LockedPlanningSceneRO(planning_scene_monitor, false) - { - } - - operator const planning_scene::PlanningScenePtr&() - { - return planning_scene_monitor_->getPlanningScene(); - } - - const planning_scene::PlanningScenePtr& operator->() - { - return planning_scene_monitor_->getPlanningScene(); - } -}; -} // namespace planning_scene_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp new file mode 100644 index 0000000000..b634928369 --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp @@ -0,0 +1,740 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace planning_scene_monitor +{ +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc + +/** + * @brief PlanningSceneMonitor + * Subscribes to the topic \e planning_scene */ +class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor +{ +public: + /** + * @brief PlanningSceneMonitor cannot be copy-constructed + */ + PlanningSceneMonitor(const PlanningSceneMonitor&) = delete; + + /** + * @brief PlanningSceneMonitor cannot be copy-assigned + */ + PlanningSceneMonitor& operator=(const PlanningSceneMonitor&) = delete; + + enum SceneUpdateType + { + /** \brief No update */ + UPDATE_NONE = 0, + + /** \brief The state in the monitored scene was updated */ + UPDATE_STATE = 1, + + /** \brief The maintained set of fixed transforms in the monitored scene was updated */ + UPDATE_TRANSFORMS = 2, + + /** \brief The geometry of the scene was updated. This includes receiving new octomaps, collision objects, attached + objects, scene geometry, etc. */ + UPDATE_GEOMETRY = 4, + + /** \brief The entire scene was updated */ + UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY + }; + + /// The name of the topic used by default for receiving joint states + static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states" + + /// The name of the topic used by default for attached collision objects + static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object" + + /// The name of the topic used by default for receiving collision objects in the world + static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object" + + /// The name of the topic used by default for receiving geometry information about a planning scene (complete + /// overwrite of world geometry) + static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world" + + /// The name of the topic used by default for receiving full planning scenes or planning scene diffs + static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene" + + /// The name of the service used by default for requesting full planning scene state + static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene" + + /// The name of the topic used by default for publishing the monitored planning scene (this is without "/" in the + /// name, so the topic is prefixed by the node name) + static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene" + + /** @brief Constructor + * @param robot_description The name of the ROS parameter that contains the URDF (in string format) + * @param name A name identifying this planning scene monitor + */ + PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, + const std::string& name = ""); + + /** @brief Constructor + * @param rml A pointer to a kinematic model loader + * @param name A name identifying this planning scene monitor + */ + PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml, + const std::string& name = ""); + + /** @brief Constructor + * @param scene The scene instance to maintain up to date with monitored information + * @param robot_description The name of the ROS parameter that contains the URDF (in string format) + * @param name A name identifying this planning scene monitor + */ + PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, + const std::string& robot_description, const std::string& name = ""); + + /** @brief Constructor + * @param scene The scene instance to maintain up to date with monitored information + * @param rml A pointer to a kinematic model loader + * @param name A name identifying this planning scene monitor + */ + PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, + const robot_model_loader::RobotModelLoaderPtr& rml, const std::string& name = ""); + + ~PlanningSceneMonitor(); + + /** \brief Get the name of this monitor */ + const std::string& getName() const + { + return monitor_name_; + } + + /** \brief Get the user kinematic model loader */ + const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const + { + return rm_loader_; + } + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + /** @brief Avoid this function! Returns an @b + * unsafe pointer to the current planning scene. + * @warning Most likely you do not want to call this function + * directly. PlanningSceneMonitor has a background thread + * which repeatedly updates and clobbers various contents + * of its internal PlanningScene instance. This function + * just returns a pointer to that dynamic internal object. + * The correct thing is usually to use a + * LockedPlanningSceneRO or LockedPlanningSceneRW, which + * locks the PlanningSceneMonitor and provides safe access + * to the PlanningScene object. + * @see LockedPlanningSceneRO + * @see LockedPlanningSceneRW. + * @return A pointer to the current planning scene.*/ + const planning_scene::PlanningScenePtr& getPlanningScene() + { + return scene_; + } + + /*! @brief Avoid this function! Returns an @b + * unsafe pointer to the current planning scene. + * @copydetails PlanningSceneMonitor::getPlanningScene() */ + const planning_scene::PlanningSceneConstPtr& getPlanningScene() const + { + return scene_const_; + } + + /** @brief Return true if the scene \e scene can be updated directly + or indirectly by this monitor. This function will return true if + the pointer of the scene is the same as the one maintained, + or if a parent of the scene is the one maintained. */ + bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const; + + /** @brief Return true if the scene \e scene can be updated directly + or indirectly by this monitor. This function will return true if + the pointer of the scene is the same as the one maintained, + or if a parent of the scene is the one maintained. */ + bool updatesScene(const planning_scene::PlanningScenePtr& scene) const; + + /** @brief Get the stored robot description + * @return An instance of the stored robot description*/ + const std::string& getRobotDescription() const + { + return robot_description_; + } + + /// Get the default robot padding + double getDefaultRobotPadding() const + { + return default_robot_padd_; + } + + /// Get the default robot scaling + double getDefaultRobotScale() const + { + return default_robot_scale_; + } + + /// Get the default object padding + double getDefaultObjectPadding() const + { + return default_object_padd_; + } + + /// Get the default attached padding + double getDefaultAttachedObjectPadding() const + { + return default_attached_padd_; + } + + /** @brief Get the instance of the TF client that was passed to the constructor of this class. */ + const std::shared_ptr& getTFClient() const + { + return tf_buffer_; + } + + /** \brief By default, the maintained planning scene does not reason about diffs. When the flag passed in is true, the + maintained + scene starts counting diffs. Future updates to the planning scene will be stored as diffs and can be retrieved as + such. Setting the flag to false restores the default behaviour. Maintaining diffs is automatically enabled when + publishing planning scenes. */ + void monitorDiffs(bool flag); + + /** \brief Start publishing the maintained planning scene. The first message set out is a complete planning scene. + Diffs are sent afterwards on updates specified by the \e event bitmask. For UPDATE_SCENE, the full scene is always + sent. */ + void startPublishingPlanningScene(SceneUpdateType event, + const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC); + + /** \brief Stop publishing the maintained planning scene. */ + void stopPublishingPlanningScene(); + + /** \brief Set the maximum frequency at which planning scenes are being published */ + void setPlanningScenePublishingFrequency(double hz); + + /** \brief Get the maximum frequency at which planning scenes are published (Hz) */ + double getPlanningScenePublishingFrequency() const + { + return publish_planning_scene_frequency_; + } + + /** @brief Get the stored instance of the stored current state monitor + * @return An instance of the stored current state monitor*/ + const CurrentStateMonitorPtr& getStateMonitor() const + { + return current_state_monitor_; + } + + CurrentStateMonitorPtr& getStateMonitorNonConst() + { + return current_state_monitor_; + } + + /** @brief Update the transforms for the frames that are not part of the kinematic model using tf. + * Examples of these frames are the "map" and "odom_combined" transforms. This function is automatically called when + * data that uses transforms is received. + * However, this function should also be called before starting a planning request, for example. + */ + void updateFrameTransforms(); + + /** @brief Start the current state monitor + @param joint_states_topic the topic to listen to for joint states + @param attached_objects_topic the topic to listen to for attached collision objects */ + void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC, + const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); + + /** @brief Stop the state monitor*/ + void stopStateMonitor(); + + /** @brief Update the scene using the monitored state. This function is automatically called when an update to the + current state is received (if startStateMonitor() has been called). + The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency(). */ + void updateSceneWithCurrentState(); + + /** @brief Update the scene using the monitored state at a specified frequency, in Hz. This function has an effect + only when updates from the CurrentStateMonitor are received at a higher frequency. + In that case, the updates are throttled down, so that they do not exceed a maximum update frequency specified + here. + @param hz the update frequency. By default this is 10Hz. */ + void setStateUpdateFrequency(double hz); + + /** @brief Get the maximum frequency (Hz) at which the current state of the planning scene is updated.*/ + double getStateUpdateFrequency() const + { + if (dt_state_update_.count() > 0.0) + { + return 1.0 / dt_state_update_.count(); + } + else + { + return 0.0; + } + } + + /** @brief Start the scene monitor (ROS topic-based) + * @param scene_topic The name of the planning scene topic + */ + void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC); + + /** @brief Request a full planning scene state using a service call + * Be careful not to use this in conjunction with providePlanningSceneService(), + * as it will create a pointless feedback loop. + * @param service_name The name of the service to use for requesting the planning scene. + * This must be a service of type moveit_msgs::srv::GetPlanningScene and is usually called + * "/get_planning_scene". + */ + bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); + + /** @brief Create an optional service for getting the complete planning scene + * This is useful for satisfying the Rviz PlanningScene display's need for a service + * without having to use a move_group node. + * Be careful not to use this in conjunction with requestPlanningSceneState(), + * as it will create a pointless feedback loop. + * @param service_name The topic to provide the service + */ + void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE); + + /** @brief Stop the scene monitor*/ + void stopSceneMonitor(); + + /** @brief Start the OccupancyMapMonitor and listening for: + * - Requests to add/remove/update collision objects to/from the world + * - The collision map + * - Requests to attached/detach collision objects + * @param collision_objects_topic The topic on which to listen for collision objects + * @param planning_scene_world_topic The topic to listen to for world scene geometry + * @param load_octomap_monitor Flag to disable octomap monitor if desired + */ + void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC, + const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + const bool load_octomap_monitor = true); + + /** @brief Stop the world geometry monitor */ + void stopWorldGeometryMonitor(); + + /** @brief Add a function to be called when an update to the scene is received */ + void addUpdateCallback(const std::function& fn); + + /** @brief Clear the functions to be called when an update to the scene is received */ + void clearUpdateCallbacks(); + + /** @brief Get the topic names that the monitor is listening to */ + void getMonitoredTopics(std::vector& topics) const; + + /** \brief Return the time when the last update was made to the planning scene (by \e any monitor) */ + const rclcpp::Time& getLastUpdateTime() const + { + return last_update_time_; + } + + void publishDebugInformation(bool flag); + + /** @brief This function is called every time there is a change to the planning scene */ + void triggerSceneUpdateEvent(SceneUpdateType update_type); + + /** \brief Wait for robot state to become more recent than time t. + * + * If there is no state monitor active, there will be no scene updates. + * Hence, you can specify a timeout to wait for those updates. Default is 1s. + */ + bool waitForCurrentRobotState(const rclcpp::Time& t, double wait_time = 1.); + + void clearOctomap(); + + // Called to update the planning scene with a new message. + bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene); + + // Called to update a collision object in the planning scene. + bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg, + const std::optional& color_msg = std::nullopt); + + // Called to update an attached collision object in the planning scene. + bool processAttachedCollisionObjectMsg( + const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg); + +protected: + /** @brief Initialize the planning scene monitor + * @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated) + */ + void initialize(const planning_scene::PlanningScenePtr& scene); + + /** \brief Lock the scene for reading (multiple threads can lock for reading at the same time) */ + void lockSceneRead(); + + /** \brief Unlock the scene from reading (multiple threads can lock for reading at the same time) */ + void unlockSceneRead(); + + /** \brief Lock the scene for writing (only one thread can lock for writing and no other thread can lock for reading) + */ + void lockSceneWrite(); + + /** \brief Lock the scene from writing (only one thread can lock for writing and no other thread can lock for reading) + */ + void unlockSceneWrite(); + + /** @brief Configure the collision matrix for a particular scene */ + void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene); + + /** @brief Configure the default padding*/ + void configureDefaultPadding(); + + /** @brief Callback for a new planning scene world*/ + void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world); + + /** @brief Callback for octomap updates */ + void octomapUpdateCallback(); + + /** @brief Callback for a change for an attached object of the current state of the planning scene */ + void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached); + + /** @brief Callback for a change in the world maintained by the planning scene */ + void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object, + collision_detection::World::Action action); + + void includeRobotLinksInOctree(); + void excludeRobotLinksFromOctree(); + + void excludeWorldObjectsFromOctree(); + void includeWorldObjectsInOctree(); + void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj); + void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj); + + void excludeAttachedBodiesFromOctree(); + void includeAttachedBodiesInOctree(); + void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body); + void includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body); + + bool getShapeTransformCache(const std::string& target_frame, const rclcpp::Time& target_time, + occupancy_map_monitor::ShapeTransformCache& cache) const; + + /// The name of this scene monitor + std::string monitor_name_; + + planning_scene::PlanningScenePtr scene_; + planning_scene::PlanningSceneConstPtr scene_const_; + planning_scene::PlanningScenePtr parent_scene_; /// if diffs are monitored, this is the pointer to the parent scene + std::shared_mutex scene_update_mutex_; /// mutex for stored scene + rclcpp::Time last_update_time_; /// Last time the state was updated + rclcpp::Time last_robot_motion_time_; /// Last time the robot has moved + + std::shared_ptr node_; + + // TODO: (anasarrak) callbacks on ROS2? + // https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/ + // ros::CallbackQueue queue_; + std::shared_ptr pnode_; + std::shared_ptr private_executor_; + std::thread private_executor_thread_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string robot_description_; + + /// default robot padding + double default_robot_padd_; + /// default robot scaling + double default_robot_scale_; + /// default object padding + double default_object_padd_; + /// default attached padding + double default_attached_padd_; + /// default robot link padding + std::map default_robot_link_padd_; + /// default robot link scale + std::map default_robot_link_scale_; + + // variables for planning scene publishing + rclcpp::Publisher::SharedPtr planning_scene_publisher_; + std::unique_ptr publish_planning_scene_; + double publish_planning_scene_frequency_; + SceneUpdateType publish_update_types_; + std::atomic new_scene_update_; + std::condition_variable_any new_scene_update_condition_; + + // subscribe to various sources of data + rclcpp::Subscription::SharedPtr planning_scene_subscriber_; + rclcpp::Subscription::SharedPtr planning_scene_world_subscriber_; + + rclcpp::Subscription::SharedPtr attached_collision_object_subscriber_; + rclcpp::Subscription::SharedPtr collision_object_subscriber_; + + // provide an optional service to get the full planning scene state + // this is used by MoveGroup and related application nodes + rclcpp::Service::SharedPtr get_scene_service_; + + // include a octomap monitor + std::unique_ptr octomap_monitor_; + + // include a current state monitor + CurrentStateMonitorPtr current_state_monitor_; + + typedef std::map > > + LinkShapeHandles; + using AttachedBodyShapeHandles = std::map > >; + using CollisionBodyShapeHandles = + std::map > >; + + LinkShapeHandles link_shape_handles_; + AttachedBodyShapeHandles attached_body_shape_handles_; + CollisionBodyShapeHandles collision_body_shape_handles_; + mutable std::recursive_mutex shape_handles_lock_; + + /// lock access to update_callbacks_ + std::recursive_mutex update_lock_; + std::vector > update_callbacks_; /// List of callbacks to trigger when updates + /// are received + +private: + void getUpdatedFrameTransforms(std::vector& transforms); + + // publish planning scene update diffs (runs in its own thread) + void scenePublishingThread(); + + // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes + void onStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state); + + // called by state_update_timer_ when a state update it pending + void stateUpdateTimerCallback(); + + // Callback for a new planning scene msg + void newPlanningSceneCallback(const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene); + + // Callback for requesting the full planning scene via service + void getPlanningSceneServiceCallback(const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req, + const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res); + + void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates, + bool publish_planning_scene, double publish_planning_scene_hz); + + // Lock for state_update_pending_ and dt_state_update_ + std::mutex state_pending_mutex_; + + /// True when we need to update the RobotState from current_state_monitor_ + // This field is protected by state_pending_mutex_ + volatile bool state_update_pending_; + + /// the amount of time to wait in between updates to the robot state + // This field is protected by state_pending_mutex_ + std::chrono::duration dt_state_update_; // 1hz + + /// the amount of time to wait when looking up transforms + // Setting this to a non-zero value resolves issues when the sensor data is + // arriving so fast that it is preceding the transform state. + rclcpp::Duration shape_transform_cache_lookup_wait_time_; + + /// timer for state updates. + // Check if last_state_update_ is true and if so call updateSceneWithCurrentState() + // Not safe to access from callback functions. + + rclcpp::TimerBase::SharedPtr state_update_timer_; + + /// Last time the state was updated from current_state_monitor_ + // Only access this from callback functions (and constructor) + std::chrono::system_clock::time_point last_robot_state_update_wall_time_; + + robot_model_loader::RobotModelLoaderPtr rm_loader_; + moveit::core::RobotModelConstPtr robot_model_; + + collision_detection::CollisionPluginLoader collision_loader_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_; + + bool use_sim_time_; + + friend class LockedPlanningSceneRO; + friend class LockedPlanningSceneRW; + + rclcpp::Logger logger_; +}; + +/** \brief This is a convenience class for obtaining access to an + * instance of a locked PlanningScene. + * + * Instances of this class can be used almost exactly like instances + * of a PlanningScenePtr because of the typecast operator and + * "operator->" functions. Therefore you will often see code like this: + * @code + * planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); + * @endcode + + * The function "getRobotModel()" is a member of PlanningScene and not + * a member of this class. However because of the "operator->" here + * which returns a PlanningSceneConstPtr, this works. + * + * Any number of these "read_only" locks can exist at a given time. + * The intention is that users which only need to read from the + * PlanningScene will use these and will thus not interfere with each + * other. + * + * @see LockedPlanningSceneRW */ +class LockedPlanningSceneRO +{ +public: + LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor) + : planning_scene_monitor_(planning_scene_monitor) + { + initialize(true); + } + + const PlanningSceneMonitorPtr& getPlanningSceneMonitor() + { + return planning_scene_monitor_; + } + + operator bool() const + { + return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene(); + } + + operator const planning_scene::PlanningSceneConstPtr&() const + { + return static_cast(planning_scene_monitor_.get())->getPlanningScene(); + } + + const planning_scene::PlanningSceneConstPtr& operator->() const + { + return static_cast(planning_scene_monitor_.get())->getPlanningScene(); + } + +protected: + LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only) + : planning_scene_monitor_(planning_scene_monitor) + { + initialize(read_only); + } + + void initialize(bool read_only) + { + if (planning_scene_monitor_) + lock_ = std::make_shared(planning_scene_monitor_.get(), read_only); + } + + MOVEIT_STRUCT_FORWARD(SingleUnlock); + + // we use this struct so that lock/unlock are called only once + // even if the LockedPlanningScene instance is copied around + struct SingleUnlock + { + SingleUnlock(PlanningSceneMonitor* planning_scene_monitor, bool read_only) + : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only) + { + if (read_only) + { + planning_scene_monitor_->lockSceneRead(); + } + else + { + planning_scene_monitor_->lockSceneWrite(); + } + } + ~SingleUnlock() + { + if (read_only_) + { + planning_scene_monitor_->unlockSceneRead(); + } + else + { + planning_scene_monitor_->unlockSceneWrite(); + } + } + PlanningSceneMonitor* planning_scene_monitor_; + bool read_only_; + }; + + PlanningSceneMonitorPtr planning_scene_monitor_; + SingleUnlockPtr lock_; +}; + +/** \brief This is a convenience class for obtaining access to an + * instance of a locked PlanningScene. + * + * Instances of this class can be used almost exactly like instances + * of a PlanningScenePtr because of the typecast operator and + * "operator->" functions. Therefore you will often see code like this: + * @code + * planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor); + * moveit::core::RobotModelConstPtr model = ls->getRobotModel(); + * @endcode + + * The function "getRobotModel()" is a member of PlanningScene and not + * a member of this class. However because of the "operator->" here + * which returns a PlanningScenePtr, this works. + * + * Only one of these "ReadWrite" locks can exist at a given time. The + * intention is that users which need to write to the PlanningScene + * will use these, preventing other writers and readers from locking + * the same PlanningScene at the same time. + * + * @see LockedPlanningSceneRO */ +class LockedPlanningSceneRW : public LockedPlanningSceneRO +{ +public: + LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor) + : LockedPlanningSceneRO(planning_scene_monitor, false) + { + } + + operator const planning_scene::PlanningScenePtr&() + { + return planning_scene_monitor_->getPlanningScene(); + } + + const planning_scene::PlanningScenePtr& operator->() + { + return planning_scene_monitor_->getPlanningScene(); + } +}; +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h index 9975093c06..2b60e485b1 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,112 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace planning_scene_monitor -{ -using TrajectoryStateAddedCallback = std::function; - -MOVEIT_CLASS_FORWARD(TrajectoryMonitor); // Defines TrajectoryMonitorPtr, ConstPtr, WeakPtr... etc - -/** @class TrajectoryMonitor - @brief Monitors the joint_states topic and tf to record the trajectory of the robot. */ -class TrajectoryMonitor -{ -public: - /** - * @brief This class contains the rcl interfaces for easier testing - */ - class MiddlewareHandle - { - public: - /** - * @brief Destroys the object. - */ - virtual ~MiddlewareHandle() = default; - - /** - * @brief set Rate using sampling frequency - */ - virtual void setRate(double sampling_frequency) = 0; - - /** - * @brief Sleep the handle for some prescribed amount of time. - */ - virtual void sleep() = 0; - }; - - /** @brief Constructor - * @param[in] state_monitor - * @param[in] sampling_frequency - */ - TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency = 0.0); - - /** @brief Constructor with middleware handle as input parameter - * @param[in] state_monitor - * @param[in] sampling_frequency - */ - TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, - std::unique_ptr middleware_handle, double sampling_frequency = 0.0); - - ~TrajectoryMonitor(); - - void startTrajectoryMonitor(); - - void stopTrajectoryMonitor(); - - void clearTrajectory(); - - bool isActive() const; - - double getSamplingFrequency() const - { - return sampling_frequency_; - } - - void setSamplingFrequency(double sampling_frequency); - - /// Return the current maintained trajectory. This function is not thread safe (hence NOT const), because the - /// trajectory could be modified. - const robot_trajectory::RobotTrajectory& getTrajectory() - { - return trajectory_; - } - - void swapTrajectory(robot_trajectory::RobotTrajectory& other) - { - trajectory_.swap(other); - } - - void setOnStateAddCallback(const TrajectoryStateAddedCallback& callback) - { - state_add_callback_ = callback; - } - -private: - void recordStates(); - - // Samples robot states. - CurrentStateMonitorConstPtr current_state_monitor_; - // Interface for communicating with ROS. - std::unique_ptr middleware_handle_; - double sampling_frequency_; - - robot_trajectory::RobotTrajectory trajectory_; - rclcpp::Time trajectory_start_time_; - rclcpp::Time last_recorded_state_time_; - - std::unique_ptr record_states_thread_; - TrajectoryStateAddedCallback state_add_callback_; - - rclcpp::Logger logger_; -}; -} // namespace planning_scene_monitor +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.hpp new file mode 100644 index 0000000000..08c7fadce9 --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor.hpp @@ -0,0 +1,146 @@ +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning_scene_monitor +{ +using TrajectoryStateAddedCallback = std::function; + +MOVEIT_CLASS_FORWARD(TrajectoryMonitor); // Defines TrajectoryMonitorPtr, ConstPtr, WeakPtr... etc + +/** @class TrajectoryMonitor + @brief Monitors the joint_states topic and tf to record the trajectory of the robot. */ +class TrajectoryMonitor +{ +public: + /** + * @brief This class contains the rcl interfaces for easier testing + */ + class MiddlewareHandle + { + public: + /** + * @brief Destroys the object. + */ + virtual ~MiddlewareHandle() = default; + + /** + * @brief set Rate using sampling frequency + */ + virtual void setRate(double sampling_frequency) = 0; + + /** + * @brief Sleep the handle for some prescribed amount of time. + */ + virtual void sleep() = 0; + }; + + /** @brief Constructor + * @param[in] state_monitor + * @param[in] sampling_frequency + */ + TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency = 0.0); + + /** @brief Constructor with middleware handle as input parameter + * @param[in] state_monitor + * @param[in] sampling_frequency + */ + TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, + std::unique_ptr middleware_handle, double sampling_frequency = 0.0); + + ~TrajectoryMonitor(); + + void startTrajectoryMonitor(); + + void stopTrajectoryMonitor(); + + void clearTrajectory(); + + bool isActive() const; + + double getSamplingFrequency() const + { + return sampling_frequency_; + } + + void setSamplingFrequency(double sampling_frequency); + + /// Return the current maintained trajectory. This function is not thread safe (hence NOT const), because the + /// trajectory could be modified. + const robot_trajectory::RobotTrajectory& getTrajectory() + { + return trajectory_; + } + + void swapTrajectory(robot_trajectory::RobotTrajectory& other) + { + trajectory_.swap(other); + } + + void setOnStateAddCallback(const TrajectoryStateAddedCallback& callback) + { + state_add_callback_ = callback; + } + +private: + void recordStates(); + + // Samples robot states. + CurrentStateMonitorConstPtr current_state_monitor_; + // Interface for communicating with ROS. + std::unique_ptr middleware_handle_; + double sampling_frequency_; + + robot_trajectory::RobotTrajectory trajectory_; + rclcpp::Time trajectory_start_time_; + rclcpp::Time last_recorded_state_time_; + + std::unique_ptr record_states_thread_; + TrajectoryStateAddedCallback state_add_callback_; + + rclcpp::Logger logger_; +}; +} // namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.h new file mode 100644 index 0000000000..ea042840cc --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik, 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 PickNik 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: Abishalini Sivaraman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.hpp index 4c2ec8fbe8..d691173065 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.hpp +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/trajectory_monitor_middleware_handle.hpp @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace planning_scene_monitor diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index 572e2fd188..eca1c474b9 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 106a6fd663..478e98d1b9 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index 1fe9ca0f4a..5f8198fd11 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include +#include #include -#include +#include #include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp index 6e46e6538b..2977b0f76c 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp @@ -40,8 +40,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp index 0547216437..71a68977b8 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp @@ -43,8 +43,8 @@ #include // Main class -#include -#include +#include +#include class PlanningSceneMonitorTest : public ::testing::Test { diff --git a/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp b/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp index 002f2b395a..0b957fdf43 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp @@ -40,9 +40,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index 6445876478..f55ce2fe60 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,113 +47,5 @@ /* Author: Ioan Sucan, Mathias Lüdtke, Dave Coleman */ #pragma once - -#include -#include -#if __has_include() -#include -#else -#include -#endif -#include -#include - -namespace rdf_loader -{ -MOVEIT_CLASS_FORWARD(RDFLoader); // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc - -using NewModelCallback = std::function; - -/** @class RDFLoader - */ -class RDFLoader -{ -public: - /** @brief Default constructor - * - * Loads the URDF from a parameter given by the string argument, - * and the SRDF that has the same name + the "_semantic" suffix - * - * If the parameter does not exist, attempt to subscribe to topics - * with the same name and type std_msgs::msg::String. - * - * (specifying default_continuous_value/default_timeout allows users - * to specify values without setting ros parameters) - * - * @param node ROS interface for parameters / topics - * @param ros_name The string name corresponding to the URDF - * @param default_continuous_value Default value for parameter with "_continuous" suffix. - * @param default_timeout Default value for parameter with "_timeout" suffix. - */ - RDFLoader(const std::shared_ptr& node, const std::string& ros_name = "robot_description", - bool default_continuous_value = false, double default_timeout = 10.0); - - /** @brief Initialize the robot model from a string representation of the URDF and SRDF documents */ - RDFLoader(const std::string& urdf_string, const std::string& srdf_string); - - /** @brief Get the resolved parameter name for the robot description */ - const std::string& getRobotDescription() const - { - return ros_name_; - } - - /** @brief Get the URDF string*/ - const std::string& getURDFString() const - { - return urdf_string_; - } - - /** @brief Get the parsed URDF model*/ - const urdf::ModelInterfaceSharedPtr& getURDF() const - { - return urdf_; - } - - /** @brief Get the parsed SRDF model*/ - const srdf::ModelSharedPtr& getSRDF() const - { - return srdf_; - } - - void setNewModelCallback(const NewModelCallback& cb) - { - new_model_cb_ = cb; - } - - /** @brief determine if given path points to a xacro file */ - static bool isXacroFile(const std::string& path); - - /** @brief load file from given path into buffer */ - static bool loadFileToString(std::string& buffer, const std::string& path); - - /** @brief run xacro with the given args on the file, return result in buffer */ - static bool loadXacroFileToString(std::string& buffer, const std::string& path, - const std::vector& xacro_args); - - /** @brief helper that branches between loadFileToString() and loadXacroFileToString() based on result of - * isXacroFile() */ - static bool loadXmlFileToString(std::string& buffer, const std::string& path, - const std::vector& xacro_args); - - /** @brief helper that generates a file path based on package name and relative file path to package */ - static bool loadPkgFileToString(std::string& buffer, const std::string& package_name, - const std::string& relative_path, const std::vector& xacro_args); - -private: - bool loadFromStrings(); - - void urdfUpdateCallback(const std::string& new_urdf_string); - void srdfUpdateCallback(const std::string& new_srdf_string); - - NewModelCallback new_model_cb_; - - std::string ros_name_; - std::string urdf_string_, srdf_string_; - - SynchronizedStringParameter urdf_ssp_; - SynchronizedStringParameter srdf_ssp_; - - srdf::ModelSharedPtr srdf_; - urdf::ModelInterfaceSharedPtr urdf_; -}; -} // namespace rdf_loader +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.hpp b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.hpp new file mode 100644 index 0000000000..788dced0d2 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.hpp @@ -0,0 +1,147 @@ +/********************************************************************* + * 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, Mathias Lüdtke, Dave Coleman */ + +#pragma once + +#include +#include +#if __has_include() +#include +#else +#include +#endif +#include +#include + +namespace rdf_loader +{ +MOVEIT_CLASS_FORWARD(RDFLoader); // Defines RDFLoaderPtr, ConstPtr, WeakPtr... etc + +using NewModelCallback = std::function; + +/** @class RDFLoader + */ +class RDFLoader +{ +public: + /** @brief Default constructor + * + * Loads the URDF from a parameter given by the string argument, + * and the SRDF that has the same name + the "_semantic" suffix + * + * If the parameter does not exist, attempt to subscribe to topics + * with the same name and type std_msgs::msg::String. + * + * (specifying default_continuous_value/default_timeout allows users + * to specify values without setting ros parameters) + * + * @param node ROS interface for parameters / topics + * @param ros_name The string name corresponding to the URDF + * @param default_continuous_value Default value for parameter with "_continuous" suffix. + * @param default_timeout Default value for parameter with "_timeout" suffix. + */ + RDFLoader(const std::shared_ptr& node, const std::string& ros_name = "robot_description", + bool default_continuous_value = false, double default_timeout = 10.0); + + /** @brief Initialize the robot model from a string representation of the URDF and SRDF documents */ + RDFLoader(const std::string& urdf_string, const std::string& srdf_string); + + /** @brief Get the resolved parameter name for the robot description */ + const std::string& getRobotDescription() const + { + return ros_name_; + } + + /** @brief Get the URDF string*/ + const std::string& getURDFString() const + { + return urdf_string_; + } + + /** @brief Get the parsed URDF model*/ + const urdf::ModelInterfaceSharedPtr& getURDF() const + { + return urdf_; + } + + /** @brief Get the parsed SRDF model*/ + const srdf::ModelSharedPtr& getSRDF() const + { + return srdf_; + } + + void setNewModelCallback(const NewModelCallback& cb) + { + new_model_cb_ = cb; + } + + /** @brief determine if given path points to a xacro file */ + static bool isXacroFile(const std::string& path); + + /** @brief load file from given path into buffer */ + static bool loadFileToString(std::string& buffer, const std::string& path); + + /** @brief run xacro with the given args on the file, return result in buffer */ + static bool loadXacroFileToString(std::string& buffer, const std::string& path, + const std::vector& xacro_args); + + /** @brief helper that branches between loadFileToString() and loadXacroFileToString() based on result of + * isXacroFile() */ + static bool loadXmlFileToString(std::string& buffer, const std::string& path, + const std::vector& xacro_args); + + /** @brief helper that generates a file path based on package name and relative file path to package */ + static bool loadPkgFileToString(std::string& buffer, const std::string& package_name, + const std::string& relative_path, const std::vector& xacro_args); + +private: + bool loadFromStrings(); + + void urdfUpdateCallback(const std::string& new_urdf_string); + void srdfUpdateCallback(const std::string& new_srdf_string); + + NewModelCallback new_model_cb_; + + std::string ros_name_; + std::string urdf_string_, srdf_string_; + + SynchronizedStringParameter urdf_ssp_; + SynchronizedStringParameter srdf_ssp_; + + srdf::ModelSharedPtr srdf_; + urdf::ModelInterfaceSharedPtr urdf_; +}; +} // namespace rdf_loader diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h index a10dbd1ba0..82304b6b7a 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,50 +47,5 @@ /* Author: David V. Lu!! */ #pragma once - -#include -#include - -namespace rdf_loader -{ -using StringCallback = std::function; - -/** - * @brief SynchronizedStringParameter is a way to load a string from the ROS environment. - * - * First it tries to load the string from a parameter. - * If that fails, it subscribes to a std_msgs::String topic of the same name to get the value. - * - * If the parameter is loaded successfully, you can publish the value as a String msg if the publish_NAME param is true. - * - * You can specify how long to wait for a subscribed message with NAME_timeout (double in seconds) - * - * By default, the subscription will be killed after the first message is received. - * If the parameter NAME_continuous is true, then the parent_callback will be called on every subsequent message. - */ -class SynchronizedStringParameter -{ -public: - std::string loadInitialValue(const std::shared_ptr& node, const std::string& name, - const StringCallback& parent_callback = {}, bool default_continuous_value = false, - double default_timeout = 10.0); - -protected: - bool getMainParameter(); - - bool shouldPublish(); - - bool waitForMessage(const rclcpp::Duration& timeout); - - void stringCallback(const std_msgs::msg::String::ConstSharedPtr& msg); - - std::shared_ptr node_; - std::string name_; - StringCallback parent_callback_; - - rclcpp::Subscription::SharedPtr string_subscriber_; - rclcpp::Publisher::SharedPtr string_publisher_; - - std::string content_; -}; -} // namespace rdf_loader +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.hpp b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.hpp new file mode 100644 index 0000000000..a10dbd1ba0 --- /dev/null +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.hpp @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once + +#include +#include + +namespace rdf_loader +{ +using StringCallback = std::function; + +/** + * @brief SynchronizedStringParameter is a way to load a string from the ROS environment. + * + * First it tries to load the string from a parameter. + * If that fails, it subscribes to a std_msgs::String topic of the same name to get the value. + * + * If the parameter is loaded successfully, you can publish the value as a String msg if the publish_NAME param is true. + * + * You can specify how long to wait for a subscribed message with NAME_timeout (double in seconds) + * + * By default, the subscription will be killed after the first message is received. + * If the parameter NAME_continuous is true, then the parent_callback will be called on every subsequent message. + */ +class SynchronizedStringParameter +{ +public: + std::string loadInitialValue(const std::shared_ptr& node, const std::string& name, + const StringCallback& parent_callback = {}, bool default_continuous_value = false, + double default_timeout = 10.0); + +protected: + bool getMainParameter(); + + bool shouldPublish(); + + bool waitForMessage(const rclcpp::Duration& timeout); + + void stringCallback(const std_msgs::msg::String::ConstSharedPtr& msg); + + std::shared_ptr node_; + std::string name_; + StringCallback parent_callback_; + + rclcpp::Subscription::SharedPtr string_subscriber_; + rclcpp::Publisher::SharedPtr string_publisher_; + + std::string content_; +}; +} // namespace rdf_loader diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index e96337a83e..de43e05744 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan, Mathias Lüdtke, Dave Coleman */ // MoveIt -#include +#include #include #include #include diff --git a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp index fa36d0b5ab..ac87024b98 100644 --- a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp +++ b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp @@ -34,7 +34,7 @@ /* Author: David V. Lu!! */ -#include +#include namespace rdf_loader { diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp index 9d813402b5..d64e2ce70a 100644 --- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp +++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp @@ -34,7 +34,7 @@ /* Author: David V. Lu!! */ -#include +#include #include #include diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h index 8b7c54220d..14aa6f0673 100644 --- a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,104 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -namespace robot_model_loader -{ -MOVEIT_CLASS_FORWARD(RobotModelLoader); // Defines RobotModelLoaderPtr, ConstPtr, WeakPtr... etc - -/** @class RobotModelLoader */ -class RobotModelLoader -{ -public: - /** @brief Structure that encodes the options to be passed to the RobotModelLoader constructor */ - struct Options - { - Options(const std::string& robot_description = "robot_description") - : robot_description(robot_description), load_kinematics_solvers(true) - { - } - - Options(const std::string& urdf_string, const std::string& srdf_string) - : urdf_string_(urdf_string), srdf_string(srdf_string), load_kinematics_solvers(true) - { - } - - /** @brief The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter name - plus the "_planning" suffix, additional configuration can be specified (e.g., additional joint limits). - Loading from the param server is attempted only if loading from string fails. */ - std::string robot_description; - - /** @brief The string content of the URDF and SRDF documents. Loading from string is attempted only if loading from - * XML fails */ - std::string urdf_string_, srdf_string; - - /** @brief Flag indicating whether the kinematics solvers should be loaded as well, using specified ROS parameters - */ - bool load_kinematics_solvers; - }; - - /** @brief Default constructor */ - RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt = Options()); - - RobotModelLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, - bool load_kinematics_solvers = true); - - ~RobotModelLoader(); - - /** @brief Get the constructed planning_models::RobotModel */ - const moveit::core::RobotModelPtr& getModel() const - { - return model_; - } - - /** @brief Get the resolved parameter name for the robot description */ - const std::string& getRobotDescription() const - { - return rdf_loader_->getRobotDescription(); - } - - /** @brief Get the parsed URDF model*/ - const urdf::ModelInterfaceSharedPtr& getURDF() const - { - return rdf_loader_->getURDF(); - } - - /** @brief Get the parsed SRDF model*/ - const srdf::ModelSharedPtr& getSRDF() const - { - return rdf_loader_->getSRDF(); - } - - /** @brief Get the instance of rdf_loader::RDFLoader that was used to load the robot description */ - const rdf_loader::RDFLoaderPtr& getRDFLoader() const - { - return rdf_loader_; - } - - /** \brief Get the kinematics solvers plugin loader. - \note This instance needs to be kept in scope, otherwise kinematics solver plugins may get unloaded. */ - const kinematics_plugin_loader::KinematicsPluginLoaderPtr& getKinematicsPluginLoader() const - { - return kinematics_loader_; - } - - /** @brief Load the kinematics solvers into the kinematic model. This is done by default, unless disabled explicitly - * by the options passed to the constructor */ - void loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader = - kinematics_plugin_loader::KinematicsPluginLoaderPtr()); - -private: - void configure(const Options& opt); - - moveit::core::RobotModelPtr model_; - rdf_loader::RDFLoaderPtr rdf_loader_; - kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; - const rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; -}; -} // namespace robot_model_loader +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.hpp b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.hpp new file mode 100644 index 0000000000..6f65c2d02c --- /dev/null +++ b/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.hpp @@ -0,0 +1,138 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace robot_model_loader +{ +MOVEIT_CLASS_FORWARD(RobotModelLoader); // Defines RobotModelLoaderPtr, ConstPtr, WeakPtr... etc + +/** @class RobotModelLoader */ +class RobotModelLoader +{ +public: + /** @brief Structure that encodes the options to be passed to the RobotModelLoader constructor */ + struct Options + { + Options(const std::string& robot_description = "robot_description") + : robot_description(robot_description), load_kinematics_solvers(true) + { + } + + Options(const std::string& urdf_string, const std::string& srdf_string) + : urdf_string_(urdf_string), srdf_string(srdf_string), load_kinematics_solvers(true) + { + } + + /** @brief The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter name + plus the "_planning" suffix, additional configuration can be specified (e.g., additional joint limits). + Loading from the param server is attempted only if loading from string fails. */ + std::string robot_description; + + /** @brief The string content of the URDF and SRDF documents. Loading from string is attempted only if loading from + * XML fails */ + std::string urdf_string_, srdf_string; + + /** @brief Flag indicating whether the kinematics solvers should be loaded as well, using specified ROS parameters + */ + bool load_kinematics_solvers; + }; + + /** @brief Default constructor */ + RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt = Options()); + + RobotModelLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, + bool load_kinematics_solvers = true); + + ~RobotModelLoader(); + + /** @brief Get the constructed planning_models::RobotModel */ + const moveit::core::RobotModelPtr& getModel() const + { + return model_; + } + + /** @brief Get the resolved parameter name for the robot description */ + const std::string& getRobotDescription() const + { + return rdf_loader_->getRobotDescription(); + } + + /** @brief Get the parsed URDF model*/ + const urdf::ModelInterfaceSharedPtr& getURDF() const + { + return rdf_loader_->getURDF(); + } + + /** @brief Get the parsed SRDF model*/ + const srdf::ModelSharedPtr& getSRDF() const + { + return rdf_loader_->getSRDF(); + } + + /** @brief Get the instance of rdf_loader::RDFLoader that was used to load the robot description */ + const rdf_loader::RDFLoaderPtr& getRDFLoader() const + { + return rdf_loader_; + } + + /** \brief Get the kinematics solvers plugin loader. + \note This instance needs to be kept in scope, otherwise kinematics solver plugins may get unloaded. */ + const kinematics_plugin_loader::KinematicsPluginLoaderPtr& getKinematicsPluginLoader() const + { + return kinematics_loader_; + } + + /** @brief Load the kinematics solvers into the kinematic model. This is done by default, unless disabled explicitly + * by the options passed to the constructor */ + void loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader = + kinematics_plugin_loader::KinematicsPluginLoaderPtr()); + +private: + void configure(const Options& opt); + + moveit::core::RobotModelPtr model_; + rdf_loader::RDFLoaderPtr rdf_loader_; + kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_; + const rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; +}; +} // namespace robot_model_loader diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 40a86a1483..5a6ea01291 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan, E. Gil Jones */ -#include +#include #include #include #include diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 1bb055e20d..211d09d6b8 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,314 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace trajectory_execution_manager -{ -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc - -// Two modes: -// Managed controllers -// Unmanaged controllers: given the trajectory, -class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager -{ -public: - static const std::string EXECUTION_EVENT_TOPIC; - - /// Definition of the function signature that is called when the execution of all the pushed trajectories completes. - /// The status of the overall execution is passed as argument - typedef std::function ExecutionCompleteCallback; - - /// Definition of the function signature that is called when the execution of a pushed trajectory completes - /// successfully. - using PathSegmentCompleteCallback = std::function; - - /// Data structure that represents information necessary to execute a trajectory - struct TrajectoryExecutionContext - { - /// The controllers to use for executing the different trajectory parts; - std::vector controllers_; - - // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one - // controller - std::vector trajectory_parts_; - }; - - /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, - const planning_scene_monitor::CurrentStateMonitorPtr& csm); - - /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, - const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers); - - /// Destructor. Cancels all running trajectories (if any) - ~TrajectoryExecutionManager(); - - /// If this function returns true, then this instance of the manager is allowed to load/unload/switch controllers - bool isManagingControllers() const; - - /// Get the instance of the controller manager used (this is the plugin instance loaded) - const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const; - - /** \brief Execute a named event (e.g., 'stop') */ - void processEvent(const std::string& event); - - /** \brief Make sure the active controllers are such that trajectories that actuate joints in the specified group can - be executed. - \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not - cover the joints in the group to be actuated, this function fails. */ - bool ensureActiveControllersForGroup(const std::string& group); - - /** \brief Make sure the active controllers are such that trajectories that actuate joints in the specified set can be - executed. - \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not - cover the joints to be actuated, this function fails. */ - bool ensureActiveControllersForJoints(const std::vector& joints); - - /** \brief Make sure a particular controller is active. - \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not - include the one specified as argument, this function fails. */ - bool ensureActiveController(const std::string& controller); - - /** \brief Make sure a particular set of controllers are active. - \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not - include the ones specified as argument, this function fails. */ - bool ensureActiveControllers(const std::vector& controllers); - - /** \brief Check if a controller is active. */ - bool isControllerActive(const std::string& controller); - - /** \brief Check if a set of controllers is active. */ - bool areControllersActive(const std::vector& controllers); - - /// Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. If no controller - /// is specified, a default is used. - bool push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& controller = ""); - - /// Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. If no controller - /// is specified, a default is used. - bool push(const trajectory_msgs::msg::JointTrajectory& trajectory, const std::string& controller = ""); - - /// Add a trajectory for future execution. Optionally specify a set of controllers to consider using for the - /// trajectory. Multiple controllers can be used simultaneously - /// to execute the different parts of the trajectory. If multiple controllers can be used, preference is given to the - /// already loaded ones. - /// If no controller is specified, a default is used. - bool push(const trajectory_msgs::msg::JointTrajectory& trajectory, const std::vector& controllers); - - /// Add a trajectory for future execution. Optionally specify a set of controllers to consider using for the - /// trajectory. Multiple controllers can be used simultaneously - /// to execute the different parts of the trajectory. If multiple controllers can be used, preference is given to the - /// already loaded ones. - /// If no controller is specified, a default is used. - bool push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::vector& controllers); - - /// Get the trajectories to be executed - const std::vector& getTrajectories() const; - - /// Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done. - void execute(const ExecutionCompleteCallback& callback = ExecutionCompleteCallback(), bool auto_clear = true); - - /// Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done. A - /// callback is also called for every trajectory part that completes successfully. - void execute(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback, - bool auto_clear = true); - - /// This is a blocking call for the execution of the passed in trajectories. This just calls execute() and - /// waitForExecution() - moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear = true); - - /// Wait until the execution is complete. This only works for executions started by execute(). If you call this after - /// pushAndExecute(), it will immediately stop execution. - moveit_controller_manager::ExecutionStatus waitForExecution(); - - /// Get the state that the robot is expected to be at, given current time, after execute() has been called. The return - /// value is a pair of two index values: - /// first = the index of the trajectory to be executed (in the order push() was called), second = the index of the - /// point within that trajectory. - /// Values of -1 are returned when there is no trajectory being executed, or if the trajectory was passed using - /// pushAndExecute(). - std::pair getCurrentExpectedTrajectoryIndex() const; - - /// Return the controller status for the last attempted execution - moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const; - - /// Stop whatever executions are active, if any - void stopExecution(bool auto_clear = true); - - /// Clear the trajectories to execute - void clear(); - - /// Enable or disable the monitoring of trajectory execution duration. If a controller takes - /// longer than expected, the trajectory is canceled - void enableExecutionDurationMonitoring(bool flag); - - /// Get the current status of the monitoring of trajectory execution duration. - bool executionDurationMonitoring() const; - - /// When determining the expected duration of a trajectory, this multiplicative factor is applied - /// to get the allowed duration of execution - void setAllowedExecutionDurationScaling(double scaling); - - /// Get the current scaling of the duration of a trajectory to get the allowed duration of execution. - double allowedExecutionDurationScaling() const; - - /// When determining the expected duration of a trajectory, this multiplicative factor is applied - /// to allow more than the expected execution time before triggering trajectory cancel - void setAllowedGoalDurationMargin(double margin); - - /// Get the current margin of the duration of a trajectory to get the allowed duration of execution. - double allowedGoalDurationMargin() const; - - /// Before sending a trajectory to a controller, scale the velocities by the factor specified. - /// By default, this is 1.0 - void setExecutionVelocityScaling(double scaling); - - /// Get the current scaling of the execution velocities. - double executionVelocityScaling() const; - - /// Set joint-value tolerance for validating trajectory's start point against current robot state - void setAllowedStartTolerance(double tolerance); - - /// Get the current joint-value for validating trajectory's start point against current robot state. - double allowedStartTolerance() const; - - /// Enable or disable waiting for trajectory completion - void setWaitForTrajectoryCompletion(bool flag); - - /// Get the current state of waiting for the trajectory being completed. - bool waitForTrajectoryCompletion() const; - - rclcpp::Node::SharedPtr getControllerManagerNode() - { - return controller_mgr_node_; - } - -private: - struct ControllerInformation - { - std::string name_; - std::set joints_; - std::set overlapping_controllers_; - moveit_controller_manager::MoveItControllerManager::ControllerState state_; - rclcpp::Time last_update_{ 0, 0, RCL_ROS_TIME }; - - bool operator<(ControllerInformation& other) const - { - if (joints_.size() != other.joints_.size()) - return joints_.size() < other.joints_.size(); - return name_ < other.name_; - } - }; - - void initialize(); - - void reloadControllerInformation(); - - /// Validate first point of trajectory matches current robot state - bool validate(const TrajectoryExecutionContext& context) const; - bool configure(TrajectoryExecutionContext& context, const moveit_msgs::msg::RobotTrajectory& trajectory, - const std::vector& controllers); - - void updateControllersState(const rclcpp::Duration& age); - void updateControllerState(const std::string& controller, const rclcpp::Duration& age); - void updateControllerState(ControllerInformation& ci, const rclcpp::Duration& age); - - bool distributeTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory, - const std::vector& controllers, - std::vector& parts); - - bool findControllers(const std::set& actuated_joints, std::size_t controller_count, - const std::vector& available_controllers, - std::vector& selected_controllers); - bool checkControllerCombination(std::vector& controllers, const std::set& actuated_joints); - void generateControllerCombination(std::size_t start_index, std::size_t controller_count, - const std::vector& available_controllers, - std::vector& selected_controllers, - std::vector >& selected_options, - const std::set& actuated_joints); - bool selectControllers(const std::set& actuated_joints, - const std::vector& available_controllers, - std::vector& selected_controllers); - - void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback, - bool auto_clear); - bool executePart(std::size_t part_index); - bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0); - - void stopExecutionInternal(); - - void receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event); - - void loadControllerParams(); - - // Name of this class for logging - const std::string name_ = "trajectory_execution_manager"; - - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - rclcpp::Node::SharedPtr controller_mgr_node_; - std::shared_ptr private_executor_; - std::thread private_executor_thread_; - moveit::core::RobotModelConstPtr robot_model_; - planning_scene_monitor::CurrentStateMonitorPtr csm_; - rclcpp::Subscription::SharedPtr event_topic_subscriber_; - std::map known_controllers_; - bool manage_controllers_; - - // thread used to execute trajectories using the execute() command - std::unique_ptr execution_thread_; - - std::mutex execution_state_mutex_; - std::mutex execution_thread_mutex_; - - // this condition is used to notify the completion of execution for given trajectories - std::condition_variable execution_complete_condition_; - - moveit_controller_manager::ExecutionStatus last_execution_status_; - std::vector active_handles_; - int current_context_; - std::vector time_index_; // used to find current expected trajectory location - mutable std::mutex time_index_mutex_; - bool execution_complete_; - - std::vector trajectories_; - - std::unique_ptr > controller_manager_loader_; - moveit_controller_manager::MoveItControllerManagerPtr controller_manager_; - - bool verbose_; - - bool execution_duration_monitoring_; - // 'global' values - double allowed_execution_duration_scaling_; - double allowed_goal_duration_margin_; - bool control_multi_dof_joint_variables_; - // controller-specific values - // override the 'global' values - std::map controller_allowed_execution_duration_scaling_; - std::map controller_allowed_goal_duration_margin_; - - double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints - double execution_velocity_scaling_; - bool wait_for_trajectory_completion_; - - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_; -}; -} // namespace trajectory_execution_manager +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp new file mode 100644 index 0000000000..b305371d54 --- /dev/null +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -0,0 +1,348 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace trajectory_execution_manager +{ +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc + +// Two modes: +// Managed controllers +// Unmanaged controllers: given the trajectory, +class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager +{ +public: + static const std::string EXECUTION_EVENT_TOPIC; + + /// Definition of the function signature that is called when the execution of all the pushed trajectories completes. + /// The status of the overall execution is passed as argument + typedef std::function ExecutionCompleteCallback; + + /// Definition of the function signature that is called when the execution of a pushed trajectory completes + /// successfully. + using PathSegmentCompleteCallback = std::function; + + /// Data structure that represents information necessary to execute a trajectory + struct TrajectoryExecutionContext + { + /// The controllers to use for executing the different trajectory parts; + std::vector controllers_; + + // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one + // controller + std::vector trajectory_parts_; + }; + + /// Load the controller manager plugin, start listening for events on a topic. + TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const planning_scene_monitor::CurrentStateMonitorPtr& csm); + + /// Load the controller manager plugin, start listening for events on a topic. + TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers); + + /// Destructor. Cancels all running trajectories (if any) + ~TrajectoryExecutionManager(); + + /// If this function returns true, then this instance of the manager is allowed to load/unload/switch controllers + bool isManagingControllers() const; + + /// Get the instance of the controller manager used (this is the plugin instance loaded) + const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const; + + /** \brief Execute a named event (e.g., 'stop') */ + void processEvent(const std::string& event); + + /** \brief Make sure the active controllers are such that trajectories that actuate joints in the specified group can + be executed. + \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not + cover the joints in the group to be actuated, this function fails. */ + bool ensureActiveControllersForGroup(const std::string& group); + + /** \brief Make sure the active controllers are such that trajectories that actuate joints in the specified set can be + executed. + \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not + cover the joints to be actuated, this function fails. */ + bool ensureActiveControllersForJoints(const std::vector& joints); + + /** \brief Make sure a particular controller is active. + \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not + include the one specified as argument, this function fails. */ + bool ensureActiveController(const std::string& controller); + + /** \brief Make sure a particular set of controllers are active. + \note If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not + include the ones specified as argument, this function fails. */ + bool ensureActiveControllers(const std::vector& controllers); + + /** \brief Check if a controller is active. */ + bool isControllerActive(const std::string& controller); + + /** \brief Check if a set of controllers is active. */ + bool areControllersActive(const std::vector& controllers); + + /// Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. If no controller + /// is specified, a default is used. + bool push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& controller = ""); + + /// Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. If no controller + /// is specified, a default is used. + bool push(const trajectory_msgs::msg::JointTrajectory& trajectory, const std::string& controller = ""); + + /// Add a trajectory for future execution. Optionally specify a set of controllers to consider using for the + /// trajectory. Multiple controllers can be used simultaneously + /// to execute the different parts of the trajectory. If multiple controllers can be used, preference is given to the + /// already loaded ones. + /// If no controller is specified, a default is used. + bool push(const trajectory_msgs::msg::JointTrajectory& trajectory, const std::vector& controllers); + + /// Add a trajectory for future execution. Optionally specify a set of controllers to consider using for the + /// trajectory. Multiple controllers can be used simultaneously + /// to execute the different parts of the trajectory. If multiple controllers can be used, preference is given to the + /// already loaded ones. + /// If no controller is specified, a default is used. + bool push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::vector& controllers); + + /// Get the trajectories to be executed + const std::vector& getTrajectories() const; + + /// Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done. + void execute(const ExecutionCompleteCallback& callback = ExecutionCompleteCallback(), bool auto_clear = true); + + /// Start the execution of pushed trajectories; this does not wait for completion, but calls a callback when done. A + /// callback is also called for every trajectory part that completes successfully. + void execute(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback, + bool auto_clear = true); + + /// This is a blocking call for the execution of the passed in trajectories. This just calls execute() and + /// waitForExecution() + moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear = true); + + /// Wait until the execution is complete. This only works for executions started by execute(). If you call this after + /// pushAndExecute(), it will immediately stop execution. + moveit_controller_manager::ExecutionStatus waitForExecution(); + + /// Get the state that the robot is expected to be at, given current time, after execute() has been called. The return + /// value is a pair of two index values: + /// first = the index of the trajectory to be executed (in the order push() was called), second = the index of the + /// point within that trajectory. + /// Values of -1 are returned when there is no trajectory being executed, or if the trajectory was passed using + /// pushAndExecute(). + std::pair getCurrentExpectedTrajectoryIndex() const; + + /// Return the controller status for the last attempted execution + moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const; + + /// Stop whatever executions are active, if any + void stopExecution(bool auto_clear = true); + + /// Clear the trajectories to execute + void clear(); + + /// Enable or disable the monitoring of trajectory execution duration. If a controller takes + /// longer than expected, the trajectory is canceled + void enableExecutionDurationMonitoring(bool flag); + + /// Get the current status of the monitoring of trajectory execution duration. + bool executionDurationMonitoring() const; + + /// When determining the expected duration of a trajectory, this multiplicative factor is applied + /// to get the allowed duration of execution + void setAllowedExecutionDurationScaling(double scaling); + + /// Get the current scaling of the duration of a trajectory to get the allowed duration of execution. + double allowedExecutionDurationScaling() const; + + /// When determining the expected duration of a trajectory, this multiplicative factor is applied + /// to allow more than the expected execution time before triggering trajectory cancel + void setAllowedGoalDurationMargin(double margin); + + /// Get the current margin of the duration of a trajectory to get the allowed duration of execution. + double allowedGoalDurationMargin() const; + + /// Before sending a trajectory to a controller, scale the velocities by the factor specified. + /// By default, this is 1.0 + void setExecutionVelocityScaling(double scaling); + + /// Get the current scaling of the execution velocities. + double executionVelocityScaling() const; + + /// Set joint-value tolerance for validating trajectory's start point against current robot state + void setAllowedStartTolerance(double tolerance); + + /// Get the current joint-value for validating trajectory's start point against current robot state. + double allowedStartTolerance() const; + + /// Enable or disable waiting for trajectory completion + void setWaitForTrajectoryCompletion(bool flag); + + /// Get the current state of waiting for the trajectory being completed. + bool waitForTrajectoryCompletion() const; + + rclcpp::Node::SharedPtr getControllerManagerNode() + { + return controller_mgr_node_; + } + +private: + struct ControllerInformation + { + std::string name_; + std::set joints_; + std::set overlapping_controllers_; + moveit_controller_manager::MoveItControllerManager::ControllerState state_; + rclcpp::Time last_update_{ 0, 0, RCL_ROS_TIME }; + + bool operator<(ControllerInformation& other) const + { + if (joints_.size() != other.joints_.size()) + return joints_.size() < other.joints_.size(); + return name_ < other.name_; + } + }; + + void initialize(); + + void reloadControllerInformation(); + + /// Validate first point of trajectory matches current robot state + bool validate(const TrajectoryExecutionContext& context) const; + bool configure(TrajectoryExecutionContext& context, const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers); + + void updateControllersState(const rclcpp::Duration& age); + void updateControllerState(const std::string& controller, const rclcpp::Duration& age); + void updateControllerState(ControllerInformation& ci, const rclcpp::Duration& age); + + bool distributeTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers, + std::vector& parts); + + bool findControllers(const std::set& actuated_joints, std::size_t controller_count, + const std::vector& available_controllers, + std::vector& selected_controllers); + bool checkControllerCombination(std::vector& controllers, const std::set& actuated_joints); + void generateControllerCombination(std::size_t start_index, std::size_t controller_count, + const std::vector& available_controllers, + std::vector& selected_controllers, + std::vector >& selected_options, + const std::set& actuated_joints); + bool selectControllers(const std::set& actuated_joints, + const std::vector& available_controllers, + std::vector& selected_controllers); + + void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback, + bool auto_clear); + bool executePart(std::size_t part_index); + bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0); + + void stopExecutionInternal(); + + void receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event); + + void loadControllerParams(); + + // Name of this class for logging + const std::string name_ = "trajectory_execution_manager"; + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Node::SharedPtr controller_mgr_node_; + std::shared_ptr private_executor_; + std::thread private_executor_thread_; + moveit::core::RobotModelConstPtr robot_model_; + planning_scene_monitor::CurrentStateMonitorPtr csm_; + rclcpp::Subscription::SharedPtr event_topic_subscriber_; + std::map known_controllers_; + bool manage_controllers_; + + // thread used to execute trajectories using the execute() command + std::unique_ptr execution_thread_; + + std::mutex execution_state_mutex_; + std::mutex execution_thread_mutex_; + + // this condition is used to notify the completion of execution for given trajectories + std::condition_variable execution_complete_condition_; + + moveit_controller_manager::ExecutionStatus last_execution_status_; + std::vector active_handles_; + int current_context_; + std::vector time_index_; // used to find current expected trajectory location + mutable std::mutex time_index_mutex_; + bool execution_complete_; + + std::vector trajectories_; + + std::unique_ptr > controller_manager_loader_; + moveit_controller_manager::MoveItControllerManagerPtr controller_manager_; + + bool verbose_; + + bool execution_duration_monitoring_; + // 'global' values + double allowed_execution_duration_scaling_; + double allowed_goal_duration_margin_; + bool control_multi_dof_joint_variables_; + // controller-specific values + // override the 'global' values + std::map controller_allowed_execution_duration_scaling_; + std::map controller_allowed_goal_duration_margin_; + + double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints + double execution_velocity_scaling_; + bool wait_for_trajectory_completion_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_; +}; +} // namespace trajectory_execution_manager diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 8a863ef80d..fd913c7960 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp index f6a7683f2b..00963d0792 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp @@ -43,8 +43,8 @@ #include // Main class -#include -#include +#include +#include // Msgs #include diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.hpp similarity index 99% rename from moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h rename to moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.hpp index 319a97c7ff..7c92386e50 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.hpp @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #include #include diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager_plugin.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager_plugin.cpp index 6da8ab307d..a8bc5f9fea 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager_plugin.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager_plugin.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include "test_moveit_controller_manager.h" +#include "test_moveit_controller_manager.hpp" #include PLUGINLIB_EXPORT_CLASS(test_moveit_controller_manager::TestRos2ControlManager, diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index 9911a9dd10..271fe6c6db 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,34 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -namespace moveit -{ -namespace planning_interface -{ -std::shared_ptr getSharedTF(); - -robot_model_loader::RobotModelLoaderPtr getSharedRobotModelLoader(const rclcpp::Node::SharedPtr& node, - const std::string& robot_description); - -moveit::core::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr& node, - const std::string& robot_description); - -/** - @brief getSharedStateMonitor - - @param node A rclcpp::Node::SharePtr to pass node specific configurations, such as callbacks queues. - @param robot_model - @param tf_buffer - @return - */ -planning_scene_monitor::CurrentStateMonitorPtr -getSharedStateMonitor(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, - const std::shared_ptr& tf_buffer); -} // namespace planning_interface -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp new file mode 100644 index 0000000000..0295e3216b --- /dev/null +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +std::shared_ptr getSharedTF(); + +robot_model_loader::RobotModelLoaderPtr getSharedRobotModelLoader(const rclcpp::Node::SharedPtr& node, + const std::string& robot_description); + +moveit::core::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr& node, + const std::string& robot_description); + +/** + @brief getSharedStateMonitor + + @param node A rclcpp::Node::SharePtr to pass node specific configurations, such as callbacks queues. + @param robot_model + @param tf_buffer + @return + */ +planning_scene_monitor::CurrentStateMonitorPtr +getSharedStateMonitor(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, + const std::shared_ptr& tf_buffer); +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp index 6f92f4115d..66b1fe08b4 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp +++ b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include using namespace planning_scene_monitor; using namespace robot_model_loader; diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 3c15fbafbe..c5a2cc62a6 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,943 +48,5 @@ /* Author: Ioan Sucan, Sachin Chitta */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#include - -namespace moveit -{ -/** \brief Simple interface to MoveIt components */ -namespace planning_interface -{ -MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc - -/** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h - - \brief Client class to conveniently use the ROS interfaces provided by the move_group node. - - This class includes many default settings to make things easy to use. */ -class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface -{ -public: - /** \brief Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description' */ - static const std::string ROBOT_DESCRIPTION; - - /** \brief Specification of options to use when constructing the MoveGroupInterface class */ - struct Options - { - Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, std::string move_group_namespace = "") - : group_name(std::move(group_name)) - , robot_description(std::move(desc)) - , move_group_namespace(std::move(move_group_namespace)) - { - } - - /// The group to construct the class instance for - std::string group_name; - - /// The robot description parameter name (if different from default) - std::string robot_description; - - /// Optionally, an instance of the RobotModel to use can be also specified - moveit::core::RobotModelConstPtr robot_model; - - /// The namespace for the move group node - std::string move_group_namespace; - }; - - MOVEIT_STRUCT_FORWARD(Plan); - - /// The representation of a motion plan (as ROS messages) - struct Plan - { - /// The full starting state used for planning - moveit_msgs::msg::RobotState start_state; - - /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) - moveit_msgs::msg::RobotTrajectory trajectory; - - /// The amount of time it took to generate the plan - double planning_time; - }; - - /** - \brief Construct a MoveGroupInterface instance call using a specified set of options \e opt. - - \param opt. A MoveGroupInterface::Options structure, if you pass a ros::NodeHandle with a specific callback queue, - it has to be of type ros::CallbackQueue - (which is the default type of callback queues used in ROS) - \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, - one will be constructed internally - \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting. - */ - MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1)); - - /** - \brief Construct a client for the MoveGroup action for a particular \e group. - - \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, - one will be constructed internally - \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting. - */ - MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1)); - - ~MoveGroupInterface(); - - /** - * @brief This class owns unique resources (e.g. action clients, threads) and its not very - * meaningful to copy. Pass by references, move it, or simply create multiple instances where - * required. - */ - MoveGroupInterface(const MoveGroupInterface&) = delete; - MoveGroupInterface& operator=(const MoveGroupInterface&) = delete; - - MoveGroupInterface(MoveGroupInterface&& other) noexcept; - MoveGroupInterface& operator=(MoveGroupInterface&& other) noexcept; - /** \brief Get the name of the group this instance operates on */ - const std::string& getName() const; - - /** \brief Get the names of the named robot states available as targets, both either remembered states or default - * states from srdf */ - const std::vector& getNamedTargets() const; - - /** \brief Get the tf2_ros::Buffer. */ - const std::shared_ptr& getTF() const; - - /** \brief Get the RobotModel object. */ - moveit::core::RobotModelConstPtr getRobotModel() const; - - /** \brief Get the ROS node handle of this instance operates on */ - const rclcpp::Node::SharedPtr& getNode() const; - - /** \brief Get the name of the frame in which the robot is planning */ - const std::string& getPlanningFrame() const; - - /** \brief Get the available planning group names */ - const std::vector& getJointModelGroupNames() const; - - /** \brief Get vector of names of joints available in move group */ - const std::vector& getJointNames() const; - - /** \brief Get vector of names of links available in move group */ - const std::vector& getLinkNames() const; - - /** \brief Get the joint angles for targets specified by name */ - std::map getNamedTargetValues(const std::string& name) const; - - /** \brief Get only the active (actuated) joints this instance operates on */ - const std::vector& getActiveJoints() const; - - /** \brief Get all the joints this instance operates on (including fixed joints)*/ - const std::vector& getJoints() const; - - /** \brief Get the number of variables used to describe the state of this group. This is larger or equal to the number - * of DOF. */ - unsigned int getVariableCount() const; - - /** \brief Get the descriptions of all planning plugins loaded by the action server */ - bool getInterfaceDescriptions(std::vector& desc) const; - - /** \brief Get the description of the default planning plugin loaded by the action server */ - bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription& desc) const; - - /** \brief Get the planner parameters for given group and planner_id */ - std::map getPlannerParams(const std::string& planner_id, - const std::string& group = "") const; - - /** \brief Set the planner parameters for given group and planner_id */ - void setPlannerParams(const std::string& planner_id, const std::string& group, - const std::map& params, bool bReplace = false); - - std::string getDefaultPlanningPipelineId() const; - - /** \brief Specify a planning pipeline to be used for further planning */ - void setPlanningPipelineId(const std::string& pipeline_id); - - /** \brief Get the current planning_pipeline_id */ - const std::string& getPlanningPipelineId() const; - - /** \brief Get the default planner of the current planning pipeline for the given group (or the pipeline's default) */ - std::string getDefaultPlannerId(const std::string& group = "") const; - - /** \brief Specify a planner to be used for further planning */ - void setPlannerId(const std::string& planner_id); - - /** \brief Get the current planner_id */ - const std::string& getPlannerId() const; - - /** \brief Specify the maximum amount of time to use when planning */ - void setPlanningTime(double seconds); - - /** \brief Set the number of times the motion plan is to be computed from scratch before the shortest solution is - * returned. The default value is 1.*/ - void setNumPlanningAttempts(unsigned int num_planning_attempts); - - /** \brief Set a scaling factor for optionally reducing the maximum joint velocity. - Allowed values are in (0,1]. The maximum joint velocity specified - in the robot model is multiplied by the factor. If the value is 0, it is set to - the default value, which is defined in joint_limits.yaml of the moveit_config. - If the value is greater than 1, it is set to 1.0. */ - void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); - - /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */ - double getMaxVelocityScalingFactor() const; - - /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. - Allowed values are in (0,1]. The maximum joint acceleration specified - in the robot model is multiplied by the factor. If the value is 0, it is set to - the default value, which is defined in joint_limits.yaml of the moveit_config. - If the value is greater than 1, it is set to 1.0. */ - void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); - - /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ - double getMaxAccelerationScalingFactor() const; - - /** \brief Get the number of seconds set by setPlanningTime() */ - double getPlanningTime() const; - - /** \brief Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configuration - * space */ - double getGoalJointTolerance() const; - - /** \brief Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where the - * end-effector must reach.*/ - double getGoalPositionTolerance() const; - - /** \brief Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll, pitch and - * yaw, in radians. */ - double getGoalOrientationTolerance() const; - - /** \brief Set the tolerance that is used for reaching the goal. For - joint state goals, this will be distance for each joint, in the - configuration space (radians or meters depending on joint type). For pose - goals this will be the radius of a sphere where the end-effector must - reach. This function simply triggers calls to setGoalPositionTolerance(), - setGoalOrientationTolerance() and setGoalJointTolerance(). */ - void setGoalTolerance(double tolerance); - - /** \brief Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint value - * target. */ - void setGoalJointTolerance(double tolerance); - - /** \brief Set the position tolerance that is used for reaching the goal when moving to a pose. */ - void setGoalPositionTolerance(double tolerance); - - /** \brief Set the orientation tolerance that is used for reaching the goal when moving to a pose. */ - void setGoalOrientationTolerance(double tolerance); - - /** \brief Specify the workspace bounding box. - The box is specified in the planning frame (i.e. relative to the robot root link start position). - This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the - robot relative to the world. */ - void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); - - /** \brief If a different start state should be considered instead of the current state of the robot, this function - * sets that state */ - void setStartState(const moveit_msgs::msg::RobotState& start_state); - - /** \brief If a different start state should be considered instead of the current state of the robot, this function - * sets that state */ - void setStartState(const moveit::core::RobotState& start_state); - - /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ - void setStartStateToCurrentState(); - - /** - * \name Setting a joint state target (goal) - * - * There are 2 types of goal targets: - * \li a JointValueTarget (aka JointStateTarget) specifies an absolute value for each joint (angle for rotational - *joints or position for prismatic joints). - * \li a PoseTarget (Position, Orientation, or Pose) specifies the pose of one or more end effectors (and the planner - *can use any joint values that reaches the pose(s)). - * - * Only one or the other is used for planning. Calling any of the - * set*JointValueTarget() functions sets the current goal target to the - * JointValueTarget. Calling any of the setPoseTarget(), - * setOrientationTarget(), setRPYTarget(), setPositionTarget() functions sets - * the current goal target to the Pose target. - */ - /**@{*/ - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e group_variable_values MUST exactly match the variable order as returned by - getJointValueTarget(std::vector&). - - This always sets all of the group's joint values. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::vector& group_variable_values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e variable_values is a map of joint variable names to values. Joints in - the group are used to set the JointValueTarget. Joints in the model but - not in the group are ignored. An exception is thrown if a joint name is - not found in the model. Joint variables in the group that are missing - from \e variable_values remain unchanged (to reset all target variables - to their current values in the robot use - setJointValueTarget(getCurrentJointValues())). - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::map& variable_values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e variable_names are variable joint names and variable_values are - variable joint positions. Joints in the group are used to set the - JointValueTarget. Joints in the model but not in the group are ignored. - An exception is thrown if a joint name is not found in the model. - Joint variables in the group that are missing from \e variable_names - remain unchanged (to reset all target variables to their current values - in the robot use setJointValueTarget(getCurrentJointValues())). - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - The target for all joints in the group are set to the value in \e robot_state. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const moveit::core::RobotState& robot_state); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e values MUST have one value for each variable in joint \e joint_name. - \e values are set as the target for this joint. - Other joint targets remain unchanged. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::string& joint_name, const std::vector& values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - Joint \e joint_name must be a 1-DOF joint. - \e value is set as the target for this joint. - Other joint targets remain unchanged. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::string& joint_name, double value); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e state is used to set the target joint state values. - Values not specified in \e state remain unchanged. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const sensor_msgs::msg::JointState& state); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then false is returned BUT THE PARTIAL - RESULT OF IK IS STILL SET AS THE GOAL. */ - bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then false is returned BUT THE PARTIAL - RESULT OF IK IS STILL SET AS THE GOAL. */ - bool setJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then false is returned BUT THE PARTIAL - RESULT OF IK IS STILL SET AS THE GOAL. */ - bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then an approximation is used. */ - bool setApproximateJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, - const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then an approximation is used. */ - bool setApproximateJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose, - const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then an approximation is used. */ - bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal to a random joint configuration - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. */ - void setRandomTarget(); - - /** \brief Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, - that are specified in the SRDF under the name \e name as a group state*/ - bool setNamedTarget(const std::string& name); - - /** \brief Get the current joint state goal in a form compatible to setJointValueTarget() */ - void getJointValueTarget(std::vector& group_variable_values) const; - - /**@}*/ - - /** - * \name Setting a pose target (goal) - * - * Setting a Pose (or Position or Orientation) target disables any previously - * set JointValueTarget. - * - * For groups that have multiple end effectors, a pose can be set for each - * end effector in the group. End effectors which do not have a pose target - * set will end up in arbitrary positions. - */ - /**@{*/ - - /** \brief Set the goal position of the end-effector \e end_effector_link to be (\e x, \e y, \e z). - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new position target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = ""); - - /** \brief Set the goal orientation of the end-effector \e end_effector_link to be (\e roll,\e pitch,\e yaw) radians. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = ""); - - /** \brief Set the goal orientation of the end-effector \e end_effector_link to be the quaternion (\e x,\e y,\e z,\e - w). - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = ""); - - /** \brief Set the goal pose of the end-effector \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new pose target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); - - /** \brief Set the goal pose of the end-effector \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPoseTarget(const geometry_msgs::msg::Pose& target, const std::string& end_effector_link = ""); - - /** \brief Set the goal pose of the end-effector \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPoseTarget(const geometry_msgs::msg::PoseStamped& target, const std::string& end_effector_link = ""); - - /** \brief Set goal poses for \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - When planning, the planner will find a path to one (arbitrarily chosen) - pose from the list. If this group contains multiple end effectors then - all end effectors in the group should have the same number of pose - targets. If planning is successful then the result of the plan will - place all end effectors at a pose from the same index in the list. (In - other words, if one end effector ends up at the 3rd pose in the list then - all end effectors in the group will end up at the 3rd pose in their - respective lists. End effectors which do not matter (i.e. can end up in - any position) can have their pose targets disabled by calling - clearPoseTarget() for that end_effector_link. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target(s) for this \e - end_effector_link. */ - bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); - - /** \brief Set goal poses for \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - When planning, the planner will find a path to one (arbitrarily chosen) - pose from the list. If this group contains multiple end effectors then - all end effectors in the group should have the same number of pose - targets. If planning is successful then the result of the plan will - place all end effectors at a pose from the same index in the list. (In - other words, if one end effector ends up at the 3rd pose in the list then - all end effectors in the group will end up at the 3rd pose in their - respective lists. End effectors which do not matter (i.e. can end up in - any position) can have their pose targets disabled by calling - clearPoseTarget() for that end_effector_link. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target(s) for this \e - end_effector_link. */ - bool setPoseTargets(const std::vector& target, const std::string& end_effector_link = ""); - - /** \brief Set goal poses for \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - When planning, the planner will find a path to one (arbitrarily chosen) - pose from the list. If this group contains multiple end effectors then - all end effectors in the group should have the same number of pose - targets. If planning is successful then the result of the plan will - place all end effectors at a pose from the same index in the list. (In - other words, if one end effector ends up at the 3rd pose in the list then - all end effectors in the group will end up at the 3rd pose in their - respective lists. End effectors which do not matter (i.e. can end up in - any position) can have their pose targets disabled by calling - clearPoseTarget() for that end_effector_link. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target(s) for this \e - end_effector_link. */ - bool setPoseTargets(const std::vector& target, - const std::string& end_effector_link = ""); - - /// Specify which reference frame to assume for poses specified without a reference frame. - void setPoseReferenceFrame(const std::string& pose_reference_frame); - - /** \brief Specify the parent link of the end-effector. - This \e end_effector_link will be used in calls to pose target functions - when end_effector_link is not explicitly specified. */ - bool setEndEffectorLink(const std::string& end_effector_link); - - /** \brief Specify the name of the end-effector to use. - This is equivalent to setting the EndEffectorLink to the parent link of this end effector. */ - bool setEndEffector(const std::string& eef_name); - - /// Forget pose(s) specified for \e end_effector_link - void clearPoseTarget(const std::string& end_effector_link = ""); - - /// Forget any poses specified for all end-effectors. - void clearPoseTargets(); - - /** Get the currently set pose goal for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed. - If multiple targets are set for \e end_effector_link this will return the first one. - If no pose target is set for this \e end_effector_link then an empty pose will be returned (check for - orientation.xyzw == 0). */ - const geometry_msgs::msg::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const; - - /** Get the currently set pose goal for the end-effector \e end_effector_link. The pose goal can consist of multiple - poses, - if corresponding setPoseTarget() calls were made. Otherwise, only one pose is returned in the vector. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - const std::vector& getPoseTargets(const std::string& end_effector_link = "") const; - - /** \brief Get the current end-effector link. - This returns the value set by setEndEffectorLink() (or indirectly by setEndEffector()). - If setEndEffectorLink() was not called, this function reports the link name that serves as parent - of an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. - If no such link is known, the empty string is returned. */ - const std::string& getEndEffectorLink() const; - - /** \brief Get the current end-effector name. - This returns the value set by setEndEffector() (or indirectly by setEndEffectorLink()). - If setEndEffector() was not called, this function reports an end-effector attached to this group. - If there are multiple end-effectors, one of them is returned. If no end-effector is known, the empty string is - returned. */ - const std::string& getEndEffector() const; - - /** \brief Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the robot - * model */ - const std::string& getPoseReferenceFrame() const; - - /**@}*/ - - /** - * \name Planning a path from the start position to the Target (goal) position, and executing that plan. - */ - /**@{*/ - - /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified - target. - This call is not blocking (does not wait for the execution of the trajectory to complete). */ - moveit::core::MoveItErrorCode asyncMove(); - - /** \brief Get the move_group action client used by the \e MoveGroupInterface. - The client can be used for querying the execution state of the trajectory and abort trajectory execution - during asynchronous execution. */ - - rclcpp_action::Client& getMoveGroupClient() const; - - /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified - target. - This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous - spinner to be started.*/ - moveit::core::MoveItErrorCode move(); - - /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the - specified - target. No execution is performed. The resulting plan is stored in \e plan*/ - moveit::core::MoveItErrorCode plan(Plan& plan); - - /** \brief Given a \e plan, execute it without waiting for completion. - * \param [in] plan The motion plan for which to execute - * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find - * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. - * \return moveit::core::MoveItErrorCode::SUCCESS if successful - */ - moveit::core::MoveItErrorCode asyncExecute(const Plan& plan, - const std::vector& controllers = std::vector()); - - /** \brief Given a \e robot trajectory, execute it without waiting for completion. - * \param [in] trajectory The trajectory to execute - * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find - * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. - * \return moveit::core::MoveItErrorCode::SUCCESS if successful - */ - moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory, - const std::vector& controllers = std::vector()); - - /** \brief Given a \e plan, execute it while waiting for completion. - * \param [in] plan Contains trajectory info as well as metadata such as a RobotModel. - * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find - * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. - * \return moveit::core::MoveItErrorCode::SUCCESS if successful - */ - moveit::core::MoveItErrorCode execute(const Plan& plan, - const std::vector& controllers = std::vector()); - - /** \brief Given a \e robot trajectory, execute it while waiting for completion. - * \param [in] trajectory The trajectory to execute - * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find - * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. - * \return moveit::core::MoveItErrorCode::SUCCESS if successful - */ - moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, - const std::vector& controllers = std::vector()); - - /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters - between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the - waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold - is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK - solutions). - Collisions are avoided if \e avoid_collisions is set to true. If collisions cannot be avoided, the function fails. - Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the - waypoints. - Return -1.0 in case of error. */ - [[deprecated("Drop jump_threshold")]] double // - computeCartesianPath(const std::vector& waypoints, double eef_step, - double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) - { - return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); - } - double computeCartesianPath(const std::vector& waypoints, double eef_step, - moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions = true, - moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); - - /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters - between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the - waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold - is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK - solutions). - Kinematic constraints for the path given by \e path_constraints will be met for every point along the trajectory, - if they are not met, a partial solution will be returned. - Constraints are checked (collision and kinematic) if \e avoid_collisions is set to true. If constraints cannot be - met, the function fails. - Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the - waypoints. - Return -1.0 in case of error. */ - [[deprecated("Drop jump_threshold")]] double // - computeCartesianPath(const std::vector& waypoints, double eef_step, - double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, - moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) - { - return computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, error_code); - } - double computeCartesianPath(const std::vector& waypoints, double eef_step, - moveit_msgs::msg::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, - moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); - - /** \brief Stop any trajectory execution, if one is active */ - void stop(); - - /** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */ - void allowReplanning(bool flag); - - /** \brief Maximum number of replanning attempts */ - void setReplanAttempts(int32_t attempts); - - /** \brief Sleep this duration between replanning attempts (in walltime seconds) */ - void setReplanDelay(double delay); - - /** \brief Specify whether the robot is allowed to look around - before moving if it determines it should (default is false) */ - void allowLooking(bool flag); - - /** \brief How often is the system allowed to move the camera to update environment model when looking */ - void setLookAroundAttempts(int32_t attempts); - - /** \brief Build a RobotState message for use with plan() or computeCartesianPath() - * If the move_group has a custom set start state, this method will use that as the robot state. - * - * Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state - * of the robot at time of the state's use. - */ - void constructRobotState(moveit_msgs::msg::RobotState& state); - - /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it - in \e request */ - void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request); - - /**@}*/ - - /** - * \name High level actions that trigger a sequence of plans and actions. - */ - /**@{*/ - - /** \brief Given the name of an object in the planning scene, make - the object attached to a link of the robot. If no link name is - specified, the end-effector is used. If there is no - end-effector, the first link in the group is used. If the object - name does not exist an error will be produced in move_group, but - the request made by this interface will succeed. */ - bool attachObject(const std::string& object, const std::string& link = ""); - - /** \brief Given the name of an object in the planning scene, make - the object attached to a link of the robot. The set of links the - object is allowed to touch without considering that a collision - is specified by \e touch_links. If \e link is empty, the - end-effector link is used. If there is no end-effector, the - first link in the group is used. If the object name does not - exist an error will be produced in move_group, but the request - made by this interface will succeed. */ - bool attachObject(const std::string& object, const std::string& link, const std::vector& touch_links); - - /** \brief Detach an object. \e name specifies the name of the - object attached to this group, or the name of the link the - object is attached to. If there is no name specified, and there - is only one attached object, that object is detached. An error - is produced if no object to detach is identified. */ - bool detachObject(const std::string& name = ""); - - /**@}*/ - - /** - * \name Query current robot state - */ - /**@{*/ - - /** \brief When reasoning about the current state of a robot, a - CurrentStateMonitor instance is automatically constructed. This - function allows triggering the construction of that object from - the beginning, so that future calls to functions such as - getCurrentState() will not take so long and are less likely to fail. */ - bool startStateMonitor(double wait = 1.0); - - /** \brief Get the current joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getCurrentJointValues() const; - - /** \brief Get the current state of the robot within the duration specified by wait. */ - moveit::core::RobotStatePtr getCurrentState(double wait = 1) const; - - /** \brief Get the pose for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - geometry_msgs::msg::PoseStamped getCurrentPose(const std::string& end_effector_link = "") const; - - /** \brief Get the roll-pitch-yaw (XYZ) for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - std::vector getCurrentRPY(const std::string& end_effector_link = "") const; - - /** \brief Get random joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getRandomJointValues() const; - - /** \brief Get a random reachable pose for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - geometry_msgs::msg::PoseStamped getRandomPose(const std::string& end_effector_link = "") const; - - /**@}*/ - - /** - * \name Manage named joint configurations - */ - /**@{*/ - - /** \brief Remember the current joint values (of the robot being monitored) under \e name. - These can be used by setNamedTarget(). - These values are remembered locally in the client. Other clients will - not have access to them. */ - void rememberJointValues(const std::string& name); - - /** \brief Remember the specified joint values under \e name. - These can be used by setNamedTarget(). - These values are remembered locally in the client. Other clients will - not have access to them. */ - void rememberJointValues(const std::string& name, const std::vector& values); - - /** \brief Get the currently remembered map of names to joint values. */ - const std::map >& getRememberedJointValues() const - { - return remembered_joint_values_; - } - - /** \brief Forget the joint values remembered under \e name */ - void forgetJointValues(const std::string& name); - - /**@}*/ - - /** - * \name Manage planning constraints - */ - /**@{*/ - - /** \brief Specify where the database server that holds known constraints resides */ - void setConstraintsDatabase(const std::string& host, unsigned int port); - - /** \brief Get the names of the known constraints as read from the Mongo database, if a connection was achieved. */ - std::vector getKnownConstraints() const; - - /** \brief Get the actual set of constraints in use with this MoveGroupInterface. - @return A copy of the current path constraints set for this interface - */ - moveit_msgs::msg::Constraints getPathConstraints() const; - - /** \brief Specify a set of path constraints to use. - The constraints are looked up by name from the Mongo database server. - This replaces any path constraints set in previous calls to setPathConstraints(). */ - bool setPathConstraints(const std::string& constraint); - - /** \brief Specify a set of path constraints to use. - This version does not require a database server. - This replaces any path constraints set in previous calls to setPathConstraints(). */ - void setPathConstraints(const moveit_msgs::msg::Constraints& constraint); - - /** \brief Specify that no path constraints are to be used. - This removes any path constraints set in previous calls to setPathConstraints(). */ - void clearPathConstraints(); - - moveit_msgs::msg::TrajectoryConstraints getTrajectoryConstraints() const; - void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& constraint); - void clearTrajectoryConstraints(); - - /**@}*/ - -protected: - /** return the full RobotState of the joint-space target, only for internal use */ - const moveit::core::RobotState& getTargetRobotState() const; - -private: - std::map > remembered_joint_values_; - class MoveGroupInterfaceImpl; - MoveGroupInterfaceImpl* impl_; - rclcpp::Logger logger_; -}; -} // namespace planning_interface -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp new file mode 100644 index 0000000000..7692dc4f6a --- /dev/null +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp @@ -0,0 +1,978 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, SRI International + * Copyright (c) 2012, 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, Sachin Chitta */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include + +namespace moveit +{ +/** \brief Simple interface to MoveIt components */ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc + +/** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h + + \brief Client class to conveniently use the ROS interfaces provided by the move_group node. + + This class includes many default settings to make things easy to use. */ +class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface +{ +public: + /** \brief Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description' */ + static const std::string ROBOT_DESCRIPTION; + + /** \brief Specification of options to use when constructing the MoveGroupInterface class */ + struct Options + { + Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, std::string move_group_namespace = "") + : group_name(std::move(group_name)) + , robot_description(std::move(desc)) + , move_group_namespace(std::move(move_group_namespace)) + { + } + + /// The group to construct the class instance for + std::string group_name; + + /// The robot description parameter name (if different from default) + std::string robot_description; + + /// Optionally, an instance of the RobotModel to use can be also specified + moveit::core::RobotModelConstPtr robot_model; + + /// The namespace for the move group node + std::string move_group_namespace; + }; + + MOVEIT_STRUCT_FORWARD(Plan); + + /// The representation of a motion plan (as ROS messages) + struct Plan + { + /// The full starting state used for planning + moveit_msgs::msg::RobotState start_state; + + /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) + moveit_msgs::msg::RobotTrajectory trajectory; + + /// The amount of time it took to generate the plan + double planning_time; + }; + + /** + \brief Construct a MoveGroupInterface instance call using a specified set of options \e opt. + + \param opt. A MoveGroupInterface::Options structure, if you pass a ros::NodeHandle with a specific callback queue, + it has to be of type ros::CallbackQueue + (which is the default type of callback queues used in ROS) + \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, + one will be constructed internally + \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting. + */ + MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt, + const std::shared_ptr& tf_buffer = std::shared_ptr(), + const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1)); + + /** + \brief Construct a client for the MoveGroup action for a particular \e group. + + \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, + one will be constructed internally + \param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting. + */ + MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group, + const std::shared_ptr& tf_buffer = std::shared_ptr(), + const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1)); + + ~MoveGroupInterface(); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveGroupInterface(const MoveGroupInterface&) = delete; + MoveGroupInterface& operator=(const MoveGroupInterface&) = delete; + + MoveGroupInterface(MoveGroupInterface&& other) noexcept; + MoveGroupInterface& operator=(MoveGroupInterface&& other) noexcept; + /** \brief Get the name of the group this instance operates on */ + const std::string& getName() const; + + /** \brief Get the names of the named robot states available as targets, both either remembered states or default + * states from srdf */ + const std::vector& getNamedTargets() const; + + /** \brief Get the tf2_ros::Buffer. */ + const std::shared_ptr& getTF() const; + + /** \brief Get the RobotModel object. */ + moveit::core::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node handle of this instance operates on */ + const rclcpp::Node::SharedPtr& getNode() const; + + /** \brief Get the name of the frame in which the robot is planning */ + const std::string& getPlanningFrame() const; + + /** \brief Get the available planning group names */ + const std::vector& getJointModelGroupNames() const; + + /** \brief Get vector of names of joints available in move group */ + const std::vector& getJointNames() const; + + /** \brief Get vector of names of links available in move group */ + const std::vector& getLinkNames() const; + + /** \brief Get the joint angles for targets specified by name */ + std::map getNamedTargetValues(const std::string& name) const; + + /** \brief Get only the active (actuated) joints this instance operates on */ + const std::vector& getActiveJoints() const; + + /** \brief Get all the joints this instance operates on (including fixed joints)*/ + const std::vector& getJoints() const; + + /** \brief Get the number of variables used to describe the state of this group. This is larger or equal to the number + * of DOF. */ + unsigned int getVariableCount() const; + + /** \brief Get the descriptions of all planning plugins loaded by the action server */ + bool getInterfaceDescriptions(std::vector& desc) const; + + /** \brief Get the description of the default planning plugin loaded by the action server */ + bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription& desc) const; + + /** \brief Get the planner parameters for given group and planner_id */ + std::map getPlannerParams(const std::string& planner_id, + const std::string& group = "") const; + + /** \brief Set the planner parameters for given group and planner_id */ + void setPlannerParams(const std::string& planner_id, const std::string& group, + const std::map& params, bool bReplace = false); + + std::string getDefaultPlanningPipelineId() const; + + /** \brief Specify a planning pipeline to be used for further planning */ + void setPlanningPipelineId(const std::string& pipeline_id); + + /** \brief Get the current planning_pipeline_id */ + const std::string& getPlanningPipelineId() const; + + /** \brief Get the default planner of the current planning pipeline for the given group (or the pipeline's default) */ + std::string getDefaultPlannerId(const std::string& group = "") const; + + /** \brief Specify a planner to be used for further planning */ + void setPlannerId(const std::string& planner_id); + + /** \brief Get the current planner_id */ + const std::string& getPlannerId() const; + + /** \brief Specify the maximum amount of time to use when planning */ + void setPlanningTime(double seconds); + + /** \brief Set the number of times the motion plan is to be computed from scratch before the shortest solution is + * returned. The default value is 1.*/ + void setNumPlanningAttempts(unsigned int num_planning_attempts); + + /** \brief Set a scaling factor for optionally reducing the maximum joint velocity. + Allowed values are in (0,1]. The maximum joint velocity specified + in the robot model is multiplied by the factor. If the value is 0, it is set to + the default value, which is defined in joint_limits.yaml of the moveit_config. + If the value is greater than 1, it is set to 1.0. */ + void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); + + /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */ + double getMaxVelocityScalingFactor() const; + + /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. + Allowed values are in (0,1]. The maximum joint acceleration specified + in the robot model is multiplied by the factor. If the value is 0, it is set to + the default value, which is defined in joint_limits.yaml of the moveit_config. + If the value is greater than 1, it is set to 1.0. */ + void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); + + /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ + double getMaxAccelerationScalingFactor() const; + + /** \brief Get the number of seconds set by setPlanningTime() */ + double getPlanningTime() const; + + /** \brief Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configuration + * space */ + double getGoalJointTolerance() const; + + /** \brief Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where the + * end-effector must reach.*/ + double getGoalPositionTolerance() const; + + /** \brief Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll, pitch and + * yaw, in radians. */ + double getGoalOrientationTolerance() const; + + /** \brief Set the tolerance that is used for reaching the goal. For + joint state goals, this will be distance for each joint, in the + configuration space (radians or meters depending on joint type). For pose + goals this will be the radius of a sphere where the end-effector must + reach. This function simply triggers calls to setGoalPositionTolerance(), + setGoalOrientationTolerance() and setGoalJointTolerance(). */ + void setGoalTolerance(double tolerance); + + /** \brief Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint value + * target. */ + void setGoalJointTolerance(double tolerance); + + /** \brief Set the position tolerance that is used for reaching the goal when moving to a pose. */ + void setGoalPositionTolerance(double tolerance); + + /** \brief Set the orientation tolerance that is used for reaching the goal when moving to a pose. */ + void setGoalOrientationTolerance(double tolerance); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief If a different start state should be considered instead of the current state of the robot, this function + * sets that state */ + void setStartState(const moveit_msgs::msg::RobotState& start_state); + + /** \brief If a different start state should be considered instead of the current state of the robot, this function + * sets that state */ + void setStartState(const moveit::core::RobotState& start_state); + + /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ + void setStartStateToCurrentState(); + + /** + * \name Setting a joint state target (goal) + * + * There are 2 types of goal targets: + * \li a JointValueTarget (aka JointStateTarget) specifies an absolute value for each joint (angle for rotational + *joints or position for prismatic joints). + * \li a PoseTarget (Position, Orientation, or Pose) specifies the pose of one or more end effectors (and the planner + *can use any joint values that reaches the pose(s)). + * + * Only one or the other is used for planning. Calling any of the + * set*JointValueTarget() functions sets the current goal target to the + * JointValueTarget. Calling any of the setPoseTarget(), + * setOrientationTarget(), setRPYTarget(), setPositionTarget() functions sets + * the current goal target to the Pose target. + */ + /**@{*/ + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e group_variable_values MUST exactly match the variable order as returned by + getJointValueTarget(std::vector&). + + This always sets all of the group's joint values. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::vector& group_variable_values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e variable_values is a map of joint variable names to values. Joints in + the group are used to set the JointValueTarget. Joints in the model but + not in the group are ignored. An exception is thrown if a joint name is + not found in the model. Joint variables in the group that are missing + from \e variable_values remain unchanged (to reset all target variables + to their current values in the robot use + setJointValueTarget(getCurrentJointValues())). + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::map& variable_values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e variable_names are variable joint names and variable_values are + variable joint positions. Joints in the group are used to set the + JointValueTarget. Joints in the model but not in the group are ignored. + An exception is thrown if a joint name is not found in the model. + Joint variables in the group that are missing from \e variable_names + remain unchanged (to reset all target variables to their current values + in the robot use setJointValueTarget(getCurrentJointValues())). + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + The target for all joints in the group are set to the value in \e robot_state. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const moveit::core::RobotState& robot_state); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e values MUST have one value for each variable in joint \e joint_name. + \e values are set as the target for this joint. + Other joint targets remain unchanged. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::string& joint_name, const std::vector& values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + Joint \e joint_name must be a 1-DOF joint. + \e value is set as the target for this joint. + Other joint targets remain unchanged. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::string& joint_name, double value); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e state is used to set the target joint state values. + Values not specified in \e state remain unchanged. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const sensor_msgs::msg::JointState& state); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then false is returned BUT THE PARTIAL + RESULT OF IK IS STILL SET AS THE GOAL. */ + bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then false is returned BUT THE PARTIAL + RESULT OF IK IS STILL SET AS THE GOAL. */ + bool setJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then false is returned BUT THE PARTIAL + RESULT OF IK IS STILL SET AS THE GOAL. */ + bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then an approximation is used. */ + bool setApproximateJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, + const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then an approximation is used. */ + bool setApproximateJointValueTarget(const geometry_msgs::msg::PoseStamped& eef_pose, + const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then an approximation is used. */ + bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal to a random joint configuration + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. */ + void setRandomTarget(); + + /** \brief Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, + that are specified in the SRDF under the name \e name as a group state*/ + bool setNamedTarget(const std::string& name); + + /** \brief Get the current joint state goal in a form compatible to setJointValueTarget() */ + void getJointValueTarget(std::vector& group_variable_values) const; + + /**@}*/ + + /** + * \name Setting a pose target (goal) + * + * Setting a Pose (or Position or Orientation) target disables any previously + * set JointValueTarget. + * + * For groups that have multiple end effectors, a pose can be set for each + * end effector in the group. End effectors which do not have a pose target + * set will end up in arbitrary positions. + */ + /**@{*/ + + /** \brief Set the goal position of the end-effector \e end_effector_link to be (\e x, \e y, \e z). + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new position target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = ""); + + /** \brief Set the goal orientation of the end-effector \e end_effector_link to be (\e roll,\e pitch,\e yaw) radians. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = ""); + + /** \brief Set the goal orientation of the end-effector \e end_effector_link to be the quaternion (\e x,\e y,\e z,\e + w). + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = ""); + + /** \brief Set the goal pose of the end-effector \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new pose target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); + + /** \brief Set the goal pose of the end-effector \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPoseTarget(const geometry_msgs::msg::Pose& target, const std::string& end_effector_link = ""); + + /** \brief Set the goal pose of the end-effector \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPoseTarget(const geometry_msgs::msg::PoseStamped& target, const std::string& end_effector_link = ""); + + /** \brief Set goal poses for \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + When planning, the planner will find a path to one (arbitrarily chosen) + pose from the list. If this group contains multiple end effectors then + all end effectors in the group should have the same number of pose + targets. If planning is successful then the result of the plan will + place all end effectors at a pose from the same index in the list. (In + other words, if one end effector ends up at the 3rd pose in the list then + all end effectors in the group will end up at the 3rd pose in their + respective lists. End effectors which do not matter (i.e. can end up in + any position) can have their pose targets disabled by calling + clearPoseTarget() for that end_effector_link. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target(s) for this \e + end_effector_link. */ + bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); + + /** \brief Set goal poses for \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + When planning, the planner will find a path to one (arbitrarily chosen) + pose from the list. If this group contains multiple end effectors then + all end effectors in the group should have the same number of pose + targets. If planning is successful then the result of the plan will + place all end effectors at a pose from the same index in the list. (In + other words, if one end effector ends up at the 3rd pose in the list then + all end effectors in the group will end up at the 3rd pose in their + respective lists. End effectors which do not matter (i.e. can end up in + any position) can have their pose targets disabled by calling + clearPoseTarget() for that end_effector_link. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target(s) for this \e + end_effector_link. */ + bool setPoseTargets(const std::vector& target, const std::string& end_effector_link = ""); + + /** \brief Set goal poses for \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + When planning, the planner will find a path to one (arbitrarily chosen) + pose from the list. If this group contains multiple end effectors then + all end effectors in the group should have the same number of pose + targets. If planning is successful then the result of the plan will + place all end effectors at a pose from the same index in the list. (In + other words, if one end effector ends up at the 3rd pose in the list then + all end effectors in the group will end up at the 3rd pose in their + respective lists. End effectors which do not matter (i.e. can end up in + any position) can have their pose targets disabled by calling + clearPoseTarget() for that end_effector_link. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target(s) for this \e + end_effector_link. */ + bool setPoseTargets(const std::vector& target, + const std::string& end_effector_link = ""); + + /// Specify which reference frame to assume for poses specified without a reference frame. + void setPoseReferenceFrame(const std::string& pose_reference_frame); + + /** \brief Specify the parent link of the end-effector. + This \e end_effector_link will be used in calls to pose target functions + when end_effector_link is not explicitly specified. */ + bool setEndEffectorLink(const std::string& end_effector_link); + + /** \brief Specify the name of the end-effector to use. + This is equivalent to setting the EndEffectorLink to the parent link of this end effector. */ + bool setEndEffector(const std::string& eef_name); + + /// Forget pose(s) specified for \e end_effector_link + void clearPoseTarget(const std::string& end_effector_link = ""); + + /// Forget any poses specified for all end-effectors. + void clearPoseTargets(); + + /** Get the currently set pose goal for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed. + If multiple targets are set for \e end_effector_link this will return the first one. + If no pose target is set for this \e end_effector_link then an empty pose will be returned (check for + orientation.xyzw == 0). */ + const geometry_msgs::msg::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const; + + /** Get the currently set pose goal for the end-effector \e end_effector_link. The pose goal can consist of multiple + poses, + if corresponding setPoseTarget() calls were made. Otherwise, only one pose is returned in the vector. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + const std::vector& getPoseTargets(const std::string& end_effector_link = "") const; + + /** \brief Get the current end-effector link. + This returns the value set by setEndEffectorLink() (or indirectly by setEndEffector()). + If setEndEffectorLink() was not called, this function reports the link name that serves as parent + of an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. + If no such link is known, the empty string is returned. */ + const std::string& getEndEffectorLink() const; + + /** \brief Get the current end-effector name. + This returns the value set by setEndEffector() (or indirectly by setEndEffectorLink()). + If setEndEffector() was not called, this function reports an end-effector attached to this group. + If there are multiple end-effectors, one of them is returned. If no end-effector is known, the empty string is + returned. */ + const std::string& getEndEffector() const; + + /** \brief Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the robot + * model */ + const std::string& getPoseReferenceFrame() const; + + /**@}*/ + + /** + * \name Planning a path from the start position to the Target (goal) position, and executing that plan. + */ + /**@{*/ + + /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified + target. + This call is not blocking (does not wait for the execution of the trajectory to complete). */ + moveit::core::MoveItErrorCode asyncMove(); + + /** \brief Get the move_group action client used by the \e MoveGroupInterface. + The client can be used for querying the execution state of the trajectory and abort trajectory execution + during asynchronous execution. */ + + rclcpp_action::Client& getMoveGroupClient() const; + + /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified + target. + This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous + spinner to be started.*/ + moveit::core::MoveItErrorCode move(); + + /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the + specified + target. No execution is performed. The resulting plan is stored in \e plan*/ + moveit::core::MoveItErrorCode plan(Plan& plan); + + /** \brief Given a \e plan, execute it without waiting for completion. + * \param [in] plan The motion plan for which to execute + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode asyncExecute(const Plan& plan, + const std::vector& controllers = std::vector()); + + /** \brief Given a \e robot trajectory, execute it without waiting for completion. + * \param [in] trajectory The trajectory to execute + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers = std::vector()); + + /** \brief Given a \e plan, execute it while waiting for completion. + * \param [in] plan Contains trajectory info as well as metadata such as a RobotModel. + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode execute(const Plan& plan, + const std::vector& controllers = std::vector()); + + /** \brief Given a \e robot trajectory, execute it while waiting for completion. + * \param [in] trajectory The trajectory to execute + * \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find + * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. + * \return moveit::core::MoveItErrorCode::SUCCESS if successful + */ + moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, + const std::vector& controllers = std::vector()); + + /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters + between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the + waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold + is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK + solutions). + Collisions are avoided if \e avoid_collisions is set to true. If collisions cannot be avoided, the function fails. + Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the + waypoints. + Return -1.0 in case of error. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); + } + double computeCartesianPath(const std::vector& waypoints, double eef_step, + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + + /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters + between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the + waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold + is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK + solutions). + Kinematic constraints for the path given by \e path_constraints will be met for every point along the trajectory, + if they are not met, a partial solution will be returned. + Constraints are checked (collision and kinematic) if \e avoid_collisions is set to true. If constraints cannot be + met, the function fails. + Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the + waypoints. + Return -1.0 in case of error. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, error_code); + } + double computeCartesianPath(const std::vector& waypoints, double eef_step, + moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + + /** \brief Stop any trajectory execution, if one is active */ + void stop(); + + /** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */ + void allowReplanning(bool flag); + + /** \brief Maximum number of replanning attempts */ + void setReplanAttempts(int32_t attempts); + + /** \brief Sleep this duration between replanning attempts (in walltime seconds) */ + void setReplanDelay(double delay); + + /** \brief Specify whether the robot is allowed to look around + before moving if it determines it should (default is false) */ + void allowLooking(bool flag); + + /** \brief How often is the system allowed to move the camera to update environment model when looking */ + void setLookAroundAttempts(int32_t attempts); + + /** \brief Build a RobotState message for use with plan() or computeCartesianPath() + * If the move_group has a custom set start state, this method will use that as the robot state. + * + * Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state + * of the robot at time of the state's use. + */ + void constructRobotState(moveit_msgs::msg::RobotState& state); + + /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it + in \e request */ + void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request); + + /**@}*/ + + /** + * \name High level actions that trigger a sequence of plans and actions. + */ + /**@{*/ + + /** \brief Given the name of an object in the planning scene, make + the object attached to a link of the robot. If no link name is + specified, the end-effector is used. If there is no + end-effector, the first link in the group is used. If the object + name does not exist an error will be produced in move_group, but + the request made by this interface will succeed. */ + bool attachObject(const std::string& object, const std::string& link = ""); + + /** \brief Given the name of an object in the planning scene, make + the object attached to a link of the robot. The set of links the + object is allowed to touch without considering that a collision + is specified by \e touch_links. If \e link is empty, the + end-effector link is used. If there is no end-effector, the + first link in the group is used. If the object name does not + exist an error will be produced in move_group, but the request + made by this interface will succeed. */ + bool attachObject(const std::string& object, const std::string& link, const std::vector& touch_links); + + /** \brief Detach an object. \e name specifies the name of the + object attached to this group, or the name of the link the + object is attached to. If there is no name specified, and there + is only one attached object, that object is detached. An error + is produced if no object to detach is identified. */ + bool detachObject(const std::string& name = ""); + + /**@}*/ + + /** + * \name Query current robot state + */ + /**@{*/ + + /** \brief When reasoning about the current state of a robot, a + CurrentStateMonitor instance is automatically constructed. This + function allows triggering the construction of that object from + the beginning, so that future calls to functions such as + getCurrentState() will not take so long and are less likely to fail. */ + bool startStateMonitor(double wait = 1.0); + + /** \brief Get the current joint values for the joints planned for by this instance (see getJoints()) */ + std::vector getCurrentJointValues() const; + + /** \brief Get the current state of the robot within the duration specified by wait. */ + moveit::core::RobotStatePtr getCurrentState(double wait = 1) const; + + /** \brief Get the pose for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + geometry_msgs::msg::PoseStamped getCurrentPose(const std::string& end_effector_link = "") const; + + /** \brief Get the roll-pitch-yaw (XYZ) for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + std::vector getCurrentRPY(const std::string& end_effector_link = "") const; + + /** \brief Get random joint values for the joints planned for by this instance (see getJoints()) */ + std::vector getRandomJointValues() const; + + /** \brief Get a random reachable pose for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + geometry_msgs::msg::PoseStamped getRandomPose(const std::string& end_effector_link = "") const; + + /**@}*/ + + /** + * \name Manage named joint configurations + */ + /**@{*/ + + /** \brief Remember the current joint values (of the robot being monitored) under \e name. + These can be used by setNamedTarget(). + These values are remembered locally in the client. Other clients will + not have access to them. */ + void rememberJointValues(const std::string& name); + + /** \brief Remember the specified joint values under \e name. + These can be used by setNamedTarget(). + These values are remembered locally in the client. Other clients will + not have access to them. */ + void rememberJointValues(const std::string& name, const std::vector& values); + + /** \brief Get the currently remembered map of names to joint values. */ + const std::map >& getRememberedJointValues() const + { + return remembered_joint_values_; + } + + /** \brief Forget the joint values remembered under \e name */ + void forgetJointValues(const std::string& name); + + /**@}*/ + + /** + * \name Manage planning constraints + */ + /**@{*/ + + /** \brief Specify where the database server that holds known constraints resides */ + void setConstraintsDatabase(const std::string& host, unsigned int port); + + /** \brief Get the names of the known constraints as read from the Mongo database, if a connection was achieved. */ + std::vector getKnownConstraints() const; + + /** \brief Get the actual set of constraints in use with this MoveGroupInterface. + @return A copy of the current path constraints set for this interface + */ + moveit_msgs::msg::Constraints getPathConstraints() const; + + /** \brief Specify a set of path constraints to use. + The constraints are looked up by name from the Mongo database server. + This replaces any path constraints set in previous calls to setPathConstraints(). */ + bool setPathConstraints(const std::string& constraint); + + /** \brief Specify a set of path constraints to use. + This version does not require a database server. + This replaces any path constraints set in previous calls to setPathConstraints(). */ + void setPathConstraints(const moveit_msgs::msg::Constraints& constraint); + + /** \brief Specify that no path constraints are to be used. + This removes any path constraints set in previous calls to setPathConstraints(). */ + void clearPathConstraints(); + + moveit_msgs::msg::TrajectoryConstraints getTrajectoryConstraints() const; + void setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& constraint); + void clearTrajectoryConstraints(); + + /**@}*/ + +protected: + /** return the full RobotState of the joint-space target, only for internal use */ + const moveit::core::RobotState& getTargetRobotState() const; + +private: + std::map > remembered_joint_values_; + class MoveGroupInterfaceImpl; + MoveGroupInterfaceImpl* impl_; + rclcpp::Logger logger_; +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index e729707913..b7238268d5 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -40,23 +40,23 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include #include #include -#include +#include #include #include diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 4fbe501e31..632b1301c0 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,118 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace planning_interface -{ -MOVEIT_CLASS_FORWARD(PlanningSceneInterface); // Defines PlanningSceneInterfacePtr, ConstPtr, WeakPtr... etc - -class PlanningSceneInterface -{ -public: - /** - \param ns. Namespace in which all MoveIt related topics and services are discovered - \param wait. Wait for services if they are not announced in ROS. - If this is false, the constructor throws std::runtime_error instead. - */ - explicit PlanningSceneInterface(const std::string& ns = "", bool wait = true); - ~PlanningSceneInterface(); - - /** - * \name Manage the world - */ - /**@{*/ - - /** \brief Get the names of all known objects in the world. If \e with_type is set to true, only return objects that - * have a known type. */ - std::vector getKnownObjectNames(bool with_type = false); - - /** \brief Get the names of known objects in the world that are located within a bounding region (specified in the - frame reported by getPlanningFrame()). - + If \e with_type is set to true, only return objects that have a known type. */ - std::vector getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, - double maxz, bool with_type, std::vector& types); - - /** \brief Get the names of known objects in the world that are located within a bounding region (specified in the - frame reported by getPlanningFrame()). - If \e with_type is set to true, only return objects that have a known type. */ - std::vector getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, - double maxz, bool with_type = false) - { - std::vector empty_vector_string; - return getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, empty_vector_string); - }; - - /** \brief Get the poses from the objects identified by the given object ids list. */ - std::map getObjectPoses(const std::vector& object_ids); - - /** \brief Get the objects identified by the given object ids list. If no ids are provided, return all the known - * objects. */ - std::map - getObjects(const std::vector& object_ids = std::vector()); - - /** \brief Get the attached objects identified by the given object ids list. If no ids are provided, return all the - * attached objects. */ - std::map - getAttachedObjects(const std::vector& object_ids = std::vector()); - - /** \brief Apply collision object to the planning scene of the move_group node synchronously. - Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ - bool applyCollisionObject(const moveit_msgs::msg::CollisionObject& collision_object); - - /** \brief Apply collision object to the planning scene of the move_group node synchronously. - Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ - bool applyCollisionObject(const moveit_msgs::msg::CollisionObject& collision_object, - const std_msgs::msg::ColorRGBA& object_color); - - /** \brief Apply collision objects to the planning scene of the move_group node synchronously. - Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene. - If object_colors do not specify an id, the corresponding object id from collision_objects is used. */ - bool applyCollisionObjects( - const std::vector& collision_objects, - const std::vector& object_colors = std::vector()); - - /** \brief Apply attached collision object to the planning scene of the move_group node synchronously. - Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ - bool applyAttachedCollisionObject(const moveit_msgs::msg::AttachedCollisionObject& attached_collision_object); - - /** \brief Apply attached collision objects to the planning scene of the move_group node synchronously. - Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ - bool applyAttachedCollisionObjects( - const std::vector& attached_collision_objects); - - /** \brief Update the planning_scene of the move_group node with the given ps synchronously. - Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ - bool applyPlanningScene(const moveit_msgs::msg::PlanningScene& ps); - - /** \brief Add collision objects to the world via /planning_scene. - Make sure object.operation is set to object.ADD. - - The update runs asynchronously. If you need the objects to be available *directly* after you called this function, - consider using `applyCollisionObjects` instead. */ - void addCollisionObjects(const std::vector& collision_objects, - const std::vector& object_colors = - std::vector()) const; - - /** \brief Remove collision objects from the world via /planning_scene. - - The update runs asynchronously. If you need the objects to be removed *directly* after you called this function, - consider using `applyCollisionObjects` instead. */ - void removeCollisionObjects(const std::vector& object_ids) const; - - /**@}*/ - -private: - class PlanningSceneInterfaceImpl; - PlanningSceneInterfaceImpl* impl_; -}; -} // namespace planning_interface -} // namespace moveit +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.hpp b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.hpp new file mode 100644 index 0000000000..e581ab5a42 --- /dev/null +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.hpp @@ -0,0 +1,152 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningSceneInterface); // Defines PlanningSceneInterfacePtr, ConstPtr, WeakPtr... etc + +class PlanningSceneInterface +{ +public: + /** + \param ns. Namespace in which all MoveIt related topics and services are discovered + \param wait. Wait for services if they are not announced in ROS. + If this is false, the constructor throws std::runtime_error instead. + */ + explicit PlanningSceneInterface(const std::string& ns = "", bool wait = true); + ~PlanningSceneInterface(); + + /** + * \name Manage the world + */ + /**@{*/ + + /** \brief Get the names of all known objects in the world. If \e with_type is set to true, only return objects that + * have a known type. */ + std::vector getKnownObjectNames(bool with_type = false); + + /** \brief Get the names of known objects in the world that are located within a bounding region (specified in the + frame reported by getPlanningFrame()). + + If \e with_type is set to true, only return objects that have a known type. */ + std::vector getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, + double maxz, bool with_type, std::vector& types); + + /** \brief Get the names of known objects in the world that are located within a bounding region (specified in the + frame reported by getPlanningFrame()). + If \e with_type is set to true, only return objects that have a known type. */ + std::vector getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, + double maxz, bool with_type = false) + { + std::vector empty_vector_string; + return getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, empty_vector_string); + }; + + /** \brief Get the poses from the objects identified by the given object ids list. */ + std::map getObjectPoses(const std::vector& object_ids); + + /** \brief Get the objects identified by the given object ids list. If no ids are provided, return all the known + * objects. */ + std::map + getObjects(const std::vector& object_ids = std::vector()); + + /** \brief Get the attached objects identified by the given object ids list. If no ids are provided, return all the + * attached objects. */ + std::map + getAttachedObjects(const std::vector& object_ids = std::vector()); + + /** \brief Apply collision object to the planning scene of the move_group node synchronously. + Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ + bool applyCollisionObject(const moveit_msgs::msg::CollisionObject& collision_object); + + /** \brief Apply collision object to the planning scene of the move_group node synchronously. + Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ + bool applyCollisionObject(const moveit_msgs::msg::CollisionObject& collision_object, + const std_msgs::msg::ColorRGBA& object_color); + + /** \brief Apply collision objects to the planning scene of the move_group node synchronously. + Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene. + If object_colors do not specify an id, the corresponding object id from collision_objects is used. */ + bool applyCollisionObjects( + const std::vector& collision_objects, + const std::vector& object_colors = std::vector()); + + /** \brief Apply attached collision object to the planning scene of the move_group node synchronously. + Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ + bool applyAttachedCollisionObject(const moveit_msgs::msg::AttachedCollisionObject& attached_collision_object); + + /** \brief Apply attached collision objects to the planning scene of the move_group node synchronously. + Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ + bool applyAttachedCollisionObjects( + const std::vector& attached_collision_objects); + + /** \brief Update the planning_scene of the move_group node with the given ps synchronously. + Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */ + bool applyPlanningScene(const moveit_msgs::msg::PlanningScene& ps); + + /** \brief Add collision objects to the world via /planning_scene. + Make sure object.operation is set to object.ADD. + + The update runs asynchronously. If you need the objects to be available *directly* after you called this function, + consider using `applyCollisionObjects` instead. */ + void addCollisionObjects(const std::vector& collision_objects, + const std::vector& object_colors = + std::vector()) const; + + /** \brief Remove collision objects from the world via /planning_scene. + + The update runs asynchronously. If you need the objects to be removed *directly* after you called this function, + consider using `applyCollisionObjects` instead. */ + void removeCollisionObjects(const std::vector& object_ids) const; + + /**@}*/ + +private: + class PlanningSceneInterfaceImpl; + PlanningSceneInterfaceImpl* impl_; +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index a4277f0731..036e34dfd3 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 2b0595005e..13b98f75aa 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -52,10 +52,10 @@ #include // MoveIt -#include -#include -#include -#include +#include +#include +#include +#include // TF2 #include diff --git a/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp b/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp index c34992291d..8d72dee6de 100644 --- a/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp @@ -45,9 +45,9 @@ #include // MoveIt -#include -#include -#include +#include +#include +#include #include // accuracy tested for position and orientation diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp index b866015e00..602cc39daf 100644 --- a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp @@ -49,8 +49,8 @@ #include // MoveIt -#include -#include +#include +#include // TF2 #include diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp index 4306daad63..615f666bd6 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -43,8 +43,8 @@ #include // Main class -#include -#include +#include +#include // Msgs #include diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index 1f07dd30d8..8cafabfb77 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -50,9 +50,9 @@ #include // MoveIt -#include -#include -#include +#include +#include +#include // TF2 #include diff --git a/moveit_ros/planning_interface/test/test_cleanup.cpp b/moveit_ros/planning_interface/test/test_cleanup.cpp index d807425057..99b1c91ebc 100644 --- a/moveit_ros/planning_interface/test/test_cleanup.cpp +++ b/moveit_ros/planning_interface/test/test_cleanup.cpp @@ -31,7 +31,7 @@ /* Author: Robert Haschke */ -#include +#include #include int main(int argc, char** argv) diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h index 9f7fd9a197..79c0c12257 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,142 +47,5 @@ /* Author: Acorn Pooley */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace core -{ -class RobotState; -} -} // namespace moveit - -namespace robot_interaction -{ -namespace InteractionStyle -{ -/// The different types of interaction that can be constructed for an end -/// effector This is a bitmask so OR together the parts you want. -enum InteractionStyle -{ - POSITION_ARROWS = 1, // arrows to change position - ORIENTATION_CIRCLES = 2, // circles to change orientation - POSITION_SPHERE = 4, // sphere: drag to change position - ORIENTATION_SPHERE = 8, // sphere: drag to change orientation - POSITION_EEF = 16, // drag end effector to change position - ORIENTATION_EEF = 32, // drag end effector to change orientation - FIXED = 64, // keep arrow and circle axis fixed - - POSITION = POSITION_ARROWS | POSITION_SPHERE | POSITION_EEF, - ORIENTATION = ORIENTATION_CIRCLES | ORIENTATION_SPHERE | ORIENTATION_EEF, - SIX_DOF = POSITION | ORIENTATION, - SIX_DOF_SPHERE = POSITION_SPHERE | ORIENTATION_SPHERE, - POSITION_NOSPHERE = POSITION_ARROWS | POSITION_EEF, - ORIENTATION_NOSPHERE = ORIENTATION_CIRCLES | ORIENTATION_EEF, - SIX_DOF_NOSPHERE = POSITION_NOSPHERE | ORIENTATION_NOSPHERE -}; -} // namespace InteractionStyle - -/// Type of function for constructing markers. -/// This callback sets up the marker used for an interaction. -/// @param state the current state of the robot -/// @param marker the function should fill this in with an InteractiveMarker -/// that will be used to control the interaction. -/// @returns true if the function succeeds, false if the function was not able -/// to fill in \e marker. -typedef std::function - InteractiveMarkerConstructorFn; - -/// Type of function for processing marker feedback. -/// This callback function handles feedback for an Interaction's marker. -/// Callback should update the robot state that was passed in according to -/// the new position of the marker. -/// -/// @param state the current state of the robot -/// @param marker the function should fill this in with an InteractiveMarker -/// that will be used to control the interaction. -/// @returns false if the state was not successfully updated or the new state -/// is somehow invalid or erronious (e.g. in collision). true if -/// everything worked well. -typedef std::function - ProcessFeedbackFn; - -/// Type of function for updating marker pose for new state. -/// This callback is called when the robot state has changed. -/// Callback should calculate a new -/// pose for the marker based on the passed in robot state. -/// @param state the new state of the robot -/// @param pose the function should fill this in with the new pose of the -/// marker, given the new state of the robot. -/// @returns true if the pose was modified, false if no update is needed (i.e. -/// if the pose did not change). -typedef std::function InteractiveMarkerUpdateFn; - -/// Representation of a generic interaction. -/// Displays one interactive marker. -struct GenericInteraction -{ - // Callback to construct interactive marker. - // See comment on typedef above. - InteractiveMarkerConstructorFn construct_marker; - - // Callback to handle interactive marker feedback messages. - // See comment on typedef above. - ProcessFeedbackFn process_feedback; - - // Callback to update marker pose when RobotState changes. - // See comment on typedef above. - InteractiveMarkerUpdateFn update_pose; - - // Suffix added to name of markers. - // Automatically generated. - std::string marker_name_suffix; -}; - -/// Representation of an interaction via an end-effector -/// Displays one marker for manipulating the EEF position. -struct EndEffectorInteraction -{ - /// The name of the group that sustains the end-effector (usually an arm) - std::string parent_group; - - /// The name of the link in the parent group that connects to the end-effector - std::string parent_link; - - /// The name of the group that defines the group joints - std::string eef_group; - - /// Which degrees of freedom to enable for the end-effector - InteractionStyle::InteractionStyle interaction; - - /// The size of the end effector group (diameter of enclosing sphere) - double size; -}; - -/// Representation of a joint interaction. -/// Displays one marker for manipulating the joint. -struct JointInteraction -{ - /// The link in the robot model this joint is a parent of - std::string connecting_link; - - /// The name of the frame that is a parent of this joint - std::string parent_frame; - - /// The name of the joint - std::string joint_name; - - /// The type of joint disguised as the number of DOF it has. 3=PLANAR in X/Y; 6=FLOATING - unsigned int dof; - - /// The size of the connecting link (diameter of enclosing sphere) - double size; -}; -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.hpp new file mode 100644 index 0000000000..6fa5620e43 --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.hpp @@ -0,0 +1,176 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, SRI International + * 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: Acorn Pooley */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace core +{ +class RobotState; +} +} // namespace moveit + +namespace robot_interaction +{ +namespace InteractionStyle +{ +/// The different types of interaction that can be constructed for an end +/// effector This is a bitmask so OR together the parts you want. +enum InteractionStyle +{ + POSITION_ARROWS = 1, // arrows to change position + ORIENTATION_CIRCLES = 2, // circles to change orientation + POSITION_SPHERE = 4, // sphere: drag to change position + ORIENTATION_SPHERE = 8, // sphere: drag to change orientation + POSITION_EEF = 16, // drag end effector to change position + ORIENTATION_EEF = 32, // drag end effector to change orientation + FIXED = 64, // keep arrow and circle axis fixed + + POSITION = POSITION_ARROWS | POSITION_SPHERE | POSITION_EEF, + ORIENTATION = ORIENTATION_CIRCLES | ORIENTATION_SPHERE | ORIENTATION_EEF, + SIX_DOF = POSITION | ORIENTATION, + SIX_DOF_SPHERE = POSITION_SPHERE | ORIENTATION_SPHERE, + POSITION_NOSPHERE = POSITION_ARROWS | POSITION_EEF, + ORIENTATION_NOSPHERE = ORIENTATION_CIRCLES | ORIENTATION_EEF, + SIX_DOF_NOSPHERE = POSITION_NOSPHERE | ORIENTATION_NOSPHERE +}; +} // namespace InteractionStyle + +/// Type of function for constructing markers. +/// This callback sets up the marker used for an interaction. +/// @param state the current state of the robot +/// @param marker the function should fill this in with an InteractiveMarker +/// that will be used to control the interaction. +/// @returns true if the function succeeds, false if the function was not able +/// to fill in \e marker. +typedef std::function + InteractiveMarkerConstructorFn; + +/// Type of function for processing marker feedback. +/// This callback function handles feedback for an Interaction's marker. +/// Callback should update the robot state that was passed in according to +/// the new position of the marker. +/// +/// @param state the current state of the robot +/// @param marker the function should fill this in with an InteractiveMarker +/// that will be used to control the interaction. +/// @returns false if the state was not successfully updated or the new state +/// is somehow invalid or erronious (e.g. in collision). true if +/// everything worked well. +typedef std::function + ProcessFeedbackFn; + +/// Type of function for updating marker pose for new state. +/// This callback is called when the robot state has changed. +/// Callback should calculate a new +/// pose for the marker based on the passed in robot state. +/// @param state the new state of the robot +/// @param pose the function should fill this in with the new pose of the +/// marker, given the new state of the robot. +/// @returns true if the pose was modified, false if no update is needed (i.e. +/// if the pose did not change). +typedef std::function InteractiveMarkerUpdateFn; + +/// Representation of a generic interaction. +/// Displays one interactive marker. +struct GenericInteraction +{ + // Callback to construct interactive marker. + // See comment on typedef above. + InteractiveMarkerConstructorFn construct_marker; + + // Callback to handle interactive marker feedback messages. + // See comment on typedef above. + ProcessFeedbackFn process_feedback; + + // Callback to update marker pose when RobotState changes. + // See comment on typedef above. + InteractiveMarkerUpdateFn update_pose; + + // Suffix added to name of markers. + // Automatically generated. + std::string marker_name_suffix; +}; + +/// Representation of an interaction via an end-effector +/// Displays one marker for manipulating the EEF position. +struct EndEffectorInteraction +{ + /// The name of the group that sustains the end-effector (usually an arm) + std::string parent_group; + + /// The name of the link in the parent group that connects to the end-effector + std::string parent_link; + + /// The name of the group that defines the group joints + std::string eef_group; + + /// Which degrees of freedom to enable for the end-effector + InteractionStyle::InteractionStyle interaction; + + /// The size of the end effector group (diameter of enclosing sphere) + double size; +}; + +/// Representation of a joint interaction. +/// Displays one marker for manipulating the joint. +struct JointInteraction +{ + /// The link in the robot model this joint is a parent of + std::string connecting_link; + + /// The name of the frame that is a parent of this joint + std::string parent_frame; + + /// The name of the joint + std::string joint_name; + + /// The type of joint disguised as the number of DOF it has. 3=PLANAR in X/Y; 6=FLOATING + unsigned int dof; + + /// The size of the connecting link (diameter of enclosing sphere) + double size; +}; +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h index 66c5f1520b..22b8c872b4 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,279 +47,5 @@ /* Author: Ioan Sucan, Adam Leeper */ #pragma once - -#include -#include -#include -//#include -#include -#include -#include - -namespace robot_interaction -{ -MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc - -struct EndEffectorInteraction; -struct JointInteraction; -struct GenericInteraction; - -/// Function type for notifying client of RobotState changes. -/// -/// This callback function is called by the InteractionHandler::handle* -/// functions, when changes are made to the internal robot state the handler -/// maintains. The handler passes its own pointer as argument to the callback, -/// as well as a boolean flag that indicates whether the error state changed -- -/// whether updates to the robot state performed in the -/// InteractionHandler::handle* functions have switched from failing to -/// succeeding or the other way around. -typedef std::function InteractionHandlerCallbackFn; - -/// Manage interactive markers to control a RobotState. -/// -/// Each instance maintains one or more interactive markers to control -/// various joints in one group of one RobotState. -/// The group being controlled is maintained by the RobotInteraction object -/// that contains this InteractionHandler object. -/// All InteractionHandler objects in the same RobotInteraction are controlling -/// the same group. -class InteractionHandler : public LockedRobotState -{ -public: - // Use this constructor if you have an initial RobotState already. - InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, - const moveit::core::RobotState& initial_robot_state, - const std::shared_ptr& tf_buffer = std::shared_ptr()); - - // Use this constructor to start with a default state. - InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, - const std::shared_ptr& tf_buffer = std::shared_ptr()); - - // DEPRECATED. - InteractionHandler(const std::string& name, const moveit::core::RobotState& initial_robot_state, - const std::shared_ptr& tf_buffer = std::shared_ptr()); - // DEPRECATED. - InteractionHandler(const std::string& name, const moveit::core::RobotModelConstPtr& model, - const std::shared_ptr& tf_buffer = std::shared_ptr()); - - ~InteractionHandler() override - { - } - - const std::string& getName() const - { - return name_; - } - - /// Get a copy of the RobotState maintained by this InteractionHandler. - using LockedRobotState::getState; - - /// Set a new RobotState for this InteractionHandler. - using LockedRobotState::setState; - - void setUpdateCallback(const InteractionHandlerCallbackFn& callback); - const InteractionHandlerCallbackFn& getUpdateCallback() const; - void setMeshesVisible(bool visible); - bool getMeshesVisible() const; - void setControlsVisible(bool visible); - bool getControlsVisible() const; - - /** \brief Set the offset from EEF to its marker. - * @param eef The target end-effector. - * @param m The pose of the marker in the frame of the end-effector parent. */ - void setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::msg::Pose& m); - - /** \brief Set the offset from a joint to its marker. - * @param j The target joint. - * @param m The pose of the marker in the frame of the joint parent. */ - void setPoseOffset(const JointInteraction& j, const geometry_msgs::msg::Pose& m); - - /** \brief Get the offset from EEF to its marker. - * @param The target end-effector. - * @param The pose offset (only valid if return value is true). - * @return True if an offset was found for the given end-effector. */ - bool getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::msg::Pose& m); - - /** \brief Get the offset from a joint to its marker. - * @param vj The joint. - * @param m The pose offset (only valid if return value is true). - * @return True if an offset was found for the given joint. */ - bool getPoseOffset(const JointInteraction& vj, geometry_msgs::msg::Pose& m); - - /** \brief Clear the interactive marker pose offset for the given - * end-effector. - * @param The target end-effector. */ - void clearPoseOffset(const EndEffectorInteraction& eef); - - /** \brief Clear the interactive marker pose offset for the given joint. - * @param The target joint. */ - void clearPoseOffset(const JointInteraction& vj); - - /** \brief Clear the pose offset for all end-effectors and virtual joints. */ - void clearPoseOffsets(); - - /** \brief Set the menu handler that defines menus and callbacks for all - * interactive markers drawn by this interaction handler. - * @param A menu handler. */ - void setMenuHandler(const std::shared_ptr& mh); - - /** \brief Get the menu handler that defines menus and callbacks for all - * interactive markers drawn by this interaction handler. - * @return The menu handler. */ - const std::shared_ptr& getMenuHandler(); - - /** \brief Remove the menu handler for this interaction handler. */ - void clearMenuHandler(); - - /** \brief Get the last interactive_marker command pose for an end-effector. - * @param The end-effector in question. - * @param A PoseStamped message containing the last (offset-removed) pose - * commanded for the end-effector. - * @return True if a pose for that end-effector was found, false otherwise. */ - bool getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef, geometry_msgs::msg::PoseStamped& pose); - - /** \brief Get the last interactive_marker command pose for a joint. - * @param The joint in question. - * @param A PoseStamped message containing the last (offset-removed) pose - * commanded for the joint. - * @return True if a pose for that joint was found, false otherwise. */ - bool getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::msg::PoseStamped& pose); - - /** \brief Clear the last interactive_marker command pose for the given - * end-effector. - * @param The target end-effector. */ - void clearLastEndEffectorMarkerPose(const EndEffectorInteraction& eef); - - /** \brief Clear the last interactive_marker command pose for the given joint. - * @param The target joint. */ - void clearLastJointMarkerPose(const JointInteraction& vj); - - /** \brief Clear the last interactive_marker command poses for all - * end-effectors and joints. */ - void clearLastMarkerPoses(); - - /** \brief Update the internal state maintained by the handler using - * information from the received feedback message. */ - virtual void handleEndEffector(const EndEffectorInteraction& eef, - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); - - /** \brief Update the internal state maintained by the handler using - * information from the received feedback message. */ - virtual void handleJoint(const JointInteraction& vj, - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); - - /** \brief Update the internal state maintained by the handler using - * information from the received feedback message. */ - virtual void handleGeneric(const GenericInteraction& g, - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); - - /** \brief Check if the marker corresponding to this end-effector leads to an - * invalid state */ - virtual bool inError(const EndEffectorInteraction& eef) const; - - /** \brief Check if the marker corresponding to this joint leads to an - * invalid state */ - virtual bool inError(const JointInteraction& vj) const; - - /** \brief Check if the generic marker to an invalid state */ - virtual bool inError(const GenericInteraction& g) const; - - /** \brief Clear any error settings. - * This makes the markers appear as if the state is no longer invalid. */ - void clearError(); - -protected: - bool transformFeedbackPose(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback, - const geometry_msgs::msg::Pose& offset, geometry_msgs::msg::PoseStamped& tpose); - - const std::string name_; - const std::string planning_frame_; - std::shared_ptr tf_buffer_; - -private: - typedef std::function StateChangeCallbackFn; - - // Update RobotState using a generic interaction feedback message. - // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateGeneric(moveit::core::RobotState& state, const GenericInteraction& g, - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback, - StateChangeCallbackFn& callback); - - // Update RobotState for a new pose of an eef. - // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateEndEffector(moveit::core::RobotState& state, const EndEffectorInteraction& eef, - const geometry_msgs::msg::Pose& pose, StateChangeCallbackFn& callback); - - // Update RobotState for a new joint position. - // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - void updateStateJoint(moveit::core::RobotState& state, const JointInteraction& vj, - const geometry_msgs::msg::Pose& pose, StateChangeCallbackFn& callback); - - // Set the error state for \e name. - // Returns true if the error state for \e name changed. - // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. - bool setErrorState(const std::string& name, bool new_error_state); - - /** Get the error state for \e name. - * True if Interaction \e name is not in a valid pose. */ - bool getErrorState(const std::string& name) const; - - // Contains the (user-programmable) pose offset between the end-effector - // parent link (or a virtual joint) and the desired control frame for the - // interactive marker. The offset is expressed in the frame of the parent - // link or virtual joint. For example, on a PR2 an offset of +0.20 along - // the x-axis will move the center of the 6-DOF interactive marker from - // the wrist to the finger tips. - // PROTECTED BY offset_map_lock_ - std::map offset_map_; - - // Contains the most recent poses received from interactive marker feedback, - // with the offset removed (e.g. in theory, coinciding with the end-effector - // parent or virtual joint). This allows a user application to query for the - // interactive marker pose (which could be useful for robot control using - // gradient-based methods) even when the IK solver failed to find a valid - // robot state that satisfies the feedback pose. - // PROTECTED BY pose_map_lock_ - std::map pose_map_; - - std::mutex pose_map_lock_; - std::mutex offset_map_lock_; - - // per group options for doing kinematics. - // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The - // CONTENTS is protected internally. - KinematicOptionsMapPtr kinematic_options_map_; - - // A set of Interactions for which the pose is invalid. - // PROTECTED BY state_lock_ - std::set error_state_; - - // For adding menus (and associated callbacks) to all the - // end-effector and virtual-joint interactive markers - // - // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The - // CONTENTS is not. - std::shared_ptr menu_handler_; - - // Called when the RobotState maintained by the handler changes. - // The caller may, for example, redraw the robot at the new state. - // handler is the handler that changed. - // error_state_changed is true if an end effector's error state may have - // changed. - // - // PROTECTED BY state_lock_ - the function pointer is protected, but the call - // is made without any lock held. - std::function update_callback_; - - // PROTECTED BY state_lock_ - bool display_meshes_; - - // PROTECTED BY state_lock_ - bool display_controls_; - - // remove '_' characters from name - static std::string fixName(std::string name); -}; -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp new file mode 100644 index 0000000000..1221ffb3d4 --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp @@ -0,0 +1,313 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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, Adam Leeper */ + +#pragma once + +#include +#include +#include +//#include +#include +#include +#include + +namespace robot_interaction +{ +MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc + +struct EndEffectorInteraction; +struct JointInteraction; +struct GenericInteraction; + +/// Function type for notifying client of RobotState changes. +/// +/// This callback function is called by the InteractionHandler::handle* +/// functions, when changes are made to the internal robot state the handler +/// maintains. The handler passes its own pointer as argument to the callback, +/// as well as a boolean flag that indicates whether the error state changed -- +/// whether updates to the robot state performed in the +/// InteractionHandler::handle* functions have switched from failing to +/// succeeding or the other way around. +typedef std::function InteractionHandlerCallbackFn; + +/// Manage interactive markers to control a RobotState. +/// +/// Each instance maintains one or more interactive markers to control +/// various joints in one group of one RobotState. +/// The group being controlled is maintained by the RobotInteraction object +/// that contains this InteractionHandler object. +/// All InteractionHandler objects in the same RobotInteraction are controlling +/// the same group. +class InteractionHandler : public LockedRobotState +{ +public: + // Use this constructor if you have an initial RobotState already. + InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, + const moveit::core::RobotState& initial_robot_state, + const std::shared_ptr& tf_buffer = std::shared_ptr()); + + // Use this constructor to start with a default state. + InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name, + const std::shared_ptr& tf_buffer = std::shared_ptr()); + + // DEPRECATED. + InteractionHandler(const std::string& name, const moveit::core::RobotState& initial_robot_state, + const std::shared_ptr& tf_buffer = std::shared_ptr()); + // DEPRECATED. + InteractionHandler(const std::string& name, const moveit::core::RobotModelConstPtr& model, + const std::shared_ptr& tf_buffer = std::shared_ptr()); + + ~InteractionHandler() override + { + } + + const std::string& getName() const + { + return name_; + } + + /// Get a copy of the RobotState maintained by this InteractionHandler. + using LockedRobotState::getState; + + /// Set a new RobotState for this InteractionHandler. + using LockedRobotState::setState; + + void setUpdateCallback(const InteractionHandlerCallbackFn& callback); + const InteractionHandlerCallbackFn& getUpdateCallback() const; + void setMeshesVisible(bool visible); + bool getMeshesVisible() const; + void setControlsVisible(bool visible); + bool getControlsVisible() const; + + /** \brief Set the offset from EEF to its marker. + * @param eef The target end-effector. + * @param m The pose of the marker in the frame of the end-effector parent. */ + void setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::msg::Pose& m); + + /** \brief Set the offset from a joint to its marker. + * @param j The target joint. + * @param m The pose of the marker in the frame of the joint parent. */ + void setPoseOffset(const JointInteraction& j, const geometry_msgs::msg::Pose& m); + + /** \brief Get the offset from EEF to its marker. + * @param The target end-effector. + * @param The pose offset (only valid if return value is true). + * @return True if an offset was found for the given end-effector. */ + bool getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::msg::Pose& m); + + /** \brief Get the offset from a joint to its marker. + * @param vj The joint. + * @param m The pose offset (only valid if return value is true). + * @return True if an offset was found for the given joint. */ + bool getPoseOffset(const JointInteraction& vj, geometry_msgs::msg::Pose& m); + + /** \brief Clear the interactive marker pose offset for the given + * end-effector. + * @param The target end-effector. */ + void clearPoseOffset(const EndEffectorInteraction& eef); + + /** \brief Clear the interactive marker pose offset for the given joint. + * @param The target joint. */ + void clearPoseOffset(const JointInteraction& vj); + + /** \brief Clear the pose offset for all end-effectors and virtual joints. */ + void clearPoseOffsets(); + + /** \brief Set the menu handler that defines menus and callbacks for all + * interactive markers drawn by this interaction handler. + * @param A menu handler. */ + void setMenuHandler(const std::shared_ptr& mh); + + /** \brief Get the menu handler that defines menus and callbacks for all + * interactive markers drawn by this interaction handler. + * @return The menu handler. */ + const std::shared_ptr& getMenuHandler(); + + /** \brief Remove the menu handler for this interaction handler. */ + void clearMenuHandler(); + + /** \brief Get the last interactive_marker command pose for an end-effector. + * @param The end-effector in question. + * @param A PoseStamped message containing the last (offset-removed) pose + * commanded for the end-effector. + * @return True if a pose for that end-effector was found, false otherwise. */ + bool getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef, geometry_msgs::msg::PoseStamped& pose); + + /** \brief Get the last interactive_marker command pose for a joint. + * @param The joint in question. + * @param A PoseStamped message containing the last (offset-removed) pose + * commanded for the joint. + * @return True if a pose for that joint was found, false otherwise. */ + bool getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::msg::PoseStamped& pose); + + /** \brief Clear the last interactive_marker command pose for the given + * end-effector. + * @param The target end-effector. */ + void clearLastEndEffectorMarkerPose(const EndEffectorInteraction& eef); + + /** \brief Clear the last interactive_marker command pose for the given joint. + * @param The target joint. */ + void clearLastJointMarkerPose(const JointInteraction& vj); + + /** \brief Clear the last interactive_marker command poses for all + * end-effectors and joints. */ + void clearLastMarkerPoses(); + + /** \brief Update the internal state maintained by the handler using + * information from the received feedback message. */ + virtual void handleEndEffector(const EndEffectorInteraction& eef, + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + + /** \brief Update the internal state maintained by the handler using + * information from the received feedback message. */ + virtual void handleJoint(const JointInteraction& vj, + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + + /** \brief Update the internal state maintained by the handler using + * information from the received feedback message. */ + virtual void handleGeneric(const GenericInteraction& g, + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + + /** \brief Check if the marker corresponding to this end-effector leads to an + * invalid state */ + virtual bool inError(const EndEffectorInteraction& eef) const; + + /** \brief Check if the marker corresponding to this joint leads to an + * invalid state */ + virtual bool inError(const JointInteraction& vj) const; + + /** \brief Check if the generic marker to an invalid state */ + virtual bool inError(const GenericInteraction& g) const; + + /** \brief Clear any error settings. + * This makes the markers appear as if the state is no longer invalid. */ + void clearError(); + +protected: + bool transformFeedbackPose(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback, + const geometry_msgs::msg::Pose& offset, geometry_msgs::msg::PoseStamped& tpose); + + const std::string name_; + const std::string planning_frame_; + std::shared_ptr tf_buffer_; + +private: + typedef std::function StateChangeCallbackFn; + + // Update RobotState using a generic interaction feedback message. + // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. + void updateStateGeneric(moveit::core::RobotState& state, const GenericInteraction& g, + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback, + StateChangeCallbackFn& callback); + + // Update RobotState for a new pose of an eef. + // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. + void updateStateEndEffector(moveit::core::RobotState& state, const EndEffectorInteraction& eef, + const geometry_msgs::msg::Pose& pose, StateChangeCallbackFn& callback); + + // Update RobotState for a new joint position. + // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. + void updateStateJoint(moveit::core::RobotState& state, const JointInteraction& vj, + const geometry_msgs::msg::Pose& pose, StateChangeCallbackFn& callback); + + // Set the error state for \e name. + // Returns true if the error state for \e name changed. + // YOU MUST LOCK state_lock_ BEFORE CALLING THIS. + bool setErrorState(const std::string& name, bool new_error_state); + + /** Get the error state for \e name. + * True if Interaction \e name is not in a valid pose. */ + bool getErrorState(const std::string& name) const; + + // Contains the (user-programmable) pose offset between the end-effector + // parent link (or a virtual joint) and the desired control frame for the + // interactive marker. The offset is expressed in the frame of the parent + // link or virtual joint. For example, on a PR2 an offset of +0.20 along + // the x-axis will move the center of the 6-DOF interactive marker from + // the wrist to the finger tips. + // PROTECTED BY offset_map_lock_ + std::map offset_map_; + + // Contains the most recent poses received from interactive marker feedback, + // with the offset removed (e.g. in theory, coinciding with the end-effector + // parent or virtual joint). This allows a user application to query for the + // interactive marker pose (which could be useful for robot control using + // gradient-based methods) even when the IK solver failed to find a valid + // robot state that satisfies the feedback pose. + // PROTECTED BY pose_map_lock_ + std::map pose_map_; + + std::mutex pose_map_lock_; + std::mutex offset_map_lock_; + + // per group options for doing kinematics. + // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The + // CONTENTS is protected internally. + KinematicOptionsMapPtr kinematic_options_map_; + + // A set of Interactions for which the pose is invalid. + // PROTECTED BY state_lock_ + std::set error_state_; + + // For adding menus (and associated callbacks) to all the + // end-effector and virtual-joint interactive markers + // + // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_. The + // CONTENTS is not. + std::shared_ptr menu_handler_; + + // Called when the RobotState maintained by the handler changes. + // The caller may, for example, redraw the robot at the new state. + // handler is the handler that changed. + // error_state_changed is true if an end effector's error state may have + // changed. + // + // PROTECTED BY state_lock_ - the function pointer is protected, but the call + // is made without any lock held. + std::function update_callback_; + + // PROTECTED BY state_lock_ + bool display_meshes_; + + // PROTECTED BY state_lock_ + bool display_controls_; + + // remove '_' characters from name + static std::string fixName(std::string name); +}; +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h index fc0c48b33b..1378d220c2 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,36 +47,5 @@ /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */ #pragma once - -#include -#include -#include - -namespace robot_interaction -{ -visualization_msgs::msg::InteractiveMarker -makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& stamped, double scale); - -visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string& name, - const geometry_msgs::msg::PoseStamped& stamped, double scale, - bool orientation_fixed = false); - -visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string& name, - const geometry_msgs::msg::PoseStamped& stamped, - double scale, bool orientation_fixed = false); - -void addTArrowMarker(visualization_msgs::msg::InteractiveMarker& im); - -void addErrorMarker(visualization_msgs::msg::InteractiveMarker& im); - -void add6DOFControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); - -void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); - -void addOrientationControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); - -void addPositionControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); - -void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker& int_marker, double radius, - const std_msgs::msg::ColorRGBA& color, bool position = true, bool orientation = true); -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.hpp new file mode 100644 index 0000000000..fc0c48b33b --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interactive_marker_helpers.hpp @@ -0,0 +1,70 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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, Acorn Pooley, Adam Leeper */ + +#pragma once + +#include +#include +#include + +namespace robot_interaction +{ +visualization_msgs::msg::InteractiveMarker +makeEmptyInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& stamped, double scale); + +visualization_msgs::msg::InteractiveMarker make6DOFMarker(const std::string& name, + const geometry_msgs::msg::PoseStamped& stamped, double scale, + bool orientation_fixed = false); + +visualization_msgs::msg::InteractiveMarker makePlanarXYMarker(const std::string& name, + const geometry_msgs::msg::PoseStamped& stamped, + double scale, bool orientation_fixed = false); + +void addTArrowMarker(visualization_msgs::msg::InteractiveMarker& im); + +void addErrorMarker(visualization_msgs::msg::InteractiveMarker& im); + +void add6DOFControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); + +void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); + +void addOrientationControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); + +void addPositionControl(visualization_msgs::msg::InteractiveMarker& int_marker, bool orientation_fixed = false); + +void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker& int_marker, double radius, + const std_msgs::msg::ColorRGBA& color, bool position = true, bool orientation = true); +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h index 4fc0b8650c..f73d5d10b5 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,56 +47,5 @@ /* Author: Acorn Pooley */ #pragma once - -#include -#include - -namespace robot_interaction -{ -// Options for inverse kinematics calculations. -// -// This is intended to be lightweight and passable by value. No virtual -// functions and no destructor. -struct KinematicOptions -{ - /// Constructor - set all options to reasonable default values - KinematicOptions(); - - /// Bits corresponding to each member. - /// NOTE: when adding fields to this structure also add the field to this - /// enum and to the setOptions() method. - enum OptionBitmask - { - TIMEOUT = 0x00000001, // timeout_seconds_ - STATE_VALIDITY_CALLBACK = 0x00000002, // state_validity_callback_ - LOCK_REDUNDANT_JOINTS = 0x00000004, // options_.lock_redundant_joints - RETURN_APPROXIMATE_SOLUTION = 0x00000008, // options_.return_approximate_solution - DISCRETIZATION_METHOD = 0x00000010, - ALL_QUERY_OPTIONS = LOCK_REDUNDANT_JOINTS | RETURN_APPROXIMATE_SOLUTION | DISCRETIZATION_METHOD, - ALL = 0x7fffffff - }; - - /// Set \e state using inverse kinematics - /// @param state the state to set - /// @param group name of group whose joints can move - /// @param tip link that will be posed - /// @param pose desired pose of tip link - /// @param result true if IK succeeded. - bool setStateFromIK(moveit::core::RobotState& state, const std::string& group, const std::string& tip, - const geometry_msgs::msg::Pose& pose) const; - - /// Copy a subset of source to this. - /// For each bit set in fields the corresponding member is copied from - /// source to this. - void setOptions(const KinematicOptions& source, OptionBitmask fields = ALL); - - /// max time an IK attempt can take before we give up. - double timeout_seconds_; - - /// This is called to determine if the state is valid - moveit::core::GroupStateValidityCallbackFn state_validity_callback_; - - /// other options - kinematics::KinematicsQueryOptions options_; -}; -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.hpp new file mode 100644 index 0000000000..5a63ea5d92 --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, SRI International + * 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: Acorn Pooley */ + +#pragma once + +#include +#include + +namespace robot_interaction +{ +// Options for inverse kinematics calculations. +// +// This is intended to be lightweight and passable by value. No virtual +// functions and no destructor. +struct KinematicOptions +{ + /// Constructor - set all options to reasonable default values + KinematicOptions(); + + /// Bits corresponding to each member. + /// NOTE: when adding fields to this structure also add the field to this + /// enum and to the setOptions() method. + enum OptionBitmask + { + TIMEOUT = 0x00000001, // timeout_seconds_ + STATE_VALIDITY_CALLBACK = 0x00000002, // state_validity_callback_ + LOCK_REDUNDANT_JOINTS = 0x00000004, // options_.lock_redundant_joints + RETURN_APPROXIMATE_SOLUTION = 0x00000008, // options_.return_approximate_solution + DISCRETIZATION_METHOD = 0x00000010, + ALL_QUERY_OPTIONS = LOCK_REDUNDANT_JOINTS | RETURN_APPROXIMATE_SOLUTION | DISCRETIZATION_METHOD, + ALL = 0x7fffffff + }; + + /// Set \e state using inverse kinematics + /// @param state the state to set + /// @param group name of group whose joints can move + /// @param tip link that will be posed + /// @param pose desired pose of tip link + /// @param result true if IK succeeded. + bool setStateFromIK(moveit::core::RobotState& state, const std::string& group, const std::string& tip, + const geometry_msgs::msg::Pose& pose) const; + + /// Copy a subset of source to this. + /// For each bit set in fields the corresponding member is copied from + /// source to this. + void setOptions(const KinematicOptions& source, OptionBitmask fields = ALL); + + /// max time an IK attempt can take before we give up. + double timeout_seconds_; + + /// This is called to determine if the state is valid + moveit::core::GroupStateValidityCallbackFn state_validity_callback_; + + /// other options + kinematics::KinematicsQueryOptions options_; +}; +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h index 154479ff2b..dc8fca95ca 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,73 +47,5 @@ /* Author: Acorn Pooley */ #pragma once - -#include -#include -#include - -#include - -namespace robot_interaction -{ -// Maintains a set of KinematicOptions with a key/value mapping and a default -// value. -class MOVEIT_ROBOT_INTERACTION_EXPORT KinematicOptionsMap -{ -public: - /// Constructor - set all options to reasonable default values. - KinematicOptionsMap(); - - /// When used as \e key this means the default value - static const std::string DEFAULT; - - /// When used as \e key this means set ALL keys (including default) - static const std::string ALL; - - /// Set \e state using inverse kinematics. - /// @param state the state to set - /// @param key used to lookup the options to use - /// @param group name of group whose joints can move - /// @param tip link that will be posed - /// @param pose desired pose of tip link - /// @param result true if IK succeeded. - bool setStateFromIK(moveit::core::RobotState& state, const std::string& key, const std::string& group, - const std::string& tip, const geometry_msgs::msg::Pose& pose) const; - - /// Get the options to use for a particular key. - /// To get the default values pass key = KinematicOptionsMap::DEFAULT - KinematicOptions getOptions(const std::string& key) const; - - /// Set some of the options to be used for a particular key. - /// - /// @param key set the options for this key. - /// To set the default options use key = KinematicOptionsMap::DEFAULT - /// To set ALL options use key = KinematicOptionsMap::ALL - /// - /// @param options the new value for the options. - /// - /// @fields which options to set for the key. - void setOptions(const std::string& key, const KinematicOptions& options, - KinematicOptions::OptionBitmask fields = KinematicOptions::ALL); - - /// Merge all options from \e other into \e this. - /// Values in \e other (including defaults_) take precedence over values in \e - /// this. - void merge(const KinematicOptionsMap& other); - -private: - // this protects all members. - mutable std::mutex lock_; - - // default kinematic options. - // PROTECTED BY lock_ - KinematicOptions defaults_; - - typedef std::map M_options; - - // per key kinematic options. - // If key is not here, defaults are used. - // PROTECTED BY lock_ - M_options options_; -}; -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.hpp new file mode 100644 index 0000000000..4c167bb00a --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/kinematic_options_map.hpp @@ -0,0 +1,107 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, SRI International + * 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: Acorn Pooley */ + +#pragma once + +#include +#include +#include + +#include + +namespace robot_interaction +{ +// Maintains a set of KinematicOptions with a key/value mapping and a default +// value. +class MOVEIT_ROBOT_INTERACTION_EXPORT KinematicOptionsMap +{ +public: + /// Constructor - set all options to reasonable default values. + KinematicOptionsMap(); + + /// When used as \e key this means the default value + static const std::string DEFAULT; + + /// When used as \e key this means set ALL keys (including default) + static const std::string ALL; + + /// Set \e state using inverse kinematics. + /// @param state the state to set + /// @param key used to lookup the options to use + /// @param group name of group whose joints can move + /// @param tip link that will be posed + /// @param pose desired pose of tip link + /// @param result true if IK succeeded. + bool setStateFromIK(moveit::core::RobotState& state, const std::string& key, const std::string& group, + const std::string& tip, const geometry_msgs::msg::Pose& pose) const; + + /// Get the options to use for a particular key. + /// To get the default values pass key = KinematicOptionsMap::DEFAULT + KinematicOptions getOptions(const std::string& key) const; + + /// Set some of the options to be used for a particular key. + /// + /// @param key set the options for this key. + /// To set the default options use key = KinematicOptionsMap::DEFAULT + /// To set ALL options use key = KinematicOptionsMap::ALL + /// + /// @param options the new value for the options. + /// + /// @fields which options to set for the key. + void setOptions(const std::string& key, const KinematicOptions& options, + KinematicOptions::OptionBitmask fields = KinematicOptions::ALL); + + /// Merge all options from \e other into \e this. + /// Values in \e other (including defaults_) take precedence over values in \e + /// this. + void merge(const KinematicOptionsMap& other); + +private: + // this protects all members. + mutable std::mutex lock_; + + // default kinematic options. + // PROTECTED BY lock_ + KinematicOptions defaults_; + + typedef std::map M_options; + + // per key kinematic options. + // If key is not here, defaults are used. + // PROTECTED BY lock_ + M_options options_; +}; +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h index 2e00e47e08..18d145cc33 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -36,76 +48,5 @@ /* Author: Ioan Sucan, Acorn Pooley */ #pragma once - -#include -#include - -#include -#include - -namespace robot_interaction -{ -MOVEIT_CLASS_FORWARD(LockedRobotState); // Defines LockedRobotStatePtr, ConstPtr, WeakPtr... etc - -/// Maintain a RobotState in a multithreaded environment. -// -// Only allow one thread to modify the RobotState at a time. -// -// Allow any thread access to the RobotState when it is not being modified. If -// a (const) reference to the robot state is held when the RobotState needs to -// be modified, a copy is made and the copy is modified. Any externally held -// references will be out of date but still valid. -// -// The RobotState can only be modified by passing a callback function which -// does the modification. -class LockedRobotState -{ -public: - LockedRobotState(const moveit::core::RobotState& state); - LockedRobotState(const moveit::core::RobotModelPtr& model); - - virtual ~LockedRobotState(); - - /// get read-only access to the state. - // - // This state may go stale, meaning the maintained state has been updated, - // but it will never be modified while the caller is holding a reference to - // it. - // - // The transforms in the returned state will always be up to date. - moveit::core::RobotStateConstPtr getState() const; - - /// Set the state to the new value. - void setState(const moveit::core::RobotState& state); - - // This is a function that can modify the maintained state. - typedef std::function ModifyStateFunction; - - // Modify the state. - // - // This modifies the state by calling \e modify on it. - // The \e modify function is passed a reference to the state which it can - // modify. No threads will be given access to the state while the \e modify - // function is running. - void modifyState(const ModifyStateFunction& modify); - -protected: - // This is called when the internally maintained state has changed. - // This is called with state_lock_ unlocked. - // Default definition does nothing. Override to get notification of state - // change. - // TODO: is this needed? - virtual void robotStateChanged(); - -protected: - // this locks all accesses to the state_ member. - // The lock can also be used by subclasses to lock additional fields. - mutable std::mutex state_lock_; - -private: - // The state maintained by this class. - // When a modify function is being called this is nullptr. - // PROTECTED BY state_lock_ - moveit::core::RobotStatePtr state_; -}; -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.hpp new file mode 100644 index 0000000000..bc5aa30f54 --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/locked_robot_state.hpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2014, SRI International + * 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, Acorn Pooley */ + +#pragma once + +#include +#include + +#include +#include + +namespace robot_interaction +{ +MOVEIT_CLASS_FORWARD(LockedRobotState); // Defines LockedRobotStatePtr, ConstPtr, WeakPtr... etc + +/// Maintain a RobotState in a multithreaded environment. +// +// Only allow one thread to modify the RobotState at a time. +// +// Allow any thread access to the RobotState when it is not being modified. If +// a (const) reference to the robot state is held when the RobotState needs to +// be modified, a copy is made and the copy is modified. Any externally held +// references will be out of date but still valid. +// +// The RobotState can only be modified by passing a callback function which +// does the modification. +class LockedRobotState +{ +public: + LockedRobotState(const moveit::core::RobotState& state); + LockedRobotState(const moveit::core::RobotModelPtr& model); + + virtual ~LockedRobotState(); + + /// get read-only access to the state. + // + // This state may go stale, meaning the maintained state has been updated, + // but it will never be modified while the caller is holding a reference to + // it. + // + // The transforms in the returned state will always be up to date. + moveit::core::RobotStateConstPtr getState() const; + + /// Set the state to the new value. + void setState(const moveit::core::RobotState& state); + + // This is a function that can modify the maintained state. + typedef std::function ModifyStateFunction; + + // Modify the state. + // + // This modifies the state by calling \e modify on it. + // The \e modify function is passed a reference to the state which it can + // modify. No threads will be given access to the state while the \e modify + // function is running. + void modifyState(const ModifyStateFunction& modify); + +protected: + // This is called when the internally maintained state has changed. + // This is called with state_lock_ unlocked. + // Default definition does nothing. Override to get notification of state + // change. + // TODO: is this needed? + virtual void robotStateChanged(); + +protected: + // this locks all accesses to the state_ member. + // The lock can also be used by subclasses to lock additional fields. + mutable std::mutex state_lock_; + +private: + // The state maintained by this class. + // When a modify function is being called this is nullptr. + // PROTECTED BY state_lock_ + moveit::core::RobotStatePtr state_; +}; +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h index 31bf726f21..9171ad6017 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,207 +47,5 @@ /* Author: Ioan Sucan, Adam Leeper */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// This is needed for legacy code that includes robot_interaction.h but not -// interaction_handler.h -#include - -namespace interactive_markers -{ -class InteractiveMarkerServer; -} - -namespace robot_interaction -{ -MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc - -// Manage interactive markers for controlling a robot state. -// -// The RobotInteraction class manages one or more InteractionHandler objects -// each of which maintains a set of interactive markers for manipulating one -// group of one RobotState. -// -// The group being manipulated is common to all InteractionHandler objects -// contained in a RobotInteraction instance. -class RobotInteraction -{ -public: - /// The topic name on which the internal Interactive Marker Server operates - static const std::string INTERACTIVE_MARKER_TOPIC; - - RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, - const std::string& ns = ""); - virtual ~RobotInteraction(); - - const std::string& getServerTopic() const - { - return topic_; - } - - /// add an interaction. - /// An interaction is a marker that can be used to manipulate the robot - /// state. - /// This function does not add any markers. To add markers for all - /// active interactions call addInteractiveMarkers(). - /// construct - a callback to construct the marker. See comment on - /// InteractiveMarkerConstructorFn above. - /// update - Called when the robot state changes. Updates the marker pose. - /// Optional. See comment on InteractiveMarkerUpdateFn above. - /// process - called when the marker moves. Updates the robot state. See - /// comment on ProcessFeedbackFn above. - void addActiveComponent(const InteractiveMarkerConstructorFn& construct, const ProcessFeedbackFn& process, - const InteractiveMarkerUpdateFn& update = InteractiveMarkerUpdateFn(), - const std::string& name = ""); - - /// Adds an interaction for: - /// - each end effector in the group that can be controller by IK - /// - each floating joint - /// - each planar joint - /// If no end effector exists in the robot then adds an interaction for - /// the last link in the chain. - /// This function does not add any markers. To add markers for all - /// active interactions call addInteractiveMarkers(). - void decideActiveComponents(const std::string& group); - void decideActiveComponents(const std::string& group, InteractionStyle::InteractionStyle style); - - /// remove all interactions. - /// Also removes all markers. - void clear(); - - /// Add interactive markers for all active interactions. - /// This adds markers just to the one handler. If there are multiple handlers - /// call this for each handler for which you want markers. - /// The markers are not actually added until you call - /// publishInteractiveMarkers(). - void addInteractiveMarkers(const InteractionHandlerPtr& handler, const double marker_scale = 0.0); - - // Update pose of all interactive markers to match the handler's RobotState. - // Call this when the handler's RobotState changes. - void updateInteractiveMarkers(const InteractionHandlerPtr& handler); - - // True if markers are being shown for this handler. - bool showingMarkers(const InteractionHandlerPtr& handler); - - // Display all markers that have been added. - // This is needed after calls to addInteractiveMarkers() to publish the - // resulting markers so they get displayed. This call is not needed after - // calling updateInteractiveMarkers() which publishes the results itself. - void publishInteractiveMarkers(); - - // Clear all interactive markers. - // This removes all interactive markers but does not affect which - // interactions are active. After this a call to publishInteractiveMarkers() - // is needed to actually remove the markers from the display. - void clearInteractiveMarkers(); - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - // Get the kinematic options map. - // Use this to set kinematic options (defaults or per-group). - KinematicOptionsMapPtr getKinematicOptionsMap() - { - return kinematic_options_map_; - } - - // enable/disable subscription of the topics to move interactive marker - void toggleMoveInteractiveMarkerTopic(bool enable); - - const std::vector& getActiveEndEffectors() const - { - return active_eef_; - } - const std::vector& getActiveJoints() const - { - return active_vj_; - } - -private: - // called by decideActiveComponents(); add markers for end effectors - void decideActiveEndEffectors(const std::string& group); - void decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style); - - // called by decideActiveComponents(); add markers for planar and floating joints - void decideActiveJoints(const std::string& group); - - void moveInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& msg); - // register the name of the topic and marker name to move interactive marker from other ROS nodes - void registerMoveInteractiveMarkerTopic(const std::string& marker_name, const std::string& name); - // return the diameter of the sphere that certainly can enclose the AABB of the link - double computeLinkMarkerSize(const std::string& link); - // return the diameter of the sphere that certainly can enclose the AABB of the links in this group - double computeGroupMarkerSize(const std::string& group); - void computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - const moveit::core::RobotState& robot_state, geometry_msgs::msg::Pose& pose, - geometry_msgs::msg::Pose& control_to_eef_tf) const; - - void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - visualization_msgs::msg::InteractiveMarker& im, bool position = true, - bool orientation = true); - void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, - const geometry_msgs::msg::Pose& offset, visualization_msgs::msg::InteractiveMarker& im, - bool position = true, bool orientation = true); - - void - processInteractiveMarkerFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); - void subscribeMoveInteractiveMarker(const std::string marker_name, const std::string& name); - void processingThread(); - void clearInteractiveMarkersUnsafe(); - - std::unique_ptr processing_thread_; - bool run_processing_thread_; - - std::condition_variable new_feedback_condition_; - std::map feedback_map_; - - moveit::core::RobotModelConstPtr robot_model_; - - std::vector active_eef_; - std::vector active_vj_; - std::vector active_generic_; - - std::map handlers_; - std::map shown_markers_; - - // This mutex is locked every time markers are read or updated; - // This includes the active_* arrays and shown_markers_ - // Please note that this mutex *MUST NOT* be locked while operations - // on the interactive marker server are called because the server - // also locks internally and we could othewrise end up with a problem - // of Thread 1: Lock A, Lock B, Unlock B, Unloack A - // Thread 2: Lock B, Lock A - // => deadlock - std::mutex marker_access_lock_; - - interactive_markers::InteractiveMarkerServer* int_marker_server_; - // ros subscribers to move the interactive markers by other ros nodes - std::vector::SharedPtr> int_marker_move_subscribers_; - // the array of the names of the topics which need to be subscribed - // to move the interactive markers by other ROS nodes - std::vector int_marker_move_topics_; - // the array of the marker names in the same order to int_marker_move_topics_ - std::vector int_marker_names_; - - std::string topic_; - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - - // options for doing IK - KinematicOptionsMapPtr kinematic_options_map_; -}; -} // namespace robot_interaction +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.hpp new file mode 100644 index 0000000000..d98503751b --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/robot_interaction.hpp @@ -0,0 +1,241 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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, Adam Leeper */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// This is needed for legacy code that includes robot_interaction.h but not +// interaction_handler.h +#include + +namespace interactive_markers +{ +class InteractiveMarkerServer; +} + +namespace robot_interaction +{ +MOVEIT_CLASS_FORWARD(InteractionHandler); // Defines InteractionHandlerPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(KinematicOptionsMap); // Defines KinematicOptionsMapPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotInteraction); // Defines RobotInteractionPtr, ConstPtr, WeakPtr... etc + +// Manage interactive markers for controlling a robot state. +// +// The RobotInteraction class manages one or more InteractionHandler objects +// each of which maintains a set of interactive markers for manipulating one +// group of one RobotState. +// +// The group being manipulated is common to all InteractionHandler objects +// contained in a RobotInteraction instance. +class RobotInteraction +{ +public: + /// The topic name on which the internal Interactive Marker Server operates + static const std::string INTERACTIVE_MARKER_TOPIC; + + RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, + const std::string& ns = ""); + virtual ~RobotInteraction(); + + const std::string& getServerTopic() const + { + return topic_; + } + + /// add an interaction. + /// An interaction is a marker that can be used to manipulate the robot + /// state. + /// This function does not add any markers. To add markers for all + /// active interactions call addInteractiveMarkers(). + /// construct - a callback to construct the marker. See comment on + /// InteractiveMarkerConstructorFn above. + /// update - Called when the robot state changes. Updates the marker pose. + /// Optional. See comment on InteractiveMarkerUpdateFn above. + /// process - called when the marker moves. Updates the robot state. See + /// comment on ProcessFeedbackFn above. + void addActiveComponent(const InteractiveMarkerConstructorFn& construct, const ProcessFeedbackFn& process, + const InteractiveMarkerUpdateFn& update = InteractiveMarkerUpdateFn(), + const std::string& name = ""); + + /// Adds an interaction for: + /// - each end effector in the group that can be controller by IK + /// - each floating joint + /// - each planar joint + /// If no end effector exists in the robot then adds an interaction for + /// the last link in the chain. + /// This function does not add any markers. To add markers for all + /// active interactions call addInteractiveMarkers(). + void decideActiveComponents(const std::string& group); + void decideActiveComponents(const std::string& group, InteractionStyle::InteractionStyle style); + + /// remove all interactions. + /// Also removes all markers. + void clear(); + + /// Add interactive markers for all active interactions. + /// This adds markers just to the one handler. If there are multiple handlers + /// call this for each handler for which you want markers. + /// The markers are not actually added until you call + /// publishInteractiveMarkers(). + void addInteractiveMarkers(const InteractionHandlerPtr& handler, const double marker_scale = 0.0); + + // Update pose of all interactive markers to match the handler's RobotState. + // Call this when the handler's RobotState changes. + void updateInteractiveMarkers(const InteractionHandlerPtr& handler); + + // True if markers are being shown for this handler. + bool showingMarkers(const InteractionHandlerPtr& handler); + + // Display all markers that have been added. + // This is needed after calls to addInteractiveMarkers() to publish the + // resulting markers so they get displayed. This call is not needed after + // calling updateInteractiveMarkers() which publishes the results itself. + void publishInteractiveMarkers(); + + // Clear all interactive markers. + // This removes all interactive markers but does not affect which + // interactions are active. After this a call to publishInteractiveMarkers() + // is needed to actually remove the markers from the display. + void clearInteractiveMarkers(); + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + // Get the kinematic options map. + // Use this to set kinematic options (defaults or per-group). + KinematicOptionsMapPtr getKinematicOptionsMap() + { + return kinematic_options_map_; + } + + // enable/disable subscription of the topics to move interactive marker + void toggleMoveInteractiveMarkerTopic(bool enable); + + const std::vector& getActiveEndEffectors() const + { + return active_eef_; + } + const std::vector& getActiveJoints() const + { + return active_vj_; + } + +private: + // called by decideActiveComponents(); add markers for end effectors + void decideActiveEndEffectors(const std::string& group); + void decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style); + + // called by decideActiveComponents(); add markers for planar and floating joints + void decideActiveJoints(const std::string& group); + + void moveInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& msg); + // register the name of the topic and marker name to move interactive marker from other ROS nodes + void registerMoveInteractiveMarkerTopic(const std::string& marker_name, const std::string& name); + // return the diameter of the sphere that certainly can enclose the AABB of the link + double computeLinkMarkerSize(const std::string& link); + // return the diameter of the sphere that certainly can enclose the AABB of the links in this group + double computeGroupMarkerSize(const std::string& group); + void computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, + const moveit::core::RobotState& robot_state, geometry_msgs::msg::Pose& pose, + geometry_msgs::msg::Pose& control_to_eef_tf) const; + + void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, + visualization_msgs::msg::InteractiveMarker& im, bool position = true, + bool orientation = true); + void addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef, + const geometry_msgs::msg::Pose& offset, visualization_msgs::msg::InteractiveMarker& im, + bool position = true, bool orientation = true); + + void + processInteractiveMarkerFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + void subscribeMoveInteractiveMarker(const std::string marker_name, const std::string& name); + void processingThread(); + void clearInteractiveMarkersUnsafe(); + + std::unique_ptr processing_thread_; + bool run_processing_thread_; + + std::condition_variable new_feedback_condition_; + std::map feedback_map_; + + moveit::core::RobotModelConstPtr robot_model_; + + std::vector active_eef_; + std::vector active_vj_; + std::vector active_generic_; + + std::map handlers_; + std::map shown_markers_; + + // This mutex is locked every time markers are read or updated; + // This includes the active_* arrays and shown_markers_ + // Please note that this mutex *MUST NOT* be locked while operations + // on the interactive marker server are called because the server + // also locks internally and we could othewrise end up with a problem + // of Thread 1: Lock A, Lock B, Unlock B, Unloack A + // Thread 2: Lock B, Lock A + // => deadlock + std::mutex marker_access_lock_; + + interactive_markers::InteractiveMarkerServer* int_marker_server_; + // ros subscribers to move the interactive markers by other ros nodes + std::vector::SharedPtr> int_marker_move_subscribers_; + // the array of the names of the topics which need to be subscribed + // to move the interactive markers by other ROS nodes + std::vector int_marker_move_topics_; + // the array of the marker names in the same order to int_marker_move_topics_ + std::vector int_marker_names_; + + std::string topic_; + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + + // options for doing IK + KinematicOptionsMapPtr kinematic_options_map_; +}; +} // namespace robot_interaction diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index a56ac384cc..8d79f91519 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -36,11 +36,11 @@ /* Author: Ioan Sucan, Adam Leeper */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp index f4c401d419..fe6fce7250 100644 --- a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp +++ b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp @@ -35,7 +35,7 @@ /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */ #include -#include +#include #include #include diff --git a/moveit_ros/robot_interaction/src/kinematic_options.cpp b/moveit_ros/robot_interaction/src/kinematic_options.cpp index de704ae6e9..2c7d1be874 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -34,7 +34,7 @@ /* Author: Acorn Pooley */ -#include +#include #include #include diff --git a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp index 30c0d23879..2cbe343710 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options_map.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options_map.cpp @@ -34,7 +34,7 @@ /* Author: Acorn Pooley */ -#include +#include // These strings have no content. They are compared by address. const std::string robot_interaction::KinematicOptionsMap::DEFAULT = ""; diff --git a/moveit_ros/robot_interaction/src/locked_robot_state.cpp b/moveit_ros/robot_interaction/src/locked_robot_state.cpp index d19402f062..15b8d0c845 100644 --- a/moveit_ros/robot_interaction/src/locked_robot_state.cpp +++ b/moveit_ros/robot_interaction/src/locked_robot_state.cpp @@ -36,7 +36,7 @@ /* Author: Ioan Sucan, Acorn Pooley */ -#include +#include robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotState& state) : state_(std::make_shared(state)) diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index d641cd040c..23d5cbacd5 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -35,11 +35,11 @@ /* Author: Ioan Sucan, Adam Leeper */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp index 0d887a6acf..7808dc3ab0 100644 --- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp +++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp @@ -36,9 +36,9 @@ /* Author: Acorn Pooley, Ioan Sucan */ #include -#include -#include -#include +#include +#include +#include #include static const char* const URDF_STR = diff --git a/moveit_ros/tests/include/parameter_name_list.h b/moveit_ros/tests/include/parameter_name_list.h new file mode 100644 index 0000000000..e6fa9950cf --- /dev/null +++ b/moveit_ros/tests/include/parameter_name_list.h @@ -0,0 +1,53 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik 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 PickNik Inc. 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: Sebastian Jahr + Description: List of all parameter names used when move_group with OMPL, CHOMP, STOMP and PILZ is launched (01/2024). +*/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/tests/include/parameter_name_list.hpp b/moveit_ros/tests/include/parameter_name_list.hpp index 90c1806fc7..8505c48e3f 100644 --- a/moveit_ros/tests/include/parameter_name_list.hpp +++ b/moveit_ros/tests/include/parameter_name_list.hpp @@ -1,4 +1,3 @@ - /********************************************************************* * Software License Agreement (BSD License) * @@ -36,6 +35,9 @@ /* Author: Sebastian Jahr Description: List of all parameter names used when move_group with OMPL, CHOMP, STOMP and PILZ is launched (01/2024). */ + +#pragma once + #include namespace move_group_test diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.h b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.h new file mode 100644 index 0000000000..d2febfa95c --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.h @@ -0,0 +1,34 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Fuzzy-Matching Trajectory Cache. + * @author methylDragon + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 2af5058840..87288cba4d 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -37,7 +37,7 @@ #include // moveit modules -#include +#include #include namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 81bceebe5c..de4939428e 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index dfe8ef7e47..425aa9cd76 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -18,8 +18,8 @@ #include -#include -#include +#include +#include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index dc4f94ac8f..711211c89f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -1,9 +1,9 @@ set(HEADERS - include/moveit/motion_planning_rviz_plugin/motion_planning_display.h - include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h - include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h - include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h - include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h) + include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp + include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp + include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.hpp + include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.hpp + include/moveit/motion_planning_rviz_plugin/interactive_marker_display.hpp) qt5_wrap_ui(UIC_FILES src/ui/motion_planning_rviz_plugin_frame.ui src/ui/motion_planning_rviz_plugin_frame_joints.ui) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h index 598b5825c5..6c5103c617 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /* * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2019, Open Source Robotics Foundation, Inc. @@ -34,125 +46,5 @@ // marker cause by Rviz having only a single thread executor #pragma once - -#include -#include -#include -#include - -#include -#include -#include - -#ifndef Q_MOC_RUN -#include -#endif - -#include - -#include - -namespace rviz_common -{ -class BoolProperty; -class Object; -} // namespace rviz_common - -namespace rviz_default_plugins -{ -class InteractiveMarkerNamespaceProperty; - -namespace displays -{ -class MarkerBase; - -/// Displays Interactive Markers -class InteractiveMarkerDisplay : public rviz_common::Display -{ - Q_OBJECT - -public: - InteractiveMarkerDisplay(); - ~InteractiveMarkerDisplay() override - { - private_executor_->cancel(); - if (private_executor_thread_.joinable()) - private_executor_thread_.join(); - private_executor_.reset(); - } - - // Overrides from Display - void update(float wall_dt, float ros_dt) override; - - void reset() override; - -protected: - // Overrides from Display - void fixedFrameChanged() override; - - void onInitialize() override; - - void onEnable() override; - - void onDisable() override; - -protected Q_SLOTS: - void namespaceChanged(); - void updateShowDescriptions(); - void updateShowAxes(); - void updateShowVisualAids(); - void updateEnableTransparency(); - void publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback& feedback); - void onStatusUpdate(rviz_common::properties::StatusProperty::Level level, const std::string& name, - const std::string& text); - -private: - /// Subscribe to all message topics. - void subscribe(); - - /// Unsubscribe from all message topics. - void unsubscribe(); - - /// Called by InteractiveMarkerClient when successfully initialized. - void initializeCallback(const visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr& /*msg*/); - - /// Called by InteractiveMarkerClient when an update from a server is received. - void updateCallback(const visualization_msgs::msg::InteractiveMarkerUpdate::ConstSharedPtr& msg); - - /// Called by InteractiveMarkerClient when it resets. - void resetCallback(); - - /// Called by InteractiveMarkerClient when there is a status message. - void statusCallback(interactive_markers::InteractiveMarkerClient::Status /*status*/, const std::string& message); - - void updateMarkers(const std::vector& markers); - - void updatePoses(const std::vector& marker_poses); - - /// Erase all visualization markers. - void eraseAllMarkers(); - - /// Erase visualization markers for an InteractionMarkerServer. - /** - * \param names The names markers to erase. - */ - void eraseMarkers(const std::vector& names); - - std::map interactive_markers_map_; - - // Properties - InteractiveMarkerNamespaceProperty* interactive_marker_namespace_property_; - rviz_common::properties::BoolProperty* show_descriptions_property_; - rviz_common::properties::BoolProperty* show_axes_property_; - rviz_common::properties::BoolProperty* show_visual_aids_property_; - rviz_common::properties::BoolProperty* enable_transparency_property_; - - std::unique_ptr interactive_marker_client_; - - std::shared_ptr pnode_; - std::shared_ptr private_executor_; - std::thread private_executor_thread_; -}; // class InteractiveMarkerDisplay - -} // namespace displays -} // namespace rviz_default_plugins +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.hpp new file mode 100644 index 0000000000..598b5825c5 --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/interactive_marker_display.hpp @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2019, Open Source Robotics Foundation, 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. + */ + +// TODO(JafarAbdi): Remove this file once the lag issue is fixed upstream https://github.com/ros2/rviz/issues/548 +// This file is copied from https://github.com/ros2/rviz, the only difference is the addition of the private members +// pnode_, private_executor_, and private_executor_thread_ to fix the lag in the motion planning display interactive +// marker cause by Rviz having only a single thread executor + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#endif + +#include + +#include + +namespace rviz_common +{ +class BoolProperty; +class Object; +} // namespace rviz_common + +namespace rviz_default_plugins +{ +class InteractiveMarkerNamespaceProperty; + +namespace displays +{ +class MarkerBase; + +/// Displays Interactive Markers +class InteractiveMarkerDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + InteractiveMarkerDisplay(); + ~InteractiveMarkerDisplay() override + { + private_executor_->cancel(); + if (private_executor_thread_.joinable()) + private_executor_thread_.join(); + private_executor_.reset(); + } + + // Overrides from Display + void update(float wall_dt, float ros_dt) override; + + void reset() override; + +protected: + // Overrides from Display + void fixedFrameChanged() override; + + void onInitialize() override; + + void onEnable() override; + + void onDisable() override; + +protected Q_SLOTS: + void namespaceChanged(); + void updateShowDescriptions(); + void updateShowAxes(); + void updateShowVisualAids(); + void updateEnableTransparency(); + void publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback& feedback); + void onStatusUpdate(rviz_common::properties::StatusProperty::Level level, const std::string& name, + const std::string& text); + +private: + /// Subscribe to all message topics. + void subscribe(); + + /// Unsubscribe from all message topics. + void unsubscribe(); + + /// Called by InteractiveMarkerClient when successfully initialized. + void initializeCallback(const visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr& /*msg*/); + + /// Called by InteractiveMarkerClient when an update from a server is received. + void updateCallback(const visualization_msgs::msg::InteractiveMarkerUpdate::ConstSharedPtr& msg); + + /// Called by InteractiveMarkerClient when it resets. + void resetCallback(); + + /// Called by InteractiveMarkerClient when there is a status message. + void statusCallback(interactive_markers::InteractiveMarkerClient::Status /*status*/, const std::string& message); + + void updateMarkers(const std::vector& markers); + + void updatePoses(const std::vector& marker_poses); + + /// Erase all visualization markers. + void eraseAllMarkers(); + + /// Erase visualization markers for an InteractionMarkerServer. + /** + * \param names The names markers to erase. + */ + void eraseMarkers(const std::vector& names); + + std::map interactive_markers_map_; + + // Properties + InteractiveMarkerNamespaceProperty* interactive_marker_namespace_property_; + rviz_common::properties::BoolProperty* show_descriptions_property_; + rviz_common::properties::BoolProperty* show_axes_property_; + rviz_common::properties::BoolProperty* show_visual_aids_property_; + rviz_common::properties::BoolProperty* enable_transparency_property_; + + std::unique_ptr interactive_marker_client_; + + std::shared_ptr pnode_; + std::shared_ptr private_executor_; + std::thread private_executor_thread_; +}; // class InteractiveMarkerDisplay + +} // namespace displays +} // namespace rviz_default_plugins diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h index 62cfcd0995..97cbded9ef 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,295 +47,5 @@ /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */ #pragma once - -#include -#include -#include -#include - -#ifndef Q_MOC_RUN -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include -#endif - -#include - -namespace Ogre -{ -class SceneNode; -} - -namespace rviz_rendering -{ -class Shape; -class MovableText; -} // namespace rviz_rendering - -namespace rviz_common -{ -namespace properties -{ -class Property; -class StringProperty; -class BoolProperty; -class FloatProperty; -class RosTopicProperty; -class EditableEnumProperty; -class ColorProperty; -class MovableText; -} // namespace properties -} // namespace rviz_common - -namespace rviz_default_plugins -{ -namespace robot -{ -class Robot; -} -} // namespace rviz_default_plugins - -namespace moveit_rviz_plugin -{ -class MotionPlanningDisplay : public PlanningSceneDisplay -{ - Q_OBJECT - -public: - MotionPlanningDisplay(); - - ~MotionPlanningDisplay() override; - - void load(const rviz_common::Config& config) override; - void save(rviz_common::Config config) const override; - - void update(float wall_dt, float ros_dt) override; - void reset() override; - - moveit::core::RobotStateConstPtr getQueryStartState() const - { - return query_start_state_->getState(); - } - - moveit::core::RobotStateConstPtr getQueryGoalState() const - { - return query_goal_state_->getState(); - } - - const moveit::core::RobotState& getPreviousState() const - { - return *previous_state_; - } - - const robot_interaction::RobotInteractionPtr& getRobotInteraction() const - { - return robot_interaction_; - } - - const robot_interaction::InteractionHandlerPtr& getQueryStartStateHandler() const - { - return query_start_state_; - } - - const robot_interaction::InteractionHandlerPtr& getQueryGoalStateHandler() const - { - return query_goal_state_; - } - - void dropVisualizedTrajectory() - { - trajectory_visual_->dropTrajectory(); - } - - void setQueryStartState(const moveit::core::RobotState& start); - void setQueryGoalState(const moveit::core::RobotState& goal); - - void updateQueryStates(const moveit::core::RobotState& current_state); - void updateQueryStartState(); - void updateQueryGoalState(); - void rememberPreviousStartState(); - - void useApproximateIK(bool flag); - - // Pick Place - void clearPlaceLocationsDisplay(); - void visualizePlaceLocations(const std::vector& place_poses); - std::vector > place_locations_display_; - - std::string getCurrentPlanningGroup() const; - - void changePlanningGroup(const std::string& group); - - void addStatusText(const std::string& text); - void addStatusText(const std::vector& text); - void setStatusTextColor(const QColor& color); - void resetStatusTextColor(); - - void toggleSelectPlanningGroupSubscription(bool enable); - -Q_SIGNALS: - // signals issued when start/goal states of a query changed - void queryStartStateChanged(); - void queryGoalStateChanged(); - -private Q_SLOTS: - - // ****************************************************************************************** - // Slot Event Functions - // ****************************************************************************************** - - void changedQueryStartState(); - void changedQueryGoalState(); - void changedQueryMarkerScale(); - void changedQueryStartColor(); - void changedQueryGoalColor(); - void changedQueryStartAlpha(); - void changedQueryGoalAlpha(); - void changedQueryCollidingLinkColor(); - void changedQueryJointViolationColor(); - void changedAttachedBodyColor() override; - void changedPlanningGroup(); - void changedShowWeightLimit(); - void changedShowManipulabilityIndex(); - void changedShowManipulability(); - void changedShowJointTorques(); - void changedMetricsSetPayload(); - void changedMetricsTextHeight(); - void changedWorkspace(); - void resetInteractiveMarkers(); - void motionPanelVisibilityChange(bool enable); - -protected: - enum LinkDisplayStatus - { - COLLISION_LINK, - OUTSIDE_BOUNDS_LINK - }; - - void clearRobotModel() override; - void onRobotModelLoaded() override; - void onNewPlanningSceneState() override; - void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; - void updateInternal(double wall_dt, double ros_dt) override; - - void renderWorkspaceBox(); - void updateLinkColors(); - - void displayTable(const std::map& values, const Ogre::ColourValue& color, - const Ogre::Vector3& pos, const Ogre::Quaternion& orient); - void displayMetrics(bool start); - - void executeMainLoopJobs(); - void publishInteractiveMarkers(bool pose_update); - - void recomputeQueryStartStateMetrics(); - void recomputeQueryGoalStateMetrics(); - void drawQueryStartState(); - void drawQueryGoalState(); - void scheduleDrawQueryStartState(robot_interaction::InteractionHandler* handler, bool error_state_changed); - void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* handler, bool error_state_changed); - - bool isIKSolutionCollisionFree(moveit::core::RobotState* state, const moveit::core::JointModelGroup* group, - const double* ik_solution) const; - - void computeMetrics(bool start, const std::string& group, double payload); - void computeMetricsInternal(std::map& metrics, - const robot_interaction::EndEffectorInteraction& eef, - const moveit::core::RobotState& state, double payload); - void updateStateExceptModified(moveit::core::RobotState& dest, const moveit::core::RobotState& src); - void updateBackgroundJobProgressBar(); - void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname); - - void setQueryStateHelper(bool use_start_state, const std::string& v); - void populateMenuHandler(std::shared_ptr& mh); - - void selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr& msg); - - // overrides from Display - void onInitialize() override; - void onEnable() override; - void onDisable() override; - void fixedFrameChanged() override; - - RobotStateVisualizationPtr query_robot_start_; ///< Handles drawing the robot at the start configuration - RobotStateVisualizationPtr query_robot_goal_; ///< Handles drawing the robot at the goal configuration - - Ogre::SceneNode* text_display_scene_node_; ///< displays texts - bool text_display_for_start_; ///< indicates whether the text display is for the start state or not - rviz_rendering::MovableText* text_to_display_; - - rclcpp::Subscription::SharedPtr planning_group_sub_; - - // render the workspace box - std::unique_ptr workspace_box_; - - // the planning frame - MotionPlanningFrame* frame_; - rviz_common::PanelDockWidget* frame_dock_; - - // robot interaction - robot_interaction::RobotInteractionPtr robot_interaction_; - robot_interaction::InteractionHandlerPtr query_start_state_; - robot_interaction::InteractionHandlerPtr query_goal_state_; - std::shared_ptr menu_handler_start_; - std::shared_ptr menu_handler_goal_; - std::map status_links_start_; - std::map status_links_goal_; - /// remember previous start state (updated before starting execution) - moveit::core::RobotStatePtr previous_state_; - - /// Hold the names of the groups for which the query states have been updated (and should not be altered when new info - /// is received from the planning scene) - std::set modified_groups_; - - /// The metrics are pairs of name-value for each of the active end effectors, for both start & goal states. - /// computed_metrics_[std::make_pair(IS_START_STATE, GROUP_NAME)] = a map of key-value pairs - std::map, std::map > computed_metrics_; - /// Some groups use position only ik, calls to the metrics have to be modified appropriately - std::map position_only_ik_; - - // Metric calculations - kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_; - std::map dynamics_solver_; - std::mutex update_metrics_lock_; - - // The trajectory playback component - TrajectoryVisualizationPtr trajectory_visual_; - - // properties to show on side panel - rviz_common::properties::Property* path_category_; - rviz_common::properties::Property* plan_category_; - rviz_common::properties::Property* metrics_category_; - - rviz_common::properties::EditableEnumProperty* planning_group_property_; - rviz_common::properties::BoolProperty* query_start_state_property_; - rviz_common::properties::BoolProperty* query_goal_state_property_; - rviz_common::properties::FloatProperty* query_marker_scale_property_; - rviz_common::properties::ColorProperty* query_start_color_property_; - rviz_common::properties::ColorProperty* query_goal_color_property_; - rviz_common::properties::FloatProperty* query_start_alpha_property_; - rviz_common::properties::FloatProperty* query_goal_alpha_property_; - rviz_common::properties::ColorProperty* query_colliding_link_color_property_; - rviz_common::properties::ColorProperty* query_outside_joint_limits_link_color_property_; - - rviz_common::properties::BoolProperty* compute_weight_limit_property_; - rviz_common::properties::BoolProperty* show_manipulability_index_property_; - rviz_common::properties::BoolProperty* show_manipulability_property_; - rviz_common::properties::BoolProperty* show_joint_torques_property_; - rviz_common::properties::FloatProperty* metrics_set_payload_property_; - rviz_common::properties::FloatProperty* metrics_text_height_property_; - rviz_common::properties::BoolProperty* show_workspace_property_; - - rviz_common::Display* int_marker_display_; -}; - -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp new file mode 100644 index 0000000000..4f5a2b9242 --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp @@ -0,0 +1,329 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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, Dave Coleman, Adam Leeper, Sachin Chitta */ + +#pragma once + +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#endif + +#include + +namespace Ogre +{ +class SceneNode; +} + +namespace rviz_rendering +{ +class Shape; +class MovableText; +} // namespace rviz_rendering + +namespace rviz_common +{ +namespace properties +{ +class Property; +class StringProperty; +class BoolProperty; +class FloatProperty; +class RosTopicProperty; +class EditableEnumProperty; +class ColorProperty; +class MovableText; +} // namespace properties +} // namespace rviz_common + +namespace rviz_default_plugins +{ +namespace robot +{ +class Robot; +} +} // namespace rviz_default_plugins + +namespace moveit_rviz_plugin +{ +class MotionPlanningDisplay : public PlanningSceneDisplay +{ + Q_OBJECT + +public: + MotionPlanningDisplay(); + + ~MotionPlanningDisplay() override; + + void load(const rviz_common::Config& config) override; + void save(rviz_common::Config config) const override; + + void update(float wall_dt, float ros_dt) override; + void reset() override; + + moveit::core::RobotStateConstPtr getQueryStartState() const + { + return query_start_state_->getState(); + } + + moveit::core::RobotStateConstPtr getQueryGoalState() const + { + return query_goal_state_->getState(); + } + + const moveit::core::RobotState& getPreviousState() const + { + return *previous_state_; + } + + const robot_interaction::RobotInteractionPtr& getRobotInteraction() const + { + return robot_interaction_; + } + + const robot_interaction::InteractionHandlerPtr& getQueryStartStateHandler() const + { + return query_start_state_; + } + + const robot_interaction::InteractionHandlerPtr& getQueryGoalStateHandler() const + { + return query_goal_state_; + } + + void dropVisualizedTrajectory() + { + trajectory_visual_->dropTrajectory(); + } + + void setQueryStartState(const moveit::core::RobotState& start); + void setQueryGoalState(const moveit::core::RobotState& goal); + + void updateQueryStates(const moveit::core::RobotState& current_state); + void updateQueryStartState(); + void updateQueryGoalState(); + void rememberPreviousStartState(); + + void useApproximateIK(bool flag); + + // Pick Place + void clearPlaceLocationsDisplay(); + void visualizePlaceLocations(const std::vector& place_poses); + std::vector > place_locations_display_; + + std::string getCurrentPlanningGroup() const; + + void changePlanningGroup(const std::string& group); + + void addStatusText(const std::string& text); + void addStatusText(const std::vector& text); + void setStatusTextColor(const QColor& color); + void resetStatusTextColor(); + + void toggleSelectPlanningGroupSubscription(bool enable); + +Q_SIGNALS: + // signals issued when start/goal states of a query changed + void queryStartStateChanged(); + void queryGoalStateChanged(); + +private Q_SLOTS: + + // ****************************************************************************************** + // Slot Event Functions + // ****************************************************************************************** + + void changedQueryStartState(); + void changedQueryGoalState(); + void changedQueryMarkerScale(); + void changedQueryStartColor(); + void changedQueryGoalColor(); + void changedQueryStartAlpha(); + void changedQueryGoalAlpha(); + void changedQueryCollidingLinkColor(); + void changedQueryJointViolationColor(); + void changedAttachedBodyColor() override; + void changedPlanningGroup(); + void changedShowWeightLimit(); + void changedShowManipulabilityIndex(); + void changedShowManipulability(); + void changedShowJointTorques(); + void changedMetricsSetPayload(); + void changedMetricsTextHeight(); + void changedWorkspace(); + void resetInteractiveMarkers(); + void motionPanelVisibilityChange(bool enable); + +protected: + enum LinkDisplayStatus + { + COLLISION_LINK, + OUTSIDE_BOUNDS_LINK + }; + + void clearRobotModel() override; + void onRobotModelLoaded() override; + void onNewPlanningSceneState() override; + void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; + void updateInternal(double wall_dt, double ros_dt) override; + + void renderWorkspaceBox(); + void updateLinkColors(); + + void displayTable(const std::map& values, const Ogre::ColourValue& color, + const Ogre::Vector3& pos, const Ogre::Quaternion& orient); + void displayMetrics(bool start); + + void executeMainLoopJobs(); + void publishInteractiveMarkers(bool pose_update); + + void recomputeQueryStartStateMetrics(); + void recomputeQueryGoalStateMetrics(); + void drawQueryStartState(); + void drawQueryGoalState(); + void scheduleDrawQueryStartState(robot_interaction::InteractionHandler* handler, bool error_state_changed); + void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* handler, bool error_state_changed); + + bool isIKSolutionCollisionFree(moveit::core::RobotState* state, const moveit::core::JointModelGroup* group, + const double* ik_solution) const; + + void computeMetrics(bool start, const std::string& group, double payload); + void computeMetricsInternal(std::map& metrics, + const robot_interaction::EndEffectorInteraction& eef, + const moveit::core::RobotState& state, double payload); + void updateStateExceptModified(moveit::core::RobotState& dest, const moveit::core::RobotState& src); + void updateBackgroundJobProgressBar(); + void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname); + + void setQueryStateHelper(bool use_start_state, const std::string& v); + void populateMenuHandler(std::shared_ptr& mh); + + void selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr& msg); + + // overrides from Display + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void fixedFrameChanged() override; + + RobotStateVisualizationPtr query_robot_start_; ///< Handles drawing the robot at the start configuration + RobotStateVisualizationPtr query_robot_goal_; ///< Handles drawing the robot at the goal configuration + + Ogre::SceneNode* text_display_scene_node_; ///< displays texts + bool text_display_for_start_; ///< indicates whether the text display is for the start state or not + rviz_rendering::MovableText* text_to_display_; + + rclcpp::Subscription::SharedPtr planning_group_sub_; + + // render the workspace box + std::unique_ptr workspace_box_; + + // the planning frame + MotionPlanningFrame* frame_; + rviz_common::PanelDockWidget* frame_dock_; + + // robot interaction + robot_interaction::RobotInteractionPtr robot_interaction_; + robot_interaction::InteractionHandlerPtr query_start_state_; + robot_interaction::InteractionHandlerPtr query_goal_state_; + std::shared_ptr menu_handler_start_; + std::shared_ptr menu_handler_goal_; + std::map status_links_start_; + std::map status_links_goal_; + /// remember previous start state (updated before starting execution) + moveit::core::RobotStatePtr previous_state_; + + /// Hold the names of the groups for which the query states have been updated (and should not be altered when new info + /// is received from the planning scene) + std::set modified_groups_; + + /// The metrics are pairs of name-value for each of the active end effectors, for both start & goal states. + /// computed_metrics_[std::make_pair(IS_START_STATE, GROUP_NAME)] = a map of key-value pairs + std::map, std::map > computed_metrics_; + /// Some groups use position only ik, calls to the metrics have to be modified appropriately + std::map position_only_ik_; + + // Metric calculations + kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_; + std::map dynamics_solver_; + std::mutex update_metrics_lock_; + + // The trajectory playback component + TrajectoryVisualizationPtr trajectory_visual_; + + // properties to show on side panel + rviz_common::properties::Property* path_category_; + rviz_common::properties::Property* plan_category_; + rviz_common::properties::Property* metrics_category_; + + rviz_common::properties::EditableEnumProperty* planning_group_property_; + rviz_common::properties::BoolProperty* query_start_state_property_; + rviz_common::properties::BoolProperty* query_goal_state_property_; + rviz_common::properties::FloatProperty* query_marker_scale_property_; + rviz_common::properties::ColorProperty* query_start_color_property_; + rviz_common::properties::ColorProperty* query_goal_color_property_; + rviz_common::properties::FloatProperty* query_start_alpha_property_; + rviz_common::properties::FloatProperty* query_goal_alpha_property_; + rviz_common::properties::ColorProperty* query_colliding_link_color_property_; + rviz_common::properties::ColorProperty* query_outside_joint_limits_link_color_property_; + + rviz_common::properties::BoolProperty* compute_weight_limit_property_; + rviz_common::properties::BoolProperty* show_manipulability_index_property_; + rviz_common::properties::BoolProperty* show_manipulability_property_; + rviz_common::properties::BoolProperty* show_joint_torques_property_; + rviz_common::properties::FloatProperty* metrics_set_payload_property_; + rviz_common::properties::FloatProperty* metrics_text_height_property_; + rviz_common::properties::BoolProperty* show_workspace_property_; + + rviz_common::Display* int_marker_display_; +}; + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h index 6288c349ab..76730c7c12 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,308 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -#ifndef Q_MOC_RUN -#include -#include -#include -#include -#include -// TODO (ddengster): Enable when moveit_ros_perception is ported -// #include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#endif - -#include -#include -#include - -namespace rviz_common -{ -class DisplayContext; -} - -namespace Ui -{ -class MotionPlanningUI; -} - -namespace moveit_warehouse -{ -MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc -} // namespace moveit_warehouse - -namespace moveit_rviz_plugin -{ -class MotionPlanningDisplay; -class MotionPlanningFrameJointsWidget; - -const std::string OBJECT_RECOGNITION_ACTION = "/recognize_objects"; - -static const std::string TAB_CONTEXT = "Context"; -static const std::string TAB_PLANNING = "Planning"; -static const std::string TAB_MANIPULATION = "Manipulation"; -static const std::string TAB_OBJECTS = "Scene Objects"; -static const std::string TAB_SCENES = "Stored Scenes"; -static const std::string TAB_STATES = "Stored States"; -static const std::string TAB_STATUS = "Status"; - -static const double LARGE_MESH_THRESHOLD = 10.0; - -class MotionPlanningFrame : public QWidget -{ - friend class MotionPlanningDisplay; - Q_OBJECT - -public: - MotionPlanningFrame(const MotionPlanningFrame&) = delete; - MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_common::DisplayContext* context, QWidget* parent = nullptr); - ~MotionPlanningFrame() override; - - void clearRobotModel(); - void changePlanningGroup(); - void enable(); - void disable(); - void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); - -protected: - static const int ITEM_TYPE_SCENE = 1; - static const int ITEM_TYPE_QUERY = 2; - - void initFromMoveGroupNS(); - void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq); - - void updateSceneMarkers(double wall_dt, double ros_dt); - - void updateExternalCommunication(); - - MotionPlanningDisplay* planning_display_; - rviz_common::DisplayContext* context_; - Ui::MotionPlanningUI* ui_; - MotionPlanningFrameJointsWidget* joints_tab_; - - moveit::planning_interface::MoveGroupInterfacePtr move_group_; - // TODO (ddengster): Enable when moveit_ros_perception is ported - // moveit::semantic_world::SemanticWorldPtr semantic_world_; - - moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_; - moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_; - moveit_warehouse::ConstraintsStoragePtr constraints_storage_; - moveit_warehouse::RobotStateStoragePtr robot_state_storage_; - - std::shared_ptr scene_marker_; - - typedef std::map RobotStateMap; - typedef std::pair RobotStatePair; - RobotStateMap robot_states_; - std::string default_planning_pipeline_; - std::vector planner_descriptions_; - -Q_SIGNALS: - void planningFinished(); - void configChanged(); - -private Q_SLOTS: - - // Context tab - void databaseConnectButtonClicked(); - void planningPipelineIndexChanged(int index); - void planningAlgorithmIndexChanged(int index); - void resetDbButtonClicked(); - void approximateIKChanged(int state); - - // Planning tab - bool computeCartesianPlan(); - bool computeJointSpacePlan(); - void planButtonClicked(); - void executeButtonClicked(); - void planAndExecuteButtonClicked(); - void stopButtonClicked(); - void allowReplanningToggled(bool checked); - void allowLookingToggled(bool checked); - void allowExternalProgramCommunication(bool enable); - void pathConstraintsIndexChanged(int index); - void onNewPlanningSceneState(); - void startStateTextChanged(const QString& start_state); - void goalStateTextChanged(const QString& goal_state); - void planningGroupTextChanged(const QString& planning_group); - void onClearOctomapClicked(); - - // Scene Objects tab - void clearScene(); - void publishScene(); - void publishSceneIfNeeded(); - void setLocalSceneEdited(bool dirty = true); - bool isLocalSceneDirty() const; - void sceneScaleChanged(int value); - void sceneScaleStartChange(); - void sceneScaleEndChange(); - void shapesComboBoxChanged(const QString& text); - void addSceneObject(); - void removeSceneObject(); - void selectedCollisionObjectChanged(); - void objectPoseValueChanged(double value); - void collisionObjectChanged(QListWidgetItem* item); - void imProcessFeedback(visualization_msgs::msg::InteractiveMarkerFeedback& feedback); - void copySelectedCollisionObject(); - void exportGeometryAsTextButtonClicked(); - void importGeometryFromTextButtonClicked(); - - // Stored scenes tab - void saveSceneButtonClicked(); - void planningSceneItemClicked(); - void saveQueryButtonClicked(); - void deleteSceneButtonClicked(); - void deleteQueryButtonClicked(); - void loadSceneButtonClicked(); - void loadQueryButtonClicked(); - void warehouseItemNameChanged(QTreeWidgetItem* item, int column); - - // States tab - void loadStateButtonClicked(); - void saveStartStateButtonClicked(); - void saveGoalStateButtonClicked(); - void removeStateButtonClicked(); - void clearStatesButtonClicked(); - void setAsStartStateButtonClicked(); - void setAsGoalStateButtonClicked(); - - // Pick and place - void detectObjectsButtonClicked(); - void pickObjectButtonClicked(); - void placeObjectButtonClicked(); - void selectedDetectedObjectChanged(); - void detectedObjectChanged(QListWidgetItem* item); - void selectedSupportSurfaceChanged(); - - // General - void tabChanged(int index); - -private: - // Context tab - void computeDatabaseConnectButtonClicked(); - void computeDatabaseConnectButtonClickedHelper(int mode); - void computeResetDbButtonClicked(const std::string& db); - void populatePlannersList(const std::vector& desc); - void populatePlannerDescription(const moveit_msgs::msg::PlannerInterfaceDescription& desc); - - // Planning tab - void computePlanButtonClicked(); - void computeExecuteButtonClicked(); - void computePlanAndExecuteButtonClicked(); - void computePlanAndExecuteButtonClickedDisplayHelper(); - void computeStopButtonClicked(); - void onFinishedExecution(bool success); - void populateConstraintsList(); - void populateConstraintsList(const std::vector& constr); - void configureForPlanning(); - void configureWorkspace(); - void updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v); - void fillStateSelectionOptions(); - void fillPlanningGroupOptions(); - void startStateTextChangedExec(const std::string& start_state); - void goalStateTextChangedExec(const std::string& goal_state); - - // Scene objects tab - void updateCollisionObjectPose(bool update_marker_position); - void createSceneInteractiveMarker(); - void renameCollisionObject(QListWidgetItem* item); - void attachDetachCollisionObject(QListWidgetItem* item); - void populateCollisionObjectsList(); - void computeImportGeometryFromText(const std::string& path); - void computeExportGeometryAsText(const std::string& path); - visualization_msgs::msg::InteractiveMarker - createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj); - - // Stored scenes tab - void computeSaveSceneButtonClicked(); - void computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name); - void computeLoadSceneButtonClicked(); - void computeLoadQueryButtonClicked(); - void populatePlanningSceneTreeView(); - void computeDeleteSceneButtonClicked(); - void computeDeleteQueryButtonClicked(); - void computeDeleteQueryButtonClickedHelper(QTreeWidgetItem* s); - void checkPlanningSceneTreeEnabledButtons(); - - // States tab - void saveRobotStateButtonClicked(const moveit::core::RobotState& state); - void populateRobotStatesList(); - - // Pick and place - void processDetectedObjects(); - void updateDetectedObjectsList(const std::vector& object_ids); - void publishTables(); - void updateSupportSurfacesList(); - - std::map pick_object_name_; - std::string place_object_name_; - std::vector place_poses_; - // void pickObject(); - // void placeObject(); - void triggerObjectDetection(); - void updateTables(); - std::string support_surface_name_; - // For coloring - std::string selected_object_name_; - std::string selected_support_surface_name_; - - rclcpp_action::Client::SharedPtr object_recognition_client_; - void listenDetectedObjects(const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& msg); - rclcpp::Subscription::SharedPtr object_recognition_subscriber_; - - rclcpp::Subscription::SharedPtr plan_subscriber_; - rclcpp::Subscription::SharedPtr execute_subscriber_; - rclcpp::Subscription::SharedPtr stop_subscriber_; - rclcpp::Subscription::SharedPtr update_start_state_subscriber_; - rclcpp::Subscription::SharedPtr update_goal_state_subscriber_; - rclcpp::Subscription::SharedPtr update_custom_start_state_subscriber_; - rclcpp::Subscription::SharedPtr update_custom_goal_state_subscriber_; - - // General - void changePlanningGroupHelper(); - shapes::ShapePtr loadMeshResource(const std::string& url); - void loadStoredStates(const std::string& pattern); - - void remotePlanCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); - void remoteExecuteCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); - void remoteStopCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); - void remoteUpdateStartStateCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); - void remoteUpdateGoalStateCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); - void remoteUpdateCustomStartStateCallback(const moveit_msgs::msg::RobotState::ConstSharedPtr& msg); - void remoteUpdateCustomGoalStateCallback(const moveit_msgs::msg::RobotState::ConstSharedPtr& msg); - - /* Selects or unselects a item in a list by the item name */ - void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list); - - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; - rclcpp::Publisher::SharedPtr planning_scene_publisher_; - rclcpp::Publisher::SharedPtr planning_scene_world_publisher_; - - collision_detection::CollisionEnv::ObjectConstPtr scaled_object_; - moveit::core::FixedTransformsMap scaled_object_subframes_; - EigenSTL::vector_Isometry3d scaled_object_shape_poses_; - - std::vector > known_collision_objects_; - long unsigned int known_collision_objects_version_; - bool first_time_; - rclcpp::Client::SharedPtr clear_octomap_service_client_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp new file mode 100644 index 0000000000..67e44bb40a --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp @@ -0,0 +1,342 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +// TODO (ddengster): Enable when moveit_ros_perception is ported +// #include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#endif + +#include +#include +#include + +namespace rviz_common +{ +class DisplayContext; +} + +namespace Ui +{ +class MotionPlanningUI; +} + +namespace moveit_warehouse +{ +MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc +} // namespace moveit_warehouse + +namespace moveit_rviz_plugin +{ +class MotionPlanningDisplay; +class MotionPlanningFrameJointsWidget; + +const std::string OBJECT_RECOGNITION_ACTION = "/recognize_objects"; + +static const std::string TAB_CONTEXT = "Context"; +static const std::string TAB_PLANNING = "Planning"; +static const std::string TAB_MANIPULATION = "Manipulation"; +static const std::string TAB_OBJECTS = "Scene Objects"; +static const std::string TAB_SCENES = "Stored Scenes"; +static const std::string TAB_STATES = "Stored States"; +static const std::string TAB_STATUS = "Status"; + +static const double LARGE_MESH_THRESHOLD = 10.0; + +class MotionPlanningFrame : public QWidget +{ + friend class MotionPlanningDisplay; + Q_OBJECT + +public: + MotionPlanningFrame(const MotionPlanningFrame&) = delete; + MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_common::DisplayContext* context, QWidget* parent = nullptr); + ~MotionPlanningFrame() override; + + void clearRobotModel(); + void changePlanningGroup(); + void enable(); + void disable(); + void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); + +protected: + static const int ITEM_TYPE_SCENE = 1; + static const int ITEM_TYPE_QUERY = 2; + + void initFromMoveGroupNS(); + void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq); + + void updateSceneMarkers(double wall_dt, double ros_dt); + + void updateExternalCommunication(); + + MotionPlanningDisplay* planning_display_; + rviz_common::DisplayContext* context_; + Ui::MotionPlanningUI* ui_; + MotionPlanningFrameJointsWidget* joints_tab_; + + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + // TODO (ddengster): Enable when moveit_ros_perception is ported + // moveit::semantic_world::SemanticWorldPtr semantic_world_; + + moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_; + moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_; + moveit_warehouse::ConstraintsStoragePtr constraints_storage_; + moveit_warehouse::RobotStateStoragePtr robot_state_storage_; + + std::shared_ptr scene_marker_; + + typedef std::map RobotStateMap; + typedef std::pair RobotStatePair; + RobotStateMap robot_states_; + std::string default_planning_pipeline_; + std::vector planner_descriptions_; + +Q_SIGNALS: + void planningFinished(); + void configChanged(); + +private Q_SLOTS: + + // Context tab + void databaseConnectButtonClicked(); + void planningPipelineIndexChanged(int index); + void planningAlgorithmIndexChanged(int index); + void resetDbButtonClicked(); + void approximateIKChanged(int state); + + // Planning tab + bool computeCartesianPlan(); + bool computeJointSpacePlan(); + void planButtonClicked(); + void executeButtonClicked(); + void planAndExecuteButtonClicked(); + void stopButtonClicked(); + void allowReplanningToggled(bool checked); + void allowLookingToggled(bool checked); + void allowExternalProgramCommunication(bool enable); + void pathConstraintsIndexChanged(int index); + void onNewPlanningSceneState(); + void startStateTextChanged(const QString& start_state); + void goalStateTextChanged(const QString& goal_state); + void planningGroupTextChanged(const QString& planning_group); + void onClearOctomapClicked(); + + // Scene Objects tab + void clearScene(); + void publishScene(); + void publishSceneIfNeeded(); + void setLocalSceneEdited(bool dirty = true); + bool isLocalSceneDirty() const; + void sceneScaleChanged(int value); + void sceneScaleStartChange(); + void sceneScaleEndChange(); + void shapesComboBoxChanged(const QString& text); + void addSceneObject(); + void removeSceneObject(); + void selectedCollisionObjectChanged(); + void objectPoseValueChanged(double value); + void collisionObjectChanged(QListWidgetItem* item); + void imProcessFeedback(visualization_msgs::msg::InteractiveMarkerFeedback& feedback); + void copySelectedCollisionObject(); + void exportGeometryAsTextButtonClicked(); + void importGeometryFromTextButtonClicked(); + + // Stored scenes tab + void saveSceneButtonClicked(); + void planningSceneItemClicked(); + void saveQueryButtonClicked(); + void deleteSceneButtonClicked(); + void deleteQueryButtonClicked(); + void loadSceneButtonClicked(); + void loadQueryButtonClicked(); + void warehouseItemNameChanged(QTreeWidgetItem* item, int column); + + // States tab + void loadStateButtonClicked(); + void saveStartStateButtonClicked(); + void saveGoalStateButtonClicked(); + void removeStateButtonClicked(); + void clearStatesButtonClicked(); + void setAsStartStateButtonClicked(); + void setAsGoalStateButtonClicked(); + + // Pick and place + void detectObjectsButtonClicked(); + void pickObjectButtonClicked(); + void placeObjectButtonClicked(); + void selectedDetectedObjectChanged(); + void detectedObjectChanged(QListWidgetItem* item); + void selectedSupportSurfaceChanged(); + + // General + void tabChanged(int index); + +private: + // Context tab + void computeDatabaseConnectButtonClicked(); + void computeDatabaseConnectButtonClickedHelper(int mode); + void computeResetDbButtonClicked(const std::string& db); + void populatePlannersList(const std::vector& desc); + void populatePlannerDescription(const moveit_msgs::msg::PlannerInterfaceDescription& desc); + + // Planning tab + void computePlanButtonClicked(); + void computeExecuteButtonClicked(); + void computePlanAndExecuteButtonClicked(); + void computePlanAndExecuteButtonClickedDisplayHelper(); + void computeStopButtonClicked(); + void onFinishedExecution(bool success); + void populateConstraintsList(); + void populateConstraintsList(const std::vector& constr); + void configureForPlanning(); + void configureWorkspace(); + void updateQueryStateHelper(moveit::core::RobotState& state, const std::string& v); + void fillStateSelectionOptions(); + void fillPlanningGroupOptions(); + void startStateTextChangedExec(const std::string& start_state); + void goalStateTextChangedExec(const std::string& goal_state); + + // Scene objects tab + void updateCollisionObjectPose(bool update_marker_position); + void createSceneInteractiveMarker(); + void renameCollisionObject(QListWidgetItem* item); + void attachDetachCollisionObject(QListWidgetItem* item); + void populateCollisionObjectsList(); + void computeImportGeometryFromText(const std::string& path); + void computeExportGeometryAsText(const std::string& path); + visualization_msgs::msg::InteractiveMarker + createObjectMarkerMsg(const collision_detection::CollisionEnv::ObjectConstPtr& obj); + + // Stored scenes tab + void computeSaveSceneButtonClicked(); + void computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name); + void computeLoadSceneButtonClicked(); + void computeLoadQueryButtonClicked(); + void populatePlanningSceneTreeView(); + void computeDeleteSceneButtonClicked(); + void computeDeleteQueryButtonClicked(); + void computeDeleteQueryButtonClickedHelper(QTreeWidgetItem* s); + void checkPlanningSceneTreeEnabledButtons(); + + // States tab + void saveRobotStateButtonClicked(const moveit::core::RobotState& state); + void populateRobotStatesList(); + + // Pick and place + void processDetectedObjects(); + void updateDetectedObjectsList(const std::vector& object_ids); + void publishTables(); + void updateSupportSurfacesList(); + + std::map pick_object_name_; + std::string place_object_name_; + std::vector place_poses_; + // void pickObject(); + // void placeObject(); + void triggerObjectDetection(); + void updateTables(); + std::string support_surface_name_; + // For coloring + std::string selected_object_name_; + std::string selected_support_surface_name_; + + rclcpp_action::Client::SharedPtr object_recognition_client_; + void listenDetectedObjects(const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& msg); + rclcpp::Subscription::SharedPtr object_recognition_subscriber_; + + rclcpp::Subscription::SharedPtr plan_subscriber_; + rclcpp::Subscription::SharedPtr execute_subscriber_; + rclcpp::Subscription::SharedPtr stop_subscriber_; + rclcpp::Subscription::SharedPtr update_start_state_subscriber_; + rclcpp::Subscription::SharedPtr update_goal_state_subscriber_; + rclcpp::Subscription::SharedPtr update_custom_start_state_subscriber_; + rclcpp::Subscription::SharedPtr update_custom_goal_state_subscriber_; + + // General + void changePlanningGroupHelper(); + shapes::ShapePtr loadMeshResource(const std::string& url); + void loadStoredStates(const std::string& pattern); + + void remotePlanCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); + void remoteExecuteCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); + void remoteStopCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); + void remoteUpdateStartStateCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); + void remoteUpdateGoalStateCallback(const std_msgs::msg::Empty::ConstSharedPtr& msg); + void remoteUpdateCustomStartStateCallback(const moveit_msgs::msg::RobotState::ConstSharedPtr& msg); + void remoteUpdateCustomGoalStateCallback(const moveit_msgs::msg::RobotState::ConstSharedPtr& msg); + + /* Selects or unselects a item in a list by the item name */ + void setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list); + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr planning_scene_publisher_; + rclcpp::Publisher::SharedPtr planning_scene_world_publisher_; + + collision_detection::CollisionEnv::ObjectConstPtr scaled_object_; + moveit::core::FixedTransformsMap scaled_object_subframes_; + EigenSTL::vector_Isometry3d scaled_object_shape_poses_; + + std::vector > known_collision_objects_; + long unsigned int known_collision_objects_version_; + bool first_time_; + rclcpp::Client::SharedPtr clear_octomap_service_client_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h index 76dd1cd4e2..2c4c6e8cbf 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,221 +47,5 @@ /* Author: Robert Haschke */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -class QSlider; - -namespace Ui -{ -class MotionPlanningFrameJointsUI; -} -namespace robot_interaction -{ -MOVEIT_CLASS_FORWARD(InteractionHandler); -} -namespace moveit_rviz_plugin -{ -/** TableModel to display joint values of a referenced RobotState. - * - * Unfortunately we cannot store the RobotStatePtr (and thus ensure existence of the state during - * the lifetime of this class instance), because RobotInteraction (which is the initial use case) - * allocates internally a new RobotState if any other copy is held somewhere else. - * Hence, we also store an (unsafe) raw pointer. Lifetime of this raw pointer needs to be ensured. - */ -class JMGItemModel : public QAbstractTableModel -{ - Q_OBJECT - moveit::core::RobotState robot_state_; - const moveit::core::JointModelGroup* jmg_; - -public: - JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent = nullptr); - - // QAbstractItemModel interface - int rowCount(const QModelIndex& parent = QModelIndex()) const override; - int columnCount(const QModelIndex& parent = QModelIndex()) const override; - Qt::ItemFlags flags(const QModelIndex& index) const override; - QVariant data(const QModelIndex& index, int role) const override; - QVariant headerData(int section, Qt::Orientation orientation, int role) const override; - bool setData(const QModelIndex& index, const QVariant& value, int role) override; - - /// call this on any external change of the RobotState - void updateRobotState(const moveit::core::RobotState& state); - - moveit::core::RobotState& getRobotState() - { - return robot_state_; - } - const moveit::core::RobotState& getRobotState() const - { - return robot_state_; - } - const moveit::core::JointModelGroup* getJointModelGroup() const - { - return jmg_; - } - -private: - /// retrieve the JointModel corresponding to the variable referenced by index - const moveit::core::JointModel* getJointModel(const QModelIndex& index) const; - /// retrieve the variable bounds referenced by variable index - const moveit::core::VariableBounds* getVariableBounds(const moveit::core::JointModel* jm, - const QModelIndex& index) const; -}; - -class JointsWidgetEventFilter : public QObject -{ - Q_OBJECT - -public: - JointsWidgetEventFilter(QAbstractItemView* view); - -protected: - bool eventFilter(QObject* target, QEvent* event) override; -}; - -class MotionPlanningDisplay; -class MotionPlanningFrameJointsWidget : public QWidget -{ - Q_OBJECT - -public: - MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete; - MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); - ~MotionPlanningFrameJointsWidget() override; - - void clearRobotModel(); - void changePlanningGroup(const std::string& group_name, - const robot_interaction::InteractionHandlerPtr& start_state_handler, - const robot_interaction::InteractionHandlerPtr& goal_state_handler); - -public Q_SLOTS: - void queryStartStateChanged(); - void queryGoalStateChanged(); - void jogNullspace(double value); - -protected: - void setActiveModel(JMGItemModel* model); - void triggerUpdate(JMGItemModel* model); - void updateNullspaceSliders(); - QSlider* createNSSlider(int i); - -private: - Ui::MotionPlanningFrameJointsUI* ui_; - MotionPlanningDisplay* planning_display_; - robot_interaction::InteractionHandlerPtr start_state_handler_; - robot_interaction::InteractionHandlerPtr goal_state_handler_; - std::unique_ptr start_state_model_; - std::unique_ptr goal_state_model_; - // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State() - bool ignore_state_changes_ = false; - - Eigen::JacobiSVD svd_; - Eigen::MatrixXd nullspace_; - std::vector ns_sliders_; -}; - -/// Delegate to show the joint value as with a progress bar indicator between min and max. -class ProgressBarDelegate : public QStyledItemDelegate -{ - Q_OBJECT - -public: - enum CustomRole - { - JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming) - VariableBoundsRole // NOLINT(readability-identifier-naming) - }; - - ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent) - { - } - - void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; - QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override; - -private Q_SLOTS: - void commitAndCloseEditor(); -}; - -/// Number editor via progress bar dragging -class ProgressBarEditor : public QWidget -{ - Q_OBJECT - Q_PROPERTY(double value READ getValue WRITE setValue NOTIFY valueChanged USER true) - -public: - /// Create a progressbar-like slider for editing values in range mix..max - ProgressBarEditor(QWidget* parent = nullptr, double min = -1.0, double max = 0.0, int digits = 0); - - void setValue(double value) - { - value_ = value; - } - double getValue() const - { - return value_; - } - -Q_SIGNALS: - void valueChanged(double value); - void editingFinished(); - -protected: - void paintEvent(QPaintEvent* event) override; - void mousePressEvent(QMouseEvent* event) override; - void mouseMoveEvent(QMouseEvent* event) override; - void mouseReleaseEvent(QMouseEvent* event) override; - -private: - double value_; - double min_; - double max_; - int digits_; ///< number of decimal digits for formatting of the value -}; - -/// Slider that jumps back to zero -class JogSlider : public QSlider -{ - Q_OBJECT - int timer_id_; - int timer_interval_; // ms - double maximum_; - -public: - JogSlider(QWidget* parent = nullptr); - - int timerInterval() const - { - return timer_interval_; - } - void setTimerInterval(int ms); - void setResolution(unsigned int resolution); - void setMaximum(double value); - double value() const - { - return QSlider::value() * maximum_ / QSlider::maximum(); - } - -protected: - void timerEvent(QTimerEvent* event) override; - void mousePressEvent(QMouseEvent* event) override; - void mouseReleaseEvent(QMouseEvent* event) override; - -private: - using QSlider::setMaximum; - using QSlider::setMinimum; - using QSlider::setRange; - -Q_SIGNALS: - void triggered(double value); -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.hpp new file mode 100644 index 0000000000..9f4fdbb32a --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.hpp @@ -0,0 +1,255 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, CITEC, Bielefeld University + * 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 CITEC / Bielefeld University 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: Robert Haschke */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class QSlider; + +namespace Ui +{ +class MotionPlanningFrameJointsUI; +} +namespace robot_interaction +{ +MOVEIT_CLASS_FORWARD(InteractionHandler); +} +namespace moveit_rviz_plugin +{ +/** TableModel to display joint values of a referenced RobotState. + * + * Unfortunately we cannot store the RobotStatePtr (and thus ensure existence of the state during + * the lifetime of this class instance), because RobotInteraction (which is the initial use case) + * allocates internally a new RobotState if any other copy is held somewhere else. + * Hence, we also store an (unsafe) raw pointer. Lifetime of this raw pointer needs to be ensured. + */ +class JMGItemModel : public QAbstractTableModel +{ + Q_OBJECT + moveit::core::RobotState robot_state_; + const moveit::core::JointModelGroup* jmg_; + +public: + JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent = nullptr); + + // QAbstractItemModel interface + int rowCount(const QModelIndex& parent = QModelIndex()) const override; + int columnCount(const QModelIndex& parent = QModelIndex()) const override; + Qt::ItemFlags flags(const QModelIndex& index) const override; + QVariant data(const QModelIndex& index, int role) const override; + QVariant headerData(int section, Qt::Orientation orientation, int role) const override; + bool setData(const QModelIndex& index, const QVariant& value, int role) override; + + /// call this on any external change of the RobotState + void updateRobotState(const moveit::core::RobotState& state); + + moveit::core::RobotState& getRobotState() + { + return robot_state_; + } + const moveit::core::RobotState& getRobotState() const + { + return robot_state_; + } + const moveit::core::JointModelGroup* getJointModelGroup() const + { + return jmg_; + } + +private: + /// retrieve the JointModel corresponding to the variable referenced by index + const moveit::core::JointModel* getJointModel(const QModelIndex& index) const; + /// retrieve the variable bounds referenced by variable index + const moveit::core::VariableBounds* getVariableBounds(const moveit::core::JointModel* jm, + const QModelIndex& index) const; +}; + +class JointsWidgetEventFilter : public QObject +{ + Q_OBJECT + +public: + JointsWidgetEventFilter(QAbstractItemView* view); + +protected: + bool eventFilter(QObject* target, QEvent* event) override; +}; + +class MotionPlanningDisplay; +class MotionPlanningFrameJointsWidget : public QWidget +{ + Q_OBJECT + +public: + MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete; + MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr); + ~MotionPlanningFrameJointsWidget() override; + + void clearRobotModel(); + void changePlanningGroup(const std::string& group_name, + const robot_interaction::InteractionHandlerPtr& start_state_handler, + const robot_interaction::InteractionHandlerPtr& goal_state_handler); + +public Q_SLOTS: + void queryStartStateChanged(); + void queryGoalStateChanged(); + void jogNullspace(double value); + +protected: + void setActiveModel(JMGItemModel* model); + void triggerUpdate(JMGItemModel* model); + void updateNullspaceSliders(); + QSlider* createNSSlider(int i); + +private: + Ui::MotionPlanningFrameJointsUI* ui_; + MotionPlanningDisplay* planning_display_; + robot_interaction::InteractionHandlerPtr start_state_handler_; + robot_interaction::InteractionHandlerPtr goal_state_handler_; + std::unique_ptr start_state_model_; + std::unique_ptr goal_state_model_; + // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State() + bool ignore_state_changes_ = false; + + Eigen::JacobiSVD svd_; + Eigen::MatrixXd nullspace_; + std::vector ns_sliders_; +}; + +/// Delegate to show the joint value as with a progress bar indicator between min and max. +class ProgressBarDelegate : public QStyledItemDelegate +{ + Q_OBJECT + +public: + enum CustomRole + { + JointTypeRole = Qt::UserRole, // NOLINT(readability-identifier-naming) + VariableBoundsRole // NOLINT(readability-identifier-naming) + }; + + ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent) + { + } + + void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override; + QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override; + +private Q_SLOTS: + void commitAndCloseEditor(); +}; + +/// Number editor via progress bar dragging +class ProgressBarEditor : public QWidget +{ + Q_OBJECT + Q_PROPERTY(double value READ getValue WRITE setValue NOTIFY valueChanged USER true) + +public: + /// Create a progressbar-like slider for editing values in range mix..max + ProgressBarEditor(QWidget* parent = nullptr, double min = -1.0, double max = 0.0, int digits = 0); + + void setValue(double value) + { + value_ = value; + } + double getValue() const + { + return value_; + } + +Q_SIGNALS: + void valueChanged(double value); + void editingFinished(); + +protected: + void paintEvent(QPaintEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + +private: + double value_; + double min_; + double max_; + int digits_; ///< number of decimal digits for formatting of the value +}; + +/// Slider that jumps back to zero +class JogSlider : public QSlider +{ + Q_OBJECT + int timer_id_; + int timer_interval_; // ms + double maximum_; + +public: + JogSlider(QWidget* parent = nullptr); + + int timerInterval() const + { + return timer_interval_; + } + void setTimerInterval(int ms); + void setResolution(unsigned int resolution); + void setMaximum(double value); + double value() const + { + return QSlider::value() * maximum_ / QSlider::maximum(); + } + +protected: + void timerEvent(QTimerEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + +private: + using QSlider::setMaximum; + using QSlider::setMinimum; + using QSlider::setRange; + +Q_SIGNALS: + void triggered(double value); +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h index 3935103719..2777245a6b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,45 +47,5 @@ /* Author: Robert Haschke */ #pragma once - -#include -#include - -namespace moveit -{ -namespace planning_interface -{ -MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc -} -} // namespace moveit - -namespace moveit_rviz_plugin -{ -class MotionPlanningParamWidget : public rviz_common::properties::PropertyTreeWidget -{ - Q_OBJECT -public: - MotionPlanningParamWidget(const MotionPlanningParamWidget&) = delete; - MotionPlanningParamWidget(QWidget* parent = nullptr); - ~MotionPlanningParamWidget() override; - - void setMoveGroup(const moveit::planning_interface::MoveGroupInterfacePtr& mg); - void setGroupName(const std::string& group_name); - -public Q_SLOTS: - void setPlannerId(const std::string& planner_id); - -private Q_SLOTS: - void changedValue(); - -private: - rviz_common::properties::Property* createPropertyTree(); - -private: - rviz_common::properties::PropertyTreeModel* property_tree_model_; - - moveit::planning_interface::MoveGroupInterfacePtr move_group_; - std::string group_name_; - std::string planner_id_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.hpp new file mode 100644 index 0000000000..157fd4a1f9 --- /dev/null +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.hpp @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Robert Haschke */ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc +} +} // namespace moveit + +namespace moveit_rviz_plugin +{ +class MotionPlanningParamWidget : public rviz_common::properties::PropertyTreeWidget +{ + Q_OBJECT +public: + MotionPlanningParamWidget(const MotionPlanningParamWidget&) = delete; + MotionPlanningParamWidget(QWidget* parent = nullptr); + ~MotionPlanningParamWidget() override; + + void setMoveGroup(const moveit::planning_interface::MoveGroupInterfacePtr& mg); + void setGroupName(const std::string& group_name); + +public Q_SLOTS: + void setPlannerId(const std::string& planner_id); + +private Q_SLOTS: + void changedValue(); + +private: + rviz_common::properties::Property* createPropertyTree(); + +private: + rviz_common::properties::PropertyTreeModel* property_tree_model_; + + moveit::planning_interface::MoveGroupInterfacePtr move_group_; + std::string group_name_; + std::string planner_id_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/interactive_marker_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/interactive_marker_display.cpp index bdd67a9a65..ad791e7053 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/interactive_marker_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/interactive_marker_display.cpp @@ -39,7 +39,7 @@ #include -#include +#include namespace rviz_default_plugins { diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 110fe48915..1880c59b3a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -34,15 +34,15 @@ /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include -#include +#include #include #include @@ -62,13 +62,13 @@ #include #include -#include -#include +#include +#include #include #include "ui_motion_planning_rviz_plugin_frame.h" -#include +#include #include namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 72850cf9e2..074545f324 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -36,11 +36,11 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp index 497f2655fd..13aa519ac6 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp @@ -33,11 +33,11 @@ *********************************************************************/ /* Author: Ioan Sucan */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 3157891ec2..39de464182 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -34,8 +34,8 @@ /* Author: Robert Haschke */ -#include -#include +#include +#include #include "ui_motion_planning_rviz_plugin_frame_joints.h" #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp index 5c20b48776..57ef02b94f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp @@ -34,11 +34,11 @@ /* Author: Sachin Chitta */ -#include -#include +#include +#include -#include -#include +#include +#include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 4d473bbf11..f3faa13b3b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -33,12 +33,12 @@ *********************************************************************/ /* Author: Ioan Sucan, Mario Prats */ -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 746685ed43..115f32bc6a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -34,17 +34,17 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include #include #include #include -#include +#include #include "ui_motion_planning_rviz_plugin_frame.h" diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp index 245b750269..bfb391b823 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp @@ -33,15 +33,15 @@ *********************************************************************/ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp index 35d84a425e..1280f2e38a 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_states.cpp @@ -33,11 +33,11 @@ *********************************************************************/ /* Author: Mario Prats, Ioan Sucan */ -#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp index b896492ac8..58ce158323 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp @@ -34,8 +34,8 @@ /* Author: Robert Haschke */ -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/plugin_init.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/plugin_init.cpp index 2ab60fe715..e67351449b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/plugin_init.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/plugin_init.cpp @@ -35,6 +35,6 @@ /* Author: Ioan Sucan */ #include -#include +#include CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::MotionPlanningDisplay, rviz_common::Display) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 346228701e..18fb7210bc 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -2080,7 +2080,7 @@ This is usually achieved by random seeding, which can flip the robot configurati moveit_rviz_plugin::MotionPlanningParamWidget QTreeView -
moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h
+
moveit/motion_planning_rviz_plugin/motion_planning_param_widget.hpp
 diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 71f84afb5d..efe2dc41f4 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -1,7 +1,7 @@ add_library( moveit_planning_scene_rviz_plugin_core SHARED src/planning_scene_display.cpp src/background_processing.cpp - include/moveit/planning_scene_rviz_plugin/planning_scene_display.h) + include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp) include(GenerateExportHeader) generate_export_header(moveit_planning_scene_rviz_plugin_core) target_include_directories( diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.h new file mode 100644 index 0000000000..c573328ffe --- /dev/null +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h index 5fa7fe9019..4b778bac2e 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,193 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#ifndef Q_MOC_RUN -#include -#include -#include -#include -#endif - -#include - -namespace Ogre -{ -class SceneNode; -} - -namespace rviz -{ -class Robot; -class Property; -class StringProperty; -class BoolProperty; -class FloatProperty; -class RosTopicProperty; -class ColorProperty; -class EnumProperty; -} // namespace rviz - -namespace moveit_rviz_plugin -{ -class MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT PlanningSceneDisplay : public rviz_common::Display -{ - Q_OBJECT - -public: - PlanningSceneDisplay(bool listen_to_planning_scene = true, bool show_scene_robot = true); - ~PlanningSceneDisplay() override; - - void load(const rviz_common::Config& config) override; - void save(rviz_common::Config config) const override; - - void update(float wall_dt, float ros_dt) override; - void reset() override; - - void setLinkColor(const std::string& link_name, const QColor& color); - void unsetLinkColor(const std::string& link_name); - - void queueRenderSceneGeometry(); - - /** Queue this function call for execution within the background thread - All jobs are queued and processed in order by a single background thread. */ - void addBackgroundJob(const std::function& job, const std::string& name); - - /** Directly spawn a (detached) background thread for execution of this function call - Should be used, when order of processing is not relevant / job can run in parallel. - Must be used, when job will be blocking. Using addBackgroundJob() in this case will block other queued jobs as - well */ - void spawnBackgroundJob(const std::function& job); - - /// queue the execution of this function for the next time the main update() loop gets called - void addMainLoopJob(const std::function& job); - - void waitForAllMainLoopJobs(); - - /// remove all queued jobs - void clearJobs(); - - const std::string getMoveGroupNS() const; - const moveit::core::RobotModelConstPtr& getRobotModel() const; - - /// wait for robot state more recent than t - bool waitForCurrentRobotState(const rclcpp::Time& t); - /// get read-only access to planning scene - planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const; - /// get write access to planning scene - planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW(); - const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor(); - -private Q_SLOTS: - - // ****************************************************************************************** - // Slot Event Functions - // ****************************************************************************************** - void changedMoveGroupNS(); - void changedRobotDescription(); - void changedSceneName(); - void changedSceneEnabled(); - void changedSceneRobotVisualEnabled(); - void changedSceneRobotCollisionEnabled(); - void changedRobotSceneAlpha(); - void changedSceneAlpha(); - void changedSceneColor(); - void changedPlanningSceneTopic(); - void changedSceneDisplayTime(); - void changedOctreeRenderMode(); - void changedOctreeColorMode(); - void setSceneName(const QString& name); - -protected Q_SLOTS: - virtual void changedAttachedBodyColor(); - -protected: - /// This function reloads the robot model and reinitializes the PlanningSceneMonitor - /// It can be called either from the Main Loop or from a Background thread - void loadRobotModel(); - - /// This function is used by loadRobotModel() and should only be called in the MainLoop - /// You probably should not call this function directly - virtual void clearRobotModel(); - - /// This function constructs a new planning scene. Probably this should be called in a background thread - /// as it may take some time to complete its execution - virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(); - - /// This is an event called by loadRobotModel() in the MainLoop; do not call directly - virtual void onRobotModelLoaded(); - /// This is called upon successful retrieval of the (initial) planning scene state - virtual void onNewPlanningSceneState(); - - /** - * \brief Set the scene node's position, given the target frame and the planning frame - */ - void calculateOffsetPosition(); - - void executeMainLoopJobs(); - void sceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); - void renderPlanningScene(); - void setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name, const QColor& color); - void unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name); - void setGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name, const QColor& color); - void unsetGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name); - void unsetAllColors(rviz_default_plugins::robot::Robot* robot); - - // overrides from Display - void onInitialize() override; - void onEnable() override; - void onDisable() override; - void fixedFrameChanged() override; - - // new virtual functions added by this plugin - virtual void updateInternal(double wall_dt, double ros_dt); - virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); - - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - std::mutex robot_model_loading_lock_; - - moveit::tools::BackgroundProcessing background_process_; - std::deque > main_loop_jobs_; - std::mutex main_loop_jobs_lock_; - std::condition_variable main_loop_jobs_empty_condition_; - - Ogre::SceneNode* planning_scene_node_; ///< displays planning scene with everything in it - - // render the planning scene - RobotStateVisualizationPtr planning_scene_robot_; - PlanningSceneRenderPtr planning_scene_render_; - - // full update required - bool planning_scene_needs_render_; - // or only the robot position (excluding attached object changes) - bool robot_state_needs_render_; - double current_scene_time_; - - rviz_common::properties::Property* scene_category_; - rviz_common::properties::Property* robot_category_; - - rviz_common::properties::StringProperty* move_group_ns_property_; - rviz_common::properties::StringProperty* robot_description_property_; - rviz_common::properties::StringProperty* scene_name_property_; - rviz_common::properties::BoolProperty* scene_enabled_property_; - rviz_common::properties::BoolProperty* scene_robot_visual_enabled_property_; - rviz_common::properties::BoolProperty* scene_robot_collision_enabled_property_; - rviz_common::properties::RosTopicProperty* planning_scene_topic_property_; - rviz_common::properties::FloatProperty* robot_alpha_property_; - rviz_common::properties::FloatProperty* scene_alpha_property_; - rviz_common::properties::ColorProperty* scene_color_property_; - rviz_common::properties::ColorProperty* attached_body_color_property_; - rviz_common::properties::FloatProperty* scene_display_time_property_; - rviz_common::properties::EnumProperty* octree_render_property_; - rviz_common::properties::EnumProperty* octree_coloring_property_; - - // rclcpp node - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; -}; - -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp new file mode 100644 index 0000000000..1c0aa57c77 --- /dev/null +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp @@ -0,0 +1,227 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#endif + +#include + +namespace Ogre +{ +class SceneNode; +} + +namespace rviz +{ +class Robot; +class Property; +class StringProperty; +class BoolProperty; +class FloatProperty; +class RosTopicProperty; +class ColorProperty; +class EnumProperty; +} // namespace rviz + +namespace moveit_rviz_plugin +{ +class MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT PlanningSceneDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + PlanningSceneDisplay(bool listen_to_planning_scene = true, bool show_scene_robot = true); + ~PlanningSceneDisplay() override; + + void load(const rviz_common::Config& config) override; + void save(rviz_common::Config config) const override; + + void update(float wall_dt, float ros_dt) override; + void reset() override; + + void setLinkColor(const std::string& link_name, const QColor& color); + void unsetLinkColor(const std::string& link_name); + + void queueRenderSceneGeometry(); + + /** Queue this function call for execution within the background thread + All jobs are queued and processed in order by a single background thread. */ + void addBackgroundJob(const std::function& job, const std::string& name); + + /** Directly spawn a (detached) background thread for execution of this function call + Should be used, when order of processing is not relevant / job can run in parallel. + Must be used, when job will be blocking. Using addBackgroundJob() in this case will block other queued jobs as + well */ + void spawnBackgroundJob(const std::function& job); + + /// queue the execution of this function for the next time the main update() loop gets called + void addMainLoopJob(const std::function& job); + + void waitForAllMainLoopJobs(); + + /// remove all queued jobs + void clearJobs(); + + const std::string getMoveGroupNS() const; + const moveit::core::RobotModelConstPtr& getRobotModel() const; + + /// wait for robot state more recent than t + bool waitForCurrentRobotState(const rclcpp::Time& t); + /// get read-only access to planning scene + planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const; + /// get write access to planning scene + planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW(); + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor(); + +private Q_SLOTS: + + // ****************************************************************************************** + // Slot Event Functions + // ****************************************************************************************** + void changedMoveGroupNS(); + void changedRobotDescription(); + void changedSceneName(); + void changedSceneEnabled(); + void changedSceneRobotVisualEnabled(); + void changedSceneRobotCollisionEnabled(); + void changedRobotSceneAlpha(); + void changedSceneAlpha(); + void changedSceneColor(); + void changedPlanningSceneTopic(); + void changedSceneDisplayTime(); + void changedOctreeRenderMode(); + void changedOctreeColorMode(); + void setSceneName(const QString& name); + +protected Q_SLOTS: + virtual void changedAttachedBodyColor(); + +protected: + /// This function reloads the robot model and reinitializes the PlanningSceneMonitor + /// It can be called either from the Main Loop or from a Background thread + void loadRobotModel(); + + /// This function is used by loadRobotModel() and should only be called in the MainLoop + /// You probably should not call this function directly + virtual void clearRobotModel(); + + /// This function constructs a new planning scene. Probably this should be called in a background thread + /// as it may take some time to complete its execution + virtual planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(); + + /// This is an event called by loadRobotModel() in the MainLoop; do not call directly + virtual void onRobotModelLoaded(); + /// This is called upon successful retrieval of the (initial) planning scene state + virtual void onNewPlanningSceneState(); + + /** + * \brief Set the scene node's position, given the target frame and the planning frame + */ + void calculateOffsetPosition(); + + void executeMainLoopJobs(); + void sceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); + void renderPlanningScene(); + void setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name, const QColor& color); + void unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name); + void setGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name, const QColor& color); + void unsetGroupColor(rviz_default_plugins::robot::Robot* robot, const std::string& group_name); + void unsetAllColors(rviz_default_plugins::robot::Robot* robot); + + // overrides from Display + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void fixedFrameChanged() override; + + // new virtual functions added by this plugin + virtual void updateInternal(double wall_dt, double ros_dt); + virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + std::mutex robot_model_loading_lock_; + + moveit::tools::BackgroundProcessing background_process_; + std::deque > main_loop_jobs_; + std::mutex main_loop_jobs_lock_; + std::condition_variable main_loop_jobs_empty_condition_; + + Ogre::SceneNode* planning_scene_node_; ///< displays planning scene with everything in it + + // render the planning scene + RobotStateVisualizationPtr planning_scene_robot_; + PlanningSceneRenderPtr planning_scene_render_; + + // full update required + bool planning_scene_needs_render_; + // or only the robot position (excluding attached object changes) + bool robot_state_needs_render_; + double current_scene_time_; + + rviz_common::properties::Property* scene_category_; + rviz_common::properties::Property* robot_category_; + + rviz_common::properties::StringProperty* move_group_ns_property_; + rviz_common::properties::StringProperty* robot_description_property_; + rviz_common::properties::StringProperty* scene_name_property_; + rviz_common::properties::BoolProperty* scene_enabled_property_; + rviz_common::properties::BoolProperty* scene_robot_visual_enabled_property_; + rviz_common::properties::BoolProperty* scene_robot_collision_enabled_property_; + rviz_common::properties::RosTopicProperty* planning_scene_topic_property_; + rviz_common::properties::FloatProperty* robot_alpha_property_; + rviz_common::properties::FloatProperty* scene_alpha_property_; + rviz_common::properties::ColorProperty* scene_color_property_; + rviz_common::properties::ColorProperty* attached_body_color_property_; + rviz_common::properties::FloatProperty* scene_display_time_property_; + rviz_common::properties::EnumProperty* octree_render_property_; + rviz_common::properties::EnumProperty* octree_coloring_property_; + + // rclcpp node + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; +}; + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 859d0c0b48..4bce580a25 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -35,10 +35,10 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -55,7 +55,7 @@ #include #include -#include +#include #include namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/plugin_init.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/plugin_init.cpp index 019342fa8d..8e9fdfa85f 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/plugin_init.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/plugin_init.cpp @@ -35,6 +35,6 @@ /* Author: Ioan Sucan */ #include -#include +#include CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::PlanningSceneDisplay, rviz_common::Display) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index 73b5751a1e..f9bd7f4c8b 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -1,7 +1,7 @@ add_library( moveit_robot_state_rviz_plugin_core SHARED src/robot_state_display.cpp - include/moveit/robot_state_rviz_plugin/robot_state_display.h) + include/moveit/robot_state_rviz_plugin/robot_state_display.hpp) set_target_properties(moveit_robot_state_rviz_plugin_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(moveit_robot_state_rviz_plugin_core diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h index 3661a09581..278e0a5154 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,115 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include - -#ifndef Q_MOC_RUN -#include -#include -#include -#include -#endif - -namespace rviz_common -{ -namespace properties -{ -class Robot; -class StringProperty; -class BoolProperty; -class FloatProperty; -class RosTopicProperty; -class ColorProperty; -} // namespace properties -} // namespace rviz_common - -namespace moveit_rviz_plugin -{ -class RobotStateVisualization; - -class RobotStateDisplay : public rviz_common::Display -{ - Q_OBJECT - -public: - RobotStateDisplay(); - ~RobotStateDisplay() override; - - void load(const rviz_common::Config& config) override; - void update(float wall_dt, float ros_dt) override; - void reset() override; - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - void setLinkColor(const std::string& link_name, const QColor& color); - void unsetLinkColor(const std::string& link_name); - -public Q_SLOTS: - void setVisible(bool visible); - -private Q_SLOTS: - - // ****************************************************************************************** - // Slot Event Functions - // ****************************************************************************************** - void changedRobotDescription(); - void changedRootLinkName(); - void changedRobotSceneAlpha(); - void changedAttachedBodyColor(); - void changedRobotStateTopic(); - void changedEnableLinkHighlight(); - void changedEnableVisualVisible(); - void changedEnableCollisionVisible(); - void changedAllLinks(); - -protected: - void initializeLoader(); - void loadRobotModel(); - - /** - * \brief Set the scene node's position, given the target frame and the planning frame - */ - void calculateOffsetPosition(); - - void setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name, const QColor& color); - void unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name); - - void newRobotStateCallback(const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state); - - void setRobotHighlights(const moveit_msgs::msg::DisplayRobotState::_highlight_links_type& highlight_links); - void setHighlight(const std::string& link_name, const std_msgs::msg::ColorRGBA& color); - void unsetHighlight(const std::string& link_name); - - // overrides from Display - void onInitialize() override; - void onEnable() override; - void onDisable() override; - void fixedFrameChanged() override; - - // render the robot - rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr robot_state_subscriber_; - - RobotStateVisualizationPtr robot_; - rdf_loader::RDFLoaderPtr rdf_loader_; - moveit::core::RobotModelConstPtr robot_model_; - moveit::core::RobotStatePtr robot_state_; - std::map highlights_; - bool update_state_; - - rviz_common::properties::StringProperty* robot_description_property_; - rviz_common::properties::StringProperty* root_link_name_property_; - rviz_common::properties::RosTopicProperty* robot_state_topic_property_; - rviz_common::properties::FloatProperty* robot_alpha_property_; - rviz_common::properties::ColorProperty* attached_body_color_property_; - rviz_common::properties::BoolProperty* enable_link_highlight_; - rviz_common::properties::BoolProperty* enable_visual_visible_; - rviz_common::properties::BoolProperty* enable_collision_visible_; - rviz_common::properties::BoolProperty* show_all_links_; -}; - -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp new file mode 100644 index 0000000000..19e4c48920 --- /dev/null +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp @@ -0,0 +1,149 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#endif + +namespace rviz_common +{ +namespace properties +{ +class Robot; +class StringProperty; +class BoolProperty; +class FloatProperty; +class RosTopicProperty; +class ColorProperty; +} // namespace properties +} // namespace rviz_common + +namespace moveit_rviz_plugin +{ +class RobotStateVisualization; + +class RobotStateDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + RobotStateDisplay(); + ~RobotStateDisplay() override; + + void load(const rviz_common::Config& config) override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + void setLinkColor(const std::string& link_name, const QColor& color); + void unsetLinkColor(const std::string& link_name); + +public Q_SLOTS: + void setVisible(bool visible); + +private Q_SLOTS: + + // ****************************************************************************************** + // Slot Event Functions + // ****************************************************************************************** + void changedRobotDescription(); + void changedRootLinkName(); + void changedRobotSceneAlpha(); + void changedAttachedBodyColor(); + void changedRobotStateTopic(); + void changedEnableLinkHighlight(); + void changedEnableVisualVisible(); + void changedEnableCollisionVisible(); + void changedAllLinks(); + +protected: + void initializeLoader(); + void loadRobotModel(); + + /** + * \brief Set the scene node's position, given the target frame and the planning frame + */ + void calculateOffsetPosition(); + + void setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name, const QColor& color); + void unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name); + + void newRobotStateCallback(const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state); + + void setRobotHighlights(const moveit_msgs::msg::DisplayRobotState::_highlight_links_type& highlight_links); + void setHighlight(const std::string& link_name, const std_msgs::msg::ColorRGBA& color); + void unsetHighlight(const std::string& link_name); + + // overrides from Display + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void fixedFrameChanged() override; + + // render the robot + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr robot_state_subscriber_; + + RobotStateVisualizationPtr robot_; + rdf_loader::RDFLoaderPtr rdf_loader_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + std::map highlights_; + bool update_state_; + + rviz_common::properties::StringProperty* robot_description_property_; + rviz_common::properties::StringProperty* root_link_name_property_; + rviz_common::properties::RosTopicProperty* robot_state_topic_property_; + rviz_common::properties::FloatProperty* robot_alpha_property_; + rviz_common::properties::ColorProperty* attached_body_color_property_; + rviz_common::properties::BoolProperty* enable_link_highlight_; + rviz_common::properties::BoolProperty* enable_visual_visible_; + rviz_common::properties::BoolProperty* enable_collision_visible_; + rviz_common::properties::BoolProperty* show_all_links_; +}; + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/plugin_init.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/plugin_init.cpp index e26e9b594d..7b07283c54 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/plugin_init.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/plugin_init.cpp @@ -35,6 +35,6 @@ /* Author: Ioan Sucan */ #include -#include +#include CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::RobotStateDisplay, rviz_common::Display) diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index c85f2ed3f1..5cedbd8def 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include // #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index 65d2ce41a4..2a8c81a4dd 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -1,11 +1,11 @@ set(HEADERS - include/moveit/rviz_plugin_render_tools/octomap_render.h - include/moveit/rviz_plugin_render_tools/planning_link_updater.h - include/moveit/rviz_plugin_render_tools/planning_scene_render.h - include/moveit/rviz_plugin_render_tools/render_shapes.h - include/moveit/rviz_plugin_render_tools/robot_state_visualization.h - include/moveit/rviz_plugin_render_tools/trajectory_visualization.h - include/moveit/rviz_plugin_render_tools/trajectory_panel.h + include/moveit/rviz_plugin_render_tools/octomap_render.hpp + include/moveit/rviz_plugin_render_tools/planning_link_updater.hpp + include/moveit/rviz_plugin_render_tools/planning_scene_render.hpp + include/moveit/rviz_plugin_render_tools/render_shapes.hpp + include/moveit/rviz_plugin_render_tools/robot_state_visualization.hpp + include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp + include/moveit/rviz_plugin_render_tools/trajectory_panel.hpp include/ogre_helpers/mesh_shape.hpp) include_directories(${CMAKE_CURRENT_BINARY_DIR}) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h index e7b7efbe45..a9313d9b11 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,58 +47,5 @@ /* Author: Julius Kammerl */ #pragma once - -#include -#include -#include -#include -#include -#include - -namespace octomap -{ -class OcTree; -} - -namespace moveit_rviz_plugin -{ -enum OctreeVoxelRenderMode -{ - OCTOMAP_FREE_VOXELS = 1, - OCTOMAP_OCCUPIED_VOXELS = 2, - OCTOMAP_DISABLED = 3 -}; - -enum OctreeVoxelColorMode -{ - OCTOMAP_Z_AXIS_COLOR, - OCTOMAP_PROBABLILTY_COLOR, -}; - -class OcTreeRender -{ -public: - OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, - OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneNode* parent_node); - virtual ~OcTreeRender(); - - void setPosition(const Ogre::Vector3& position); - void setOrientation(const Ogre::Quaternion& orientation); - -private: - void setColor(double z_pos, double min_z, double max_z, double color_factor, rviz_rendering::PointCloud::Point* point); - void setProbColor(double prob, rviz_rendering::PointCloud::Point* point); - - void octreeDecoding(const std::shared_ptr& octree, - OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode); - - // Ogre-rviz point clouds - std::vector cloud_; - std::shared_ptr octree_; - - Ogre::SceneNode* scene_node_; - - double colorFactor_; - std::size_t octree_depth_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.hpp new file mode 100644 index 0000000000..e7b7efbe45 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.hpp @@ -0,0 +1,92 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Julius Kammerl */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace octomap +{ +class OcTree; +} + +namespace moveit_rviz_plugin +{ +enum OctreeVoxelRenderMode +{ + OCTOMAP_FREE_VOXELS = 1, + OCTOMAP_OCCUPIED_VOXELS = 2, + OCTOMAP_DISABLED = 3 +}; + +enum OctreeVoxelColorMode +{ + OCTOMAP_Z_AXIS_COLOR, + OCTOMAP_PROBABLILTY_COLOR, +}; + +class OcTreeRender +{ +public: + OcTreeRender(const std::shared_ptr& octree, OctreeVoxelRenderMode octree_voxel_rendering, + OctreeVoxelColorMode octree_color_mode, std::size_t max_octree_depth, Ogre::SceneNode* parent_node); + virtual ~OcTreeRender(); + + void setPosition(const Ogre::Vector3& position); + void setOrientation(const Ogre::Quaternion& orientation); + +private: + void setColor(double z_pos, double min_z, double max_z, double color_factor, rviz_rendering::PointCloud::Point* point); + void setProbColor(double prob, rviz_rendering::PointCloud::Point* point); + + void octreeDecoding(const std::shared_ptr& octree, + OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode); + + // Ogre-rviz point clouds + std::vector cloud_; + std::shared_ptr octree_; + + Ogre::SceneNode* scene_node_; + + double colorFactor_; + std::size_t octree_depth_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h index ada06febcb..734cef9f45 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,25 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include - -namespace moveit_rviz_plugin -{ -/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ -class PlanningLinkUpdater : public rviz_default_plugins::robot::LinkUpdater -{ -public: - PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : robot_state_(state) - { - } - - bool getLinkTransforms(const std::string& link_name, Ogre::Vector3& visual_position, - Ogre::Quaternion& visual_orientation, Ogre::Vector3& collision_position, - Ogre::Quaternion& collision_orientation) const override; - -private: - moveit::core::RobotStateConstPtr robot_state_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.hpp new file mode 100644 index 0000000000..95e3e32ea9 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.hpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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 */ + +#pragma once + +#include +#include + +namespace moveit_rviz_plugin +{ +/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ +class PlanningLinkUpdater : public rviz_default_plugins::robot::LinkUpdater +{ +public: + PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : robot_state_(state) + { + } + + bool getLinkTransforms(const std::string& link_name, Ogre::Vector3& visual_position, + Ogre::Quaternion& visual_orientation, Ogre::Vector3& collision_position, + Ogre::Quaternion& collision_orientation) const override; + +private: + moveit::core::RobotStateConstPtr robot_state_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h index 06ab1d8fda..0932818571 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,48 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include - -namespace moveit_rviz_plugin -{ -MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(PlanningSceneRender); // Defines PlanningSceneRenderPtr, ConstPtr, WeakPtr... etc - -class PlanningSceneRender -{ -public: - PlanningSceneRender(Ogre::SceneNode* root_node, rviz_common::DisplayContext* context, - const RobotStateVisualizationPtr& robot); - ~PlanningSceneRender(); - - Ogre::SceneNode* getGeometryNode() - { - return planning_scene_geometry_node_; - } - - const RobotStateVisualizationPtr& getRobotVisualization() - { - return scene_robot_; - } - - void updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene); - - void renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene, - const Ogre::ColourValue& default_scene_color, - const Ogre::ColourValue& default_attached_color, OctreeVoxelRenderMode voxel_render_mode, - OctreeVoxelColorMode voxel_color_mode, double default_scene_alpha); - void clear(); - -private: - Ogre::SceneNode* planning_scene_geometry_node_; - rviz_common::DisplayContext* context_; - RenderShapesPtr render_shapes_; - RobotStateVisualizationPtr scene_robot_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.hpp new file mode 100644 index 0000000000..cffb6d60d1 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_scene_render.hpp @@ -0,0 +1,82 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit_rviz_plugin +{ +MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(PlanningSceneRender); // Defines PlanningSceneRenderPtr, ConstPtr, WeakPtr... etc + +class PlanningSceneRender +{ +public: + PlanningSceneRender(Ogre::SceneNode* root_node, rviz_common::DisplayContext* context, + const RobotStateVisualizationPtr& robot); + ~PlanningSceneRender(); + + Ogre::SceneNode* getGeometryNode() + { + return planning_scene_geometry_node_; + } + + const RobotStateVisualizationPtr& getRobotVisualization() + { + return scene_robot_; + } + + void updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene); + + void renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene, + const Ogre::ColourValue& default_scene_color, + const Ogre::ColourValue& default_attached_color, OctreeVoxelRenderMode voxel_render_mode, + OctreeVoxelColorMode voxel_color_mode, double default_scene_alpha); + void clear(); + +private: + Ogre::SceneNode* planning_scene_geometry_node_; + rviz_common::DisplayContext* context_; + RenderShapesPtr render_shapes_; + RobotStateVisualizationPtr scene_robot_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h index c7d7d3fcaf..6125fb9258 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,38 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace moveit_rviz_plugin -{ -MOVEIT_CLASS_FORWARD(OcTreeRender); // Defines OcTreeRenderPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc - -class RenderShapes -{ -public: - RenderShapes(rviz_common::DisplayContext* context); - ~RenderShapes(); - - void renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p, - OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, - const Ogre::ColourValue& color, double alpha); - void updateShapeColors(double r, double g, double b, double a); - void clear(); - -private: - rviz_common::DisplayContext* context_; - - std::vector > scene_shapes_; - std::vector octree_voxel_grids_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.hpp new file mode 100644 index 0000000000..2604aea953 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/render_shapes.hpp @@ -0,0 +1,72 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_rviz_plugin +{ +MOVEIT_CLASS_FORWARD(OcTreeRender); // Defines OcTreeRenderPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc + +class RenderShapes +{ +public: + RenderShapes(rviz_common::DisplayContext* context); + ~RenderShapes(); + + void renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p, + OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, + const Ogre::ColourValue& color, double alpha); + void updateShapeColors(double r, double g, double b, double a); + void clear(); + +private: + rviz_common::DisplayContext* context_; + + std::vector > scene_shapes_; + std::vector octree_voxel_grids_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h index fae4c85c29..35698767d4 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,80 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -namespace moveit_rviz_plugin -{ -MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc -MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc - -/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ -class RobotStateVisualization -{ -public: - RobotStateVisualization(Ogre::SceneNode* root_node, rviz_common::DisplayContext* context, const std::string& name, - rviz_common::properties::Property* parent_property); - - rviz_default_plugins::robot::Robot& getRobot() - { - return robot_; - } - - void load(const urdf::ModelInterface& descr, bool visual = true, bool collision = true); - void clear(); - - void update(const moveit::core::RobotStateConstPtr& robot_state); - void update(const moveit::core::RobotStateConstPtr& robot_state, - const std_msgs::msg::ColorRGBA& default_attached_object_color); - void update(const moveit::core::RobotStateConstPtr& robot_state, - const std_msgs::msg::ColorRGBA& default_attached_object_color, - const std::map& color_map); - void updateKinematicState(const moveit::core::RobotStateConstPtr& robot_state); - void setDefaultAttachedObjectColor(const std_msgs::msg::ColorRGBA& default_attached_object_color); - /// update color of all attached object shapes - void updateAttachedObjectColors(const std_msgs::msg::ColorRGBA& attached_object_color); - - bool isVisible() const - { - return visible_; - } - - /** - * \brief Set the robot as a whole to be visible or not - * @param visible Should we be visible? - */ - void setVisible(bool visible); - - /** - * \brief Set whether the visual meshes of the robot should be visible - * @param visible Whether the visual meshes of the robot should be visible - */ - void setVisualVisible(bool visible); - - /** - * \brief Set whether the collision meshes/primitives of the robot should be visible - * @param visible Whether the collision meshes/primitives should be visible - */ - void setCollisionVisible(bool visible); - - void setAlpha(double alpha); - -private: - void updateHelper(const moveit::core::RobotStateConstPtr& robot_state, - const std_msgs::msg::ColorRGBA& default_attached_object_color, - const std::map* color_map); - rviz_default_plugins::robot::Robot robot_; - RenderShapesPtr render_shapes_; - std_msgs::msg::ColorRGBA default_attached_object_color_; - OctreeVoxelRenderMode octree_voxel_render_mode_; - OctreeVoxelColorMode octree_voxel_color_mode_; - - bool visible_; - bool visual_visible_; - bool collision_visible_; -}; -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.hpp new file mode 100644 index 0000000000..c8f08a74aa --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.hpp @@ -0,0 +1,114 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit_rviz_plugin +{ +MOVEIT_CLASS_FORWARD(RenderShapes); // Defines RenderShapesPtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(RobotStateVisualization); // Defines RobotStateVisualizationPtr, ConstPtr, WeakPtr... etc + +/** \brief Update the links of an rviz::Robot using a moveit::core::RobotState */ +class RobotStateVisualization +{ +public: + RobotStateVisualization(Ogre::SceneNode* root_node, rviz_common::DisplayContext* context, const std::string& name, + rviz_common::properties::Property* parent_property); + + rviz_default_plugins::robot::Robot& getRobot() + { + return robot_; + } + + void load(const urdf::ModelInterface& descr, bool visual = true, bool collision = true); + void clear(); + + void update(const moveit::core::RobotStateConstPtr& robot_state); + void update(const moveit::core::RobotStateConstPtr& robot_state, + const std_msgs::msg::ColorRGBA& default_attached_object_color); + void update(const moveit::core::RobotStateConstPtr& robot_state, + const std_msgs::msg::ColorRGBA& default_attached_object_color, + const std::map& color_map); + void updateKinematicState(const moveit::core::RobotStateConstPtr& robot_state); + void setDefaultAttachedObjectColor(const std_msgs::msg::ColorRGBA& default_attached_object_color); + /// update color of all attached object shapes + void updateAttachedObjectColors(const std_msgs::msg::ColorRGBA& attached_object_color); + + bool isVisible() const + { + return visible_; + } + + /** + * \brief Set the robot as a whole to be visible or not + * @param visible Should we be visible? + */ + void setVisible(bool visible); + + /** + * \brief Set whether the visual meshes of the robot should be visible + * @param visible Whether the visual meshes of the robot should be visible + */ + void setVisualVisible(bool visible); + + /** + * \brief Set whether the collision meshes/primitives of the robot should be visible + * @param visible Whether the collision meshes/primitives should be visible + */ + void setCollisionVisible(bool visible); + + void setAlpha(double alpha); + +private: + void updateHelper(const moveit::core::RobotStateConstPtr& robot_state, + const std_msgs::msg::ColorRGBA& default_attached_object_color, + const std::map* color_map); + rviz_default_plugins::robot::Robot robot_; + RenderShapesPtr render_shapes_; + std_msgs::msg::ColorRGBA default_attached_object_color_; + OctreeVoxelRenderMode octree_voxel_render_mode_; + OctreeVoxelColorMode octree_voxel_color_mode_; + + bool visible_; + bool visual_visible_; + bool collision_visible_; +}; +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h index 0675d9cc30..ba5b922d5b 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,60 +47,5 @@ /* Author: Yannick Jonetzko */ #pragma once - -#ifndef Q_MOC_RUN -#include -#endif - -#include - -#include -#include -#include - -namespace moveit_rviz_plugin -{ -class TrajectoryPanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - TrajectoryPanel(QWidget* parent = nullptr); - - ~TrajectoryPanel() override; - - void onInitialize() override; - void onEnable(); - void onDisable(); - void update(int way_point_count); - - // Switches between pause and play mode - void pauseButton(bool check); - - void setSliderPosition(int position); - - int getSliderPosition() const - { - return slider_->sliderPosition(); - } - - bool isPaused() const - { - return paused_; - } - -private Q_SLOTS: - void sliderValueChanged(int value); - void buttonClicked(); - -protected: - QSlider* slider_; - QLabel* maximum_label_; - QLabel* minimum_label_; - QPushButton* button_; - - bool paused_; - int last_way_point_; -}; - -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.hpp new file mode 100644 index 0000000000..0675d9cc30 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_panel.hpp @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Yannick Jonetzko + * 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: Yannick Jonetzko */ + +#pragma once + +#ifndef Q_MOC_RUN +#include +#endif + +#include + +#include +#include +#include + +namespace moveit_rviz_plugin +{ +class TrajectoryPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + TrajectoryPanel(QWidget* parent = nullptr); + + ~TrajectoryPanel() override; + + void onInitialize() override; + void onEnable(); + void onDisable(); + void update(int way_point_count); + + // Switches between pause and play mode + void pauseButton(bool check); + + void setSliderPosition(int position); + + int getSliderPosition() const + { + return slider_->sliderPosition(); + } + + bool isPaused() const + { + return paused_; + } + +private Q_SLOTS: + void sliderValueChanged(int value); + void buttonClicked(); + +protected: + QSlider* slider_; + QLabel* maximum_label_; + QLabel* minimum_label_; + QPushButton* button_; + + bool paused_; + int last_way_point_; +}; + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h index 2e11e94c61..a70f2a9f87 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,151 +47,5 @@ /* Author: Dave Coleman */ #pragma once - -#include -#include -#include -#include -#include -#include -#include - -#ifndef Q_MOC_RUN -#include -#include -#include -#include -#include -#include -#include -#endif - -namespace rviz -{ -class Robot; -class Shape; -class Property; -class IntProperty; -class StringProperty; -class BoolProperty; -class FloatProperty; -class RosTopicProperty; -class EditableEnumProperty; -class ColorProperty; -class MovableText; -} // namespace rviz - -namespace moveit_rviz_plugin -{ -MOVEIT_CLASS_FORWARD(TrajectoryVisualization); // Defines TrajectoryVisualizationPtr, ConstPtr, WeakPtr... etc - -class TrajectoryVisualization : public QObject -{ - Q_OBJECT - -public: - /** - * \brief Playback a trajectory from a planned path - * \param widget - either a rviz::Display or rviz::Property - * \param display - the rviz::Display from the parent - * \return true on success - */ - TrajectoryVisualization(rviz_common::properties::Property* widget, rviz_common::Display* display); - - ~TrajectoryVisualization() override; - - virtual void update(double wall_dt, double sim_dt); - virtual void reset(); - - void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node, - rviz_common::DisplayContext* context); - void clearRobotModel(); - void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); - void onEnable(); - void onDisable(); - void setName(const QString& name); - - void dropTrajectory(); - -public Q_SLOTS: - void interruptCurrentDisplay(); - void setDefaultAttachedObjectColor(const QColor& color); - -private Q_SLOTS: - - /** - * \brief Slot Event Functions - */ - void changedDisplayPathVisualEnabled(); - void changedDisplayPathCollisionEnabled(); - void changedRobotPathAlpha(); - void changedLoopDisplay(); - void changedShowTrail(); - void changedTrailStepSize(); - void changedTrajectoryTopic(); - void changedStateDisplayTime(); - void changedRobotColor(); - void enabledRobotColor(); - void trajectorySliderPanelVisibilityChange(bool enable); - -protected: - /** - * \brief ROS callback for an incoming path message - */ - void incomingDisplayTrajectory(const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg); - - /** - * \brief get time to show each single robot state - * \return Positive values indicate a fixed time per state - * Negative values indicate a realtime-factor - */ - double getStateDisplayTime(); - void clearTrajectoryTrail(); - - // Handles actually drawing the robot along motion plans - RobotStateVisualizationPtr display_path_robot_; - std_msgs::msg::ColorRGBA default_attached_object_color_; - - // Handle colouring of robot - void setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color); - void unsetRobotColor(rviz_default_plugins::robot::Robot* robot); - - robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_; - robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_; - std::vector trajectory_trail_; - rclcpp::Subscription::SharedPtr trajectory_topic_sub_; - bool animating_path_; - bool drop_displaying_trajectory_; - int current_state_; - double current_state_time_; - std::mutex update_trajectory_message_; - - moveit::core::RobotModelConstPtr robot_model_; - moveit::core::RobotStatePtr robot_state_; - - // Pointers from parent display that we save - rviz_common::Display* display_; // the parent display that this class populates - rviz_common::properties::Property* widget_; - Ogre::SceneNode* scene_node_; - rviz_common::DisplayContext* context_; - rclcpp::Node::SharedPtr node_; - TrajectoryPanel* trajectory_slider_panel_; - rviz_common::PanelDockWidget* trajectory_slider_dock_panel_; - rclcpp::Logger logger_; - - // Properties - rviz_common::properties::BoolProperty* display_path_visual_enabled_property_; - rviz_common::properties::BoolProperty* display_path_collision_enabled_property_; - rviz_common::properties::EditableEnumProperty* state_display_time_property_; - rviz_common::properties::RosTopicProperty* trajectory_topic_property_; - rviz_common::properties::FloatProperty* robot_path_alpha_property_; - rviz_common::properties::BoolProperty* loop_display_property_; - rviz_common::properties::BoolProperty* use_sim_time_property_; - rviz_common::properties::BoolProperty* trail_display_property_; - rviz_common::properties::BoolProperty* interrupt_display_property_; - rviz_common::properties::ColorProperty* robot_color_property_; - rviz_common::properties::BoolProperty* enable_robot_color_property_; - rviz_common::properties::IntProperty* trail_step_size_property_; -}; - -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp new file mode 100644 index 0000000000..9a52e96e65 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp @@ -0,0 +1,185 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 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: Dave Coleman */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#endif + +namespace rviz +{ +class Robot; +class Shape; +class Property; +class IntProperty; +class StringProperty; +class BoolProperty; +class FloatProperty; +class RosTopicProperty; +class EditableEnumProperty; +class ColorProperty; +class MovableText; +} // namespace rviz + +namespace moveit_rviz_plugin +{ +MOVEIT_CLASS_FORWARD(TrajectoryVisualization); // Defines TrajectoryVisualizationPtr, ConstPtr, WeakPtr... etc + +class TrajectoryVisualization : public QObject +{ + Q_OBJECT + +public: + /** + * \brief Playback a trajectory from a planned path + * \param widget - either a rviz::Display or rviz::Property + * \param display - the rviz::Display from the parent + * \return true on success + */ + TrajectoryVisualization(rviz_common::properties::Property* widget, rviz_common::Display* display); + + ~TrajectoryVisualization() override; + + virtual void update(double wall_dt, double sim_dt); + virtual void reset(); + + void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node, + rviz_common::DisplayContext* context); + void clearRobotModel(); + void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); + void onEnable(); + void onDisable(); + void setName(const QString& name); + + void dropTrajectory(); + +public Q_SLOTS: + void interruptCurrentDisplay(); + void setDefaultAttachedObjectColor(const QColor& color); + +private Q_SLOTS: + + /** + * \brief Slot Event Functions + */ + void changedDisplayPathVisualEnabled(); + void changedDisplayPathCollisionEnabled(); + void changedRobotPathAlpha(); + void changedLoopDisplay(); + void changedShowTrail(); + void changedTrailStepSize(); + void changedTrajectoryTopic(); + void changedStateDisplayTime(); + void changedRobotColor(); + void enabledRobotColor(); + void trajectorySliderPanelVisibilityChange(bool enable); + +protected: + /** + * \brief ROS callback for an incoming path message + */ + void incomingDisplayTrajectory(const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg); + + /** + * \brief get time to show each single robot state + * \return Positive values indicate a fixed time per state + * Negative values indicate a realtime-factor + */ + double getStateDisplayTime(); + void clearTrajectoryTrail(); + + // Handles actually drawing the robot along motion plans + RobotStateVisualizationPtr display_path_robot_; + std_msgs::msg::ColorRGBA default_attached_object_color_; + + // Handle colouring of robot + void setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color); + void unsetRobotColor(rviz_default_plugins::robot::Robot* robot); + + robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_; + robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_; + std::vector trajectory_trail_; + rclcpp::Subscription::SharedPtr trajectory_topic_sub_; + bool animating_path_; + bool drop_displaying_trajectory_; + int current_state_; + double current_state_time_; + std::mutex update_trajectory_message_; + + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + + // Pointers from parent display that we save + rviz_common::Display* display_; // the parent display that this class populates + rviz_common::properties::Property* widget_; + Ogre::SceneNode* scene_node_; + rviz_common::DisplayContext* context_; + rclcpp::Node::SharedPtr node_; + TrajectoryPanel* trajectory_slider_panel_; + rviz_common::PanelDockWidget* trajectory_slider_dock_panel_; + rclcpp::Logger logger_; + + // Properties + rviz_common::properties::BoolProperty* display_path_visual_enabled_property_; + rviz_common::properties::BoolProperty* display_path_collision_enabled_property_; + rviz_common::properties::EditableEnumProperty* state_display_time_property_; + rviz_common::properties::RosTopicProperty* trajectory_topic_property_; + rviz_common::properties::FloatProperty* robot_path_alpha_property_; + rviz_common::properties::BoolProperty* loop_display_property_; + rviz_common::properties::BoolProperty* use_sim_time_property_; + rviz_common::properties::BoolProperty* trail_display_property_; + rviz_common::properties::BoolProperty* interrupt_display_property_; + rviz_common::properties::ColorProperty* robot_color_property_; + rviz_common::properties::BoolProperty* enable_robot_color_property_; + rviz_common::properties::IntProperty* trail_step_size_property_; +}; + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/ogre_helpers/mesh_shape.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/ogre_helpers/mesh_shape.h new file mode 100644 index 0000000000..08bb84afd9 --- /dev/null +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/ogre_helpers/mesh_shape.h @@ -0,0 +1,44 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/* + * Copyright (c) 2008, 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 Willow Garage, Inc. 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. + */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index 393d11bc9e..9ade5f9e7e 100755 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -34,7 +34,7 @@ /* Author: Julius Kammerl */ -#include +#include #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index b01dd7978a..11f0c9f2f1 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::string& link_name, diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 10b12aa76b..6493462198 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index dc340cd4ae..7b93b0b7ce 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -34,8 +34,8 @@ /* Author: Ioan Sucan */ -#include -#include +#include +#include #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 7558fa7f23..a539292100 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_panel.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_panel.cpp index d471818254..6a1872b433 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_panel.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_panel.cpp @@ -34,7 +34,7 @@ /* Author: Yannick Jonetzko */ -#include +#include #include namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index aec921e06e..a60f7cbbe9 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -37,13 +37,13 @@ #include #include -#include +#include -#include -#include +#include +#include #include #include -#include +#include #include #include #include diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index 63cdcf6316..8a58e56eba 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ # Header files that need Qt Moc pre-processing for use with Qt signals, etc: -set(HEADERS include/moveit/trajectory_rviz_plugin/trajectory_display.h) +set(HEADERS include/moveit/trajectory_rviz_plugin/trajectory_display.hpp) include_directories(${CMAKE_CURRENT_BINARY_DIR}) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h index 4d15023463..26cc8d0d88 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -37,65 +49,5 @@ */ #pragma once - -#include -#include -#include - -#include -#ifndef Q_MOC_RUN -#include -#include -#endif - -namespace rviz_common -{ -class DisplayContext; -} - -namespace moveit_rviz_plugin -{ -class TrajectoryDisplay : public rviz_common::Display -{ - Q_OBJECT - // friend class TrajectoryVisualization; // allow the visualization class to access the display - -public: - TrajectoryDisplay(); - - ~TrajectoryDisplay() override; - - void loadRobotModel(); - - void load(const rviz_common::Config& config) override; - void update(float wall_dt, float ros_dt) override; - void reset() override; - - // overrides from Display - void onInitialize() override; - void onEnable() override; - void onDisable() override; - -private Q_SLOTS: - /** - * \brief Slot Event Functions - */ - void changedRobotDescription(); - -protected: - // The trajectory playback component - TrajectoryVisualizationPtr trajectory_visual_; - - // Load robot model - rdf_loader::RDFLoaderPtr rdf_loader_; - moveit::core::RobotModelConstPtr robot_model_; - moveit::core::RobotStatePtr robot_state_; - - // Properties - rviz_common::properties::StringProperty* robot_description_property_; - - rclcpp::Node::SharedPtr node_; - rclcpp::Logger logger_; -}; - -} // namespace moveit_rviz_plugin +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp new file mode 100644 index 0000000000..3949bbb084 --- /dev/null +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder 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: Dave Coleman + Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display +*/ + +#pragma once + +#include +#include +#include + +#include +#ifndef Q_MOC_RUN +#include +#include +#endif + +namespace rviz_common +{ +class DisplayContext; +} + +namespace moveit_rviz_plugin +{ +class TrajectoryDisplay : public rviz_common::Display +{ + Q_OBJECT + // friend class TrajectoryVisualization; // allow the visualization class to access the display + +public: + TrajectoryDisplay(); + + ~TrajectoryDisplay() override; + + void loadRobotModel(); + + void load(const rviz_common::Config& config) override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + // overrides from Display + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + /** + * \brief Slot Event Functions + */ + void changedRobotDescription(); + +protected: + // The trajectory playback component + TrajectoryVisualizationPtr trajectory_visual_; + + // Load robot model + rdf_loader::RDFLoaderPtr rdf_loader_; + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + + // Properties + rviz_common::properties::StringProperty* robot_description_property_; + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; +}; + +} // namespace moveit_rviz_plugin diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/plugin_init.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/plugin_init.cpp index db4d3ab851..31cd848596 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/plugin_init.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/plugin_init.cpp @@ -35,6 +35,6 @@ /* Author: Dave Coleman */ #include -#include +#include CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::TrajectoryDisplay, rviz_common::Display) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index da6d73de0c..78a13c718f 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -36,7 +36,7 @@ Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display */ -#include +#include #include #include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h index 230b1c8555..9e941fc033 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,56 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -#include - -#include - -namespace moveit_warehouse -{ -typedef warehouse_ros::MessageWithMetadata::ConstPtr ConstraintsWithMetadata; -typedef warehouse_ros::MessageCollection::Ptr ConstraintsCollection; - -MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc - -class MOVEIT_WAREHOUSE_EXPORT ConstraintsStorage : public MoveItMessageStorage -{ -public: - static const std::string DATABASE_NAME; - - static const std::string CONSTRAINTS_ID_NAME; - static const std::string CONSTRAINTS_GROUP_NAME; - static const std::string ROBOT_NAME; - - ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); - - void addConstraints(const moveit_msgs::msg::Constraints& msg, const std::string& robot = "", - const std::string& group = ""); - bool hasConstraints(const std::string& name, const std::string& robot = "", const std::string& group = "") const; - void getKnownConstraints(std::vector& names, const std::string& robot = "", - const std::string& group = "") const; - void getKnownConstraints(const std::string& regex, std::vector& names, const std::string& robot = "", - const std::string& group = "") const; - - /** \brief Get the constraints named \e name. Return false on failure. */ - bool getConstraints(ConstraintsWithMetadata& msg_m, const std::string& name, const std::string& robot = "", - const std::string& group = "") const; - - void renameConstraints(const std::string& old_name, const std::string& new_name, const std::string& robot = "", - const std::string& group = ""); - - void removeConstraints(const std::string& name, const std::string& robot = "", const std::string& group = ""); - - void reset(); - -private: - void createCollections(); - - ConstraintsCollection constraints_collection_; - rclcpp::Logger logger_; -}; -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.hpp b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.hpp new file mode 100644 index 0000000000..44558f5c12 --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/constraints_storage.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +#include + +#include + +namespace moveit_warehouse +{ +typedef warehouse_ros::MessageWithMetadata::ConstPtr ConstraintsWithMetadata; +typedef warehouse_ros::MessageCollection::Ptr ConstraintsCollection; + +MOVEIT_CLASS_FORWARD(ConstraintsStorage); // Defines ConstraintsStoragePtr, ConstPtr, WeakPtr... etc + +class MOVEIT_WAREHOUSE_EXPORT ConstraintsStorage : public MoveItMessageStorage +{ +public: + static const std::string DATABASE_NAME; + + static const std::string CONSTRAINTS_ID_NAME; + static const std::string CONSTRAINTS_GROUP_NAME; + static const std::string ROBOT_NAME; + + ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); + + void addConstraints(const moveit_msgs::msg::Constraints& msg, const std::string& robot = "", + const std::string& group = ""); + bool hasConstraints(const std::string& name, const std::string& robot = "", const std::string& group = "") const; + void getKnownConstraints(std::vector& names, const std::string& robot = "", + const std::string& group = "") const; + void getKnownConstraints(const std::string& regex, std::vector& names, const std::string& robot = "", + const std::string& group = "") const; + + /** \brief Get the constraints named \e name. Return false on failure. */ + bool getConstraints(ConstraintsWithMetadata& msg_m, const std::string& name, const std::string& robot = "", + const std::string& group = "") const; + + void renameConstraints(const std::string& old_name, const std::string& new_name, const std::string& robot = "", + const std::string& group = ""); + + void removeConstraints(const std::string& name, const std::string& robot = "", const std::string& group = ""); + + void reset(); + +private: + void createCollections(); + + ConstraintsCollection constraints_collection_; + rclcpp::Logger logger_; +}; +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.h index dd73ffa285..16aa781fca 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,33 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace moveit_warehouse -{ -/** \brief This class provides the mechanism to connect to a database and reads needed ROS parameters when appropriate. - */ -class MoveItMessageStorage -{ -public: - /// \brief Takes a warehouse_ros DatabaseConnection. The DatabaseConnection is expected to have already been - /// initialized. - MoveItMessageStorage(warehouse_ros::DatabaseConnection::Ptr conn); - - virtual ~MoveItMessageStorage() - { - } - -protected: - /// Keep only the \e names that match \e regex - void filterNames(const std::string& regex, std::vector& names) const; - - warehouse_ros::DatabaseConnection::Ptr conn_; -}; - -/// \brief Load a database connection -typename warehouse_ros::DatabaseConnection::Ptr loadDatabase(const rclcpp::Node::SharedPtr& node); -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.hpp b/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.hpp new file mode 100644 index 0000000000..dd73ffa285 --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/moveit_message_storage.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +namespace moveit_warehouse +{ +/** \brief This class provides the mechanism to connect to a database and reads needed ROS parameters when appropriate. + */ +class MoveItMessageStorage +{ +public: + /// \brief Takes a warehouse_ros DatabaseConnection. The DatabaseConnection is expected to have already been + /// initialized. + MoveItMessageStorage(warehouse_ros::DatabaseConnection::Ptr conn); + + virtual ~MoveItMessageStorage() + { + } + +protected: + /// Keep only the \e names that match \e regex + void filterNames(const std::string& regex, std::vector& names) const; + + warehouse_ros::DatabaseConnection::Ptr conn_; +}; + +/// \brief Load a database connection +typename warehouse_ros::DatabaseConnection::Ptr loadDatabase(const rclcpp::Node::SharedPtr& node); +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h index 8139c00ea8..2308cd5aab 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,91 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include -#include -#include - -#include - -namespace moveit_warehouse -{ -typedef warehouse_ros::MessageWithMetadata::ConstPtr PlanningSceneWithMetadata; -typedef warehouse_ros::MessageWithMetadata::ConstPtr MotionPlanRequestWithMetadata; -typedef warehouse_ros::MessageWithMetadata::ConstPtr RobotTrajectoryWithMetadata; - -typedef warehouse_ros::MessageCollection::Ptr PlanningSceneCollection; -typedef warehouse_ros::MessageCollection::Ptr MotionPlanRequestCollection; -typedef warehouse_ros::MessageCollection::Ptr RobotTrajectoryCollection; - -MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc - -class MOVEIT_WAREHOUSE_EXPORT PlanningSceneStorage : public MoveItMessageStorage -{ -public: - static const std::string DATABASE_NAME; - - static const std::string PLANNING_SCENE_ID_NAME; - static const std::string MOTION_PLAN_REQUEST_ID_NAME; - - PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn); - - void addPlanningScene(const moveit_msgs::msg::PlanningScene& scene); - void addPlanningQuery(const moveit_msgs::msg::MotionPlanRequest& planning_query, const std::string& scene_name, - const std::string& query_name = ""); - void addPlanningResult(const moveit_msgs::msg::MotionPlanRequest& planning_query, - const moveit_msgs::msg::RobotTrajectory& result, const std::string& scene_name); - - bool hasPlanningScene(const std::string& name) const; - void getPlanningSceneNames(std::vector& names) const; - void getPlanningSceneNames(const std::string& regex, std::vector& names) const; - - /** \brief Get the latest planning scene named \e scene_name */ - bool getPlanningScene(PlanningSceneWithMetadata& scene_m, const std::string& scene_name) const; - bool getPlanningSceneWorld(moveit_msgs::msg::PlanningSceneWorld& world, const std::string& scene_name) const; - - bool hasPlanningQuery(const std::string& scene_name, const std::string& query_name) const; - bool getPlanningQuery(MotionPlanRequestWithMetadata& query_m, const std::string& scene_name, - const std::string& query_name); - void getPlanningQueries(std::vector& planning_queries, - const std::string& scene_name) const; - void getPlanningQueriesNames(std::vector& query_names, const std::string& scene_name) const; - void getPlanningQueriesNames(const std::string& regex, std::vector& query_names, - const std::string& scene_name) const; - void getPlanningQueries(std::vector& planning_queries, - std::vector& query_names, const std::string& scene_name) const; - - void getPlanningResults(std::vector& planning_results, const std::string& scene_name, - const moveit_msgs::msg::MotionPlanRequest& planning_query) const; - void getPlanningResults(std::vector& planning_results, const std::string& scene_name, - const std::string& query_name) const; - - void renamePlanningScene(const std::string& old_scene_name, const std::string& new_scene_name); - void renamePlanningQuery(const std::string& scene_name, const std::string& old_query_name, - const std::string& new_query_name); - - void removePlanningScene(const std::string& scene_name); - void removePlanningQuery(const std::string& scene_name, const std::string& query_name); - void removePlanningQueries(const std::string& scene_name); - void removePlanningResults(const std::string& scene_name); - void removePlanningResults(const std::string& scene_name, const std::string& query_name); - - void reset(); - -private: - void createCollections(); - - std::string getMotionPlanRequestName(const moveit_msgs::msg::MotionPlanRequest& planning_query, - const std::string& scene_name) const; - std::string addNewPlanningRequest(const moveit_msgs::msg::MotionPlanRequest& planning_query, - const std::string& scene_name, const std::string& query_name); - - PlanningSceneCollection planning_scene_collection_; - MotionPlanRequestCollection motion_plan_request_collection_; - RobotTrajectoryCollection robot_trajectory_collection_; - rclcpp::Logger logger_; -}; -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.hpp b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.hpp new file mode 100644 index 0000000000..11468defd3 --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_storage.hpp @@ -0,0 +1,125 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace moveit_warehouse +{ +typedef warehouse_ros::MessageWithMetadata::ConstPtr PlanningSceneWithMetadata; +typedef warehouse_ros::MessageWithMetadata::ConstPtr MotionPlanRequestWithMetadata; +typedef warehouse_ros::MessageWithMetadata::ConstPtr RobotTrajectoryWithMetadata; + +typedef warehouse_ros::MessageCollection::Ptr PlanningSceneCollection; +typedef warehouse_ros::MessageCollection::Ptr MotionPlanRequestCollection; +typedef warehouse_ros::MessageCollection::Ptr RobotTrajectoryCollection; + +MOVEIT_CLASS_FORWARD(PlanningSceneStorage); // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc + +class MOVEIT_WAREHOUSE_EXPORT PlanningSceneStorage : public MoveItMessageStorage +{ +public: + static const std::string DATABASE_NAME; + + static const std::string PLANNING_SCENE_ID_NAME; + static const std::string MOTION_PLAN_REQUEST_ID_NAME; + + PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn); + + void addPlanningScene(const moveit_msgs::msg::PlanningScene& scene); + void addPlanningQuery(const moveit_msgs::msg::MotionPlanRequest& planning_query, const std::string& scene_name, + const std::string& query_name = ""); + void addPlanningResult(const moveit_msgs::msg::MotionPlanRequest& planning_query, + const moveit_msgs::msg::RobotTrajectory& result, const std::string& scene_name); + + bool hasPlanningScene(const std::string& name) const; + void getPlanningSceneNames(std::vector& names) const; + void getPlanningSceneNames(const std::string& regex, std::vector& names) const; + + /** \brief Get the latest planning scene named \e scene_name */ + bool getPlanningScene(PlanningSceneWithMetadata& scene_m, const std::string& scene_name) const; + bool getPlanningSceneWorld(moveit_msgs::msg::PlanningSceneWorld& world, const std::string& scene_name) const; + + bool hasPlanningQuery(const std::string& scene_name, const std::string& query_name) const; + bool getPlanningQuery(MotionPlanRequestWithMetadata& query_m, const std::string& scene_name, + const std::string& query_name); + void getPlanningQueries(std::vector& planning_queries, + const std::string& scene_name) const; + void getPlanningQueriesNames(std::vector& query_names, const std::string& scene_name) const; + void getPlanningQueriesNames(const std::string& regex, std::vector& query_names, + const std::string& scene_name) const; + void getPlanningQueries(std::vector& planning_queries, + std::vector& query_names, const std::string& scene_name) const; + + void getPlanningResults(std::vector& planning_results, const std::string& scene_name, + const moveit_msgs::msg::MotionPlanRequest& planning_query) const; + void getPlanningResults(std::vector& planning_results, const std::string& scene_name, + const std::string& query_name) const; + + void renamePlanningScene(const std::string& old_scene_name, const std::string& new_scene_name); + void renamePlanningQuery(const std::string& scene_name, const std::string& old_query_name, + const std::string& new_query_name); + + void removePlanningScene(const std::string& scene_name); + void removePlanningQuery(const std::string& scene_name, const std::string& query_name); + void removePlanningQueries(const std::string& scene_name); + void removePlanningResults(const std::string& scene_name); + void removePlanningResults(const std::string& scene_name, const std::string& query_name); + + void reset(); + +private: + void createCollections(); + + std::string getMotionPlanRequestName(const moveit_msgs::msg::MotionPlanRequest& planning_query, + const std::string& scene_name) const; + std::string addNewPlanningRequest(const moveit_msgs::msg::MotionPlanRequest& planning_query, + const std::string& scene_name, const std::string& query_name); + + PlanningSceneCollection planning_scene_collection_; + MotionPlanRequestCollection motion_plan_request_collection_; + RobotTrajectoryCollection robot_trajectory_collection_; + rclcpp::Logger logger_; +}; +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h index 4c17459656..59de13d41c 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,42 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include - -namespace moveit_warehouse -{ -typedef warehouse_ros::MessageWithMetadata::ConstPtr PlanningSceneWorldWithMetadata; -typedef warehouse_ros::MessageCollection::Ptr PlanningSceneWorldCollection; - -class PlanningSceneWorldStorage : public MoveItMessageStorage -{ -public: - static const std::string DATABASE_NAME; - static const std::string PLANNING_SCENE_WORLD_ID_NAME; - - PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn); - - void addPlanningSceneWorld(const moveit_msgs::msg::PlanningSceneWorld& msg, const std::string& name); - bool hasPlanningSceneWorld(const std::string& name) const; - void getKnownPlanningSceneWorlds(std::vector& names) const; - void getKnownPlanningSceneWorlds(const std::string& regex, std::vector& names) const; - - /** \brief Get the constraints named \e name. Return false on failure. */ - bool getPlanningSceneWorld(PlanningSceneWorldWithMetadata& msg_m, const std::string& name) const; - - void renamePlanningSceneWorld(const std::string& old_name, const std::string& new_name); - - void removePlanningSceneWorld(const std::string& name); - - void reset(); - -private: - void createCollections(); - - PlanningSceneWorldCollection planning_scene_world_collection_; - rclcpp::Logger logger_; -}; -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.hpp b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.hpp new file mode 100644 index 0000000000..ad5c207e5d --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/planning_scene_world_storage.hpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include + +namespace moveit_warehouse +{ +typedef warehouse_ros::MessageWithMetadata::ConstPtr PlanningSceneWorldWithMetadata; +typedef warehouse_ros::MessageCollection::Ptr PlanningSceneWorldCollection; + +class PlanningSceneWorldStorage : public MoveItMessageStorage +{ +public: + static const std::string DATABASE_NAME; + static const std::string PLANNING_SCENE_WORLD_ID_NAME; + + PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn); + + void addPlanningSceneWorld(const moveit_msgs::msg::PlanningSceneWorld& msg, const std::string& name); + bool hasPlanningSceneWorld(const std::string& name) const; + void getKnownPlanningSceneWorlds(std::vector& names) const; + void getKnownPlanningSceneWorlds(const std::string& regex, std::vector& names) const; + + /** \brief Get the constraints named \e name. Return false on failure. */ + bool getPlanningSceneWorld(PlanningSceneWorldWithMetadata& msg_m, const std::string& name) const; + + void renamePlanningSceneWorld(const std::string& old_name, const std::string& new_name); + + void removePlanningSceneWorld(const std::string& name); + + void reset(); + +private: + void createCollections(); + + PlanningSceneWorldCollection planning_scene_world_collection_; + rclcpp::Logger logger_; +}; +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h index 58a8c91a70..081edf5299 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/state_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,50 +47,5 @@ /* Author: Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -#include - -namespace moveit_warehouse -{ -typedef warehouse_ros::MessageWithMetadata::ConstPtr RobotStateWithMetadata; -typedef warehouse_ros::MessageCollection::Ptr RobotStateCollection; - -MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc - -class MOVEIT_WAREHOUSE_EXPORT RobotStateStorage : public MoveItMessageStorage -{ -public: - static const std::string DATABASE_NAME; - - static const std::string STATE_NAME; - static const std::string ROBOT_NAME; - - RobotStateStorage(warehouse_ros::DatabaseConnection::Ptr conn); - - void addRobotState(const moveit_msgs::msg::RobotState& msg, const std::string& name, const std::string& robot = ""); - bool hasRobotState(const std::string& name, const std::string& robot = "") const; - void getKnownRobotStates(std::vector& names, const std::string& robot = "") const; - void getKnownRobotStates(const std::string& regex, std::vector& names, - const std::string& robot = "") const; - - /** \brief Get the constraints named \e name. Return false on failure. */ - bool getRobotState(RobotStateWithMetadata& msg_m, const std::string& name, const std::string& robot = "") const; - - void renameRobotState(const std::string& old_name, const std::string& new_name, const std::string& robot = ""); - - void removeRobotState(const std::string& name, const std::string& robot = ""); - - void reset(); - -private: - void createCollections(); - - RobotStateCollection state_collection_; - rclcpp::Logger logger_; -}; -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/state_storage.hpp b/moveit_ros/warehouse/include/moveit/warehouse/state_storage.hpp new file mode 100644 index 0000000000..d8b0d1d03c --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/state_storage.hpp @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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 */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace moveit_warehouse +{ +typedef warehouse_ros::MessageWithMetadata::ConstPtr RobotStateWithMetadata; +typedef warehouse_ros::MessageCollection::Ptr RobotStateCollection; + +MOVEIT_CLASS_FORWARD(RobotStateStorage); // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc + +class MOVEIT_WAREHOUSE_EXPORT RobotStateStorage : public MoveItMessageStorage +{ +public: + static const std::string DATABASE_NAME; + + static const std::string STATE_NAME; + static const std::string ROBOT_NAME; + + RobotStateStorage(warehouse_ros::DatabaseConnection::Ptr conn); + + void addRobotState(const moveit_msgs::msg::RobotState& msg, const std::string& name, const std::string& robot = ""); + bool hasRobotState(const std::string& name, const std::string& robot = "") const; + void getKnownRobotStates(std::vector& names, const std::string& robot = "") const; + void getKnownRobotStates(const std::string& regex, std::vector& names, + const std::string& robot = "") const; + + /** \brief Get the constraints named \e name. Return false on failure. */ + bool getRobotState(RobotStateWithMetadata& msg_m, const std::string& name, const std::string& robot = "") const; + + void renameRobotState(const std::string& old_name, const std::string& new_name, const std::string& robot = ""); + + void removeRobotState(const std::string& name, const std::string& robot = ""); + + void reset(); + +private: + void createCollections(); + + RobotStateCollection state_collection_; + rclcpp::Logger logger_; +}; +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h b/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h index e729fac14c..626018eb3b 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,56 +47,5 @@ /* Author: Mario Prats, Ioan Sucan */ #pragma once - -#include -#include -#include -#include - -namespace moveit_warehouse -{ -typedef warehouse_ros::MessageWithMetadata::ConstPtr - TrajectoryConstraintsWithMetadata; -typedef warehouse_ros::MessageCollection::Ptr TrajectoryConstraintsCollection; - -MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage); // Defines TrajectoryConstraintsStoragePtr, ConstPtr, WeakPtr... etc - -class TrajectoryConstraintsStorage : public MoveItMessageStorage -{ -public: - static const std::string DATABASE_NAME; - - static const std::string CONSTRAINTS_ID_NAME; - static const std::string CONSTRAINTS_GROUP_NAME; - static const std::string ROBOT_NAME; - - TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); - - void addTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& msg, const std::string& name, - const std::string& robot = "", const std::string& group = ""); - bool hasTrajectoryConstraints(const std::string& name, const std::string& robot = "", - const std::string& group = "") const; - void getKnownTrajectoryConstraints(std::vector& names, const std::string& robot = "", - const std::string& group = "") const; - void getKnownTrajectoryConstraints(const std::string& regex, std::vector& names, - const std::string& robot = "", const std::string& group = "") const; - - /** \brief Get the constraints named \e name. Return false on failure. */ - bool getTrajectoryConstraints(TrajectoryConstraintsWithMetadata& msg_m, const std::string& name, - const std::string& robot = "", const std::string& group = "") const; - - void renameTrajectoryConstraints(const std::string& old_name, const std::string& new_name, - const std::string& robot = "", const std::string& group = ""); - - void removeTrajectoryConstraints(const std::string& name, const std::string& robot = "", - const std::string& group = ""); - - void reset(); - -private: - void createCollections(); - - TrajectoryConstraintsCollection constraints_collection_; - rclcpp::Logger logger_; -}; -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.hpp b/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.hpp new file mode 100644 index 0000000000..dac9f36baa --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/trajectory_constraints_storage.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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: Mario Prats, Ioan Sucan */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit_warehouse +{ +typedef warehouse_ros::MessageWithMetadata::ConstPtr + TrajectoryConstraintsWithMetadata; +typedef warehouse_ros::MessageCollection::Ptr TrajectoryConstraintsCollection; + +MOVEIT_CLASS_FORWARD(TrajectoryConstraintsStorage); // Defines TrajectoryConstraintsStoragePtr, ConstPtr, WeakPtr... etc + +class TrajectoryConstraintsStorage : public MoveItMessageStorage +{ +public: + static const std::string DATABASE_NAME; + + static const std::string CONSTRAINTS_ID_NAME; + static const std::string CONSTRAINTS_GROUP_NAME; + static const std::string ROBOT_NAME; + + TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); + + void addTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& msg, const std::string& name, + const std::string& robot = "", const std::string& group = ""); + bool hasTrajectoryConstraints(const std::string& name, const std::string& robot = "", + const std::string& group = "") const; + void getKnownTrajectoryConstraints(std::vector& names, const std::string& robot = "", + const std::string& group = "") const; + void getKnownTrajectoryConstraints(const std::string& regex, std::vector& names, + const std::string& robot = "", const std::string& group = "") const; + + /** \brief Get the constraints named \e name. Return false on failure. */ + bool getTrajectoryConstraints(TrajectoryConstraintsWithMetadata& msg_m, const std::string& name, + const std::string& robot = "", const std::string& group = "") const; + + void renameTrajectoryConstraints(const std::string& old_name, const std::string& new_name, + const std::string& robot = "", const std::string& group = ""); + + void removeTrajectoryConstraints(const std::string& name, const std::string& robot = "", + const std::string& group = ""); + + void reset(); + +private: + void createCollections(); + + TrajectoryConstraintsCollection constraints_collection_; + rclcpp::Logger logger_; +}; +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h index 04e279a3ca..14cf5ee79f 100644 --- a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h +++ b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.h @@ -1,3 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ /********************************************************************* * Software License Agreement (BSD License) * @@ -35,22 +47,5 @@ /* Author: E. Gil Jones */ #pragma once - -#include - -namespace moveit_warehouse -{ -class WarehouseConnector -{ -public: - WarehouseConnector(const std::string& dbexec); - - ~WarehouseConnector(); - - bool connectToDatabase(const std::string& db_dirname); - -private: - std::string dbexec_; - int child_pid_; -}; -} // namespace moveit_warehouse +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.hpp b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.hpp new file mode 100644 index 0000000000..04e279a3ca --- /dev/null +++ b/moveit_ros/warehouse/include/moveit/warehouse/warehouse_connector.hpp @@ -0,0 +1,56 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: E. Gil Jones */ + +#pragma once + +#include + +namespace moveit_warehouse +{ +class WarehouseConnector +{ +public: + WarehouseConnector(const std::string& dbexec); + + ~WarehouseConnector(); + + bool connectToDatabase(const std::string& db_dirname); + +private: + std::string dbexec_; + int child_pid_; +}; +} // namespace moveit_warehouse diff --git a/moveit_ros/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/src/broadcast.cpp index 7e17f6db14..a070e10be6 100644 --- a/moveit_ros/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/src/broadcast.cpp @@ -34,9 +34,9 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/constraints_storage.cpp b/moveit_ros/warehouse/src/constraints_storage.cpp index 9ec2b63b13..f38df9f067 100644 --- a/moveit_ros/warehouse/src/constraints_storage.cpp +++ b/moveit_ros/warehouse/src/constraints_storage.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index 6fa876220b..a33d2b328a 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -34,12 +34,12 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/src/initialize_demo_db.cpp index 855714dcab..af3d047373 100644 --- a/moveit_ros/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/src/initialize_demo_db.cpp @@ -35,12 +35,12 @@ /* Author: Ioan Sucan */ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/src/moveit_message_storage.cpp index 3eb87f1352..d50c91b795 100644 --- a/moveit_ros/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/src/moveit_message_storage.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index 5e861bd6eb..af622f8c9d 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp index f245d9b9f1..168e30011a 100644 --- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/warehouse/src/save_as_text.cpp b/moveit_ros/warehouse/src/save_as_text.cpp index 72592f9e7c..5fbe0433aa 100644 --- a/moveit_ros/warehouse/src/save_as_text.cpp +++ b/moveit_ros/warehouse/src/save_as_text.cpp @@ -34,15 +34,15 @@ /* Author: Ioan Sucan */ -#include -#include -#include +#include +#include +#include #include #include #include #include -#include -#include +#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 2e478afa38..b837cec649 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/state_storage.cpp b/moveit_ros/warehouse/src/state_storage.cpp index 965aba9ccf..9eadb02fc8 100644 --- a/moveit_ros/warehouse/src/state_storage.cpp +++ b/moveit_ros/warehouse/src/state_storage.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp index b60c9233f8..dde199b3da 100644 --- a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp @@ -34,7 +34,7 @@ /* Author: Mario Prats, Ioan Sucan */ -#include +#include #include #include diff --git a/moveit_ros/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/src/warehouse_connector.cpp index d297652840..14b5ea70f8 100644 --- a/moveit_ros/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/src/warehouse_connector.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include #include #include diff --git a/moveit_ros/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/src/warehouse_services.cpp index cae2de5823..e735c19425 100644 --- a/moveit_ros/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/src/warehouse_services.cpp @@ -34,7 +34,7 @@ /* Author: Dan Greenwald */ -#include +#include #include #include #include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.h new file mode 100644 index 0000000000..a5fb6815a9 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches.h new file mode 100644 index 0000000000..b7896b7018 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_config.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_config.h new file mode 100644 index 0000000000..d8d9e896cc --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_config.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_widget.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_widget.h new file mode 100644 index 0000000000..90eab15d77 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launches_widget.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.h new file mode 100644 index 0000000000..801740977a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.h new file mode 100644 index 0000000000..a208e2bedc --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.h b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.h new file mode 100644 index 0000000000..5f7f43d729 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_widget.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Mohamad Ayman. + * 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. + * * The name of Mohamad Ayman may not 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: Mohamad Ayman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt index e7c69dcc92..6dc113eab1 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt @@ -66,6 +66,7 @@ install( INCLUDES DESTINATION include/moveit_setup_assistant) install(DIRECTORY include/ DESTINATION include/moveit_setup_assistant) + ament_export_include_directories(include) install(DIRECTORY launch DESTINATION share/moveit_setup_assistant) install(DIRECTORY resources DESTINATION share/moveit_setup_assistant) diff --git a/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.h b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.h new file mode 100644 index 0000000000..2bb5b430ca --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/setup_assistant_widget.h b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/setup_assistant_widget.h new file mode 100644 index 0000000000..c57e6ab446 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/setup_assistant_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp index d6c0eeb245..d8963e1be1 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/control_xacro_config.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/control_xacro_config.h new file mode 100644 index 0000000000..e2b5884081 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/control_xacro_config.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controller_edit_widget.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controller_edit_widget.h new file mode 100644 index 0000000000..2720e4a71a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controller_edit_widget.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Mohamad Ayman. + * 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. + * * The name of Mohamad Ayman 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: Mohamad Ayman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.h new file mode 100644 index 0000000000..c44ac245d8 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_config.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_config.h new file mode 100644 index 0000000000..4314e776c4 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_config.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_widget.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_widget.h new file mode 100644 index 0000000000..a46510f27e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/controllers_widget.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Mohamad Ayman. + * 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. + * * The name of Mohamad Ayman 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: Mohamad Ayman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/included_xacro_config.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/included_xacro_config.h new file mode 100644 index 0000000000..7f265af80f --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/included_xacro_config.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/modified_urdf_config.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/modified_urdf_config.h new file mode 100644 index 0000000000..810bb80994 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/modified_urdf_config.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.h new file mode 100644 index 0000000000..1826c27f31 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.h new file mode 100644 index 0000000000..2304ca4a8c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.h new file mode 100644 index 0000000000..b08da4f4a2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers_config.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers_config.h new file mode 100644 index 0000000000..d8034f1560 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers_config.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications.h new file mode 100644 index 0000000000..e392ddf966 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.h b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.h new file mode 100644 index 0000000000..462daca3f3 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp index 9580de2b3c..770f1725d5 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include namespace moveit_setup { diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information.h new file mode 100644 index 0000000000..680837837c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information_widget.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information_widget.h new file mode 100644 index 0000000000..9984b5e7bc --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/author_information_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman, Michael 'v4hn' Goerner */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.h new file mode 100644 index 0000000000..539f4ce78a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.h new file mode 100644 index 0000000000..cf8bfb6f59 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.h new file mode 100644 index 0000000000..4df57556a7 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.h b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.h new file mode 100644 index 0000000000..244a5e8af9 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/start_screen_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.h new file mode 100644 index 0000000000..67168733d2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp index c3b444e205..4e066efdd9 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include #include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.h new file mode 100644 index 0000000000..f928a8b61b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, 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 PickNik Robotics 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. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.h new file mode 100644 index 0000000000..a7fa8bfae1 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp index c5ad29cbf0..92508e0251 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp @@ -38,9 +38,9 @@ #include #include #include -#include -#include // for getting kinematic model -#include // for writing srdf data +#include +#include // for getting kinematic model +#include // for writing srdf data namespace moveit_setup { diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.h new file mode 100644 index 0000000000..d3b01f4f9a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/urdf_config.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, 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 PickNik Robotics 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. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.h new file mode 100644 index 0000000000..8a6cf28bed --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.h @@ -0,0 +1,57 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#include +#include +#include +#include +#include + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp index 5c2c110dc9..e51521d02b 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data_warehouse.hpp @@ -35,7 +35,7 @@ /* Author: David V. Lu!! */ #include -#include +#include #include #include #include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.h new file mode 100644 index 0000000000..986a6f483d --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp index 4f3288ead4..d476bcd38f 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_file.hpp @@ -37,7 +37,7 @@ #pragma once #include #include -#include +#include #include #include #include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_time.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_time.h new file mode 100644 index 0000000000..ccb3e54516 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/generated_time.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.h new file mode 100644 index 0000000000..38eda09fd8 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/helper_widgets.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/helper_widgets.h new file mode 100644 index 0000000000..78463ebfe9 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/helper_widgets.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.h new file mode 100644 index 0000000000..d79e7ed04c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp index e455d336ac..863a06b46b 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/rviz_panel.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/setup_step_widget.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/setup_step_widget.h new file mode 100644 index 0000000000..253070b421 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/setup_step_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.h new file mode 100644 index 0000000000..5b13e57925 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/xml_syntax_highlighter.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Bielefeld University, 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 Bielefeld University 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: Robert Haschke */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.h new file mode 100644 index 0000000000..deb5540752 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/setup_step.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.h new file mode 100644 index 0000000000..14134d5a81 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics, 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/testing_utils.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/testing_utils.h new file mode 100644 index 0000000000..996541d626 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/testing_utils.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.h b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.h new file mode 100644 index 0000000000..a9f5ae9cfd --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp index cfd7682978..d19bdbfa50 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include namespace moveit_setup { diff --git a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp index 2480848908..72d9bf4af8 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation.h b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation.h new file mode 100644 index 0000000000..c5041b864e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation_widget.h b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation_widget.h new file mode 100644 index 0000000000..5dd183e652 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_simulation/include/moveit_setup_simulation/simulation_widget.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Mohamad Ayman. + * 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. + * * The name of Mohamad Ayman may not 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: Mohamad Ayman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp b/moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp index 51be95cf18..16920693cb 100644 --- a/moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_simulation/src/simulation_widget.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.h new file mode 100644 index 0000000000..271fe9aa0e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_linear_model.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, CITEC, Bielefeld University + * 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: Robert Haschke */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.h new file mode 100644 index 0000000000..1cbab31495 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/collision_matrix_model.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, CITEC, Bielefeld University + * 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: Robert Haschke */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.h new file mode 100644 index 0000000000..3a1a8e1d2e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp index 846b99cc5c..19f18e81b5 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/compute_default_collisions.hpp @@ -37,7 +37,7 @@ #pragma once #include -#include +#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); // Defines PlanningScenePtr, ConstPtr, WeakPtr... etc diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.h new file mode 100644 index 0000000000..55120b9bc2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.h new file mode 100644 index 0000000000..0493af75e8 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.h new file mode 100644 index 0000000000..a080097be7 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.h new file mode 100644 index 0000000000..56b8500aca --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/end_effectors_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.h new file mode 100644 index 0000000000..0f1c1a8cdd --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_edit_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.h new file mode 100644 index 0000000000..c9b2ca1583 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/group_meta_config.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.h new file mode 100644 index 0000000000..99c29f127c --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/kinematic_chain_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.h new file mode 100644 index 0000000000..5167d2b36e --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.h new file mode 100644 index 0000000000..3226c84d32 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/passive_joints_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.h new file mode 100644 index 0000000000..437e8d162b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp index f62880ae94..52dc4c8e8f 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups.hpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include namespace moveit_setup diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.h new file mode 100644 index 0000000000..a1b06c2967 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/planning_groups_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.h new file mode 100644 index 0000000000..7e8cfd8d4a --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.h new file mode 100644 index 0000000000..f5e360af89 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/robot_poses_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/rotated_header_view.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/rotated_header_view.h new file mode 100644 index 0000000000..cd9bfe3479 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/rotated_header_view.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, CITEC, Bielefeld University + * 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: Robert Haschke */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.h new file mode 100644 index 0000000000..f765ad4778 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/srdf_step.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Metro Robots + * 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 Metro Robots 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.h new file mode 100644 index 0000000000..4ae077923b --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints.h @@ -0,0 +1,50 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik Robotics + * 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 PickNik Robotics 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: David V. Lu!! */ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints_widget.h b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints_widget.h new file mode 100644 index 0000000000..1a00e505b2 --- /dev/null +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/virtual_joints_widget.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, 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: Dave Coleman */ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index b45620a885..b481ce6a6c 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -34,7 +34,7 @@ /* Author: Dave Coleman */ -#include +#include #include #include // for statistics at end #include diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp index 713cdd1fbb..1010a4d8e0 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups.cpp @@ -35,7 +35,7 @@ /* Author: David V. Lu!! */ #include -#include +#include #include // for loading all avail kinematic planners //// Cycle checking diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp index 9e9529fbbd..2387ad6e55 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp @@ -35,7 +35,7 @@ /* Author: David V. Lu!! */ #include -#include +#include namespace moveit_setup {