From 77bc858ae504e4792a46b74b5f8005fabfbb2cce Mon Sep 17 00:00:00 2001 From: unknown Date: Sat, 2 May 2020 18:04:30 -0700 Subject: [PATCH 01/26] initial commit with visibility added and some functions moved from .h to .cpp to prevent symbol related build breaks. rclcpp added to ament target dependencies. Compile options updated for MSVC. Assimp find_package updated. --- moveit_core/CMakeLists.txt | 15 +++- moveit_core/exceptions/CMakeLists.txt | 8 ++- .../include/moveit/exceptions/exceptions.h | 5 +- moveit_core/kinematics_base/CMakeLists.txt | 7 +- .../moveit/kinematics_base/kinematics_base.h | 3 +- .../moveit/macros/visibility_control.hpp | 70 +++++++++++++++++++ moveit_core/profiler/CMakeLists.txt | 7 +- .../include/moveit/profiler/profiler.h | 5 +- moveit_core/robot_model/CMakeLists.txt | 6 ++ .../include/moveit/robot_model/aabb.h | 3 +- .../moveit/robot_model/fixed_joint_model.h | 3 +- .../moveit/robot_model/floating_joint_model.h | 3 +- .../include/moveit/robot_model/joint_model.h | 39 +++++++++-- .../moveit/robot_model/joint_model_group.h | 32 +++------ .../include/moveit/robot_model/link_model.h | 3 +- .../moveit/robot_model/planar_joint_model.h | 3 +- .../robot_model/prismatic_joint_model.h | 3 +- .../moveit/robot_model/revolute_joint_model.h | 3 +- .../include/moveit/robot_model/robot_model.h | 3 +- moveit_core/robot_model/src/joint_model.cpp | 32 --------- .../robot_model/src/joint_model_group.cpp | 30 ++++++++ .../robot_model/src/revolute_joint_model.cpp | 1 + moveit_core/transforms/CMakeLists.txt | 5 ++ .../include/moveit/transforms/transforms.h | 3 +- moveit_core/utils/CMakeLists.txt | 1 - 25 files changed, 213 insertions(+), 80 deletions(-) create mode 100644 moveit_core/macros/include/moveit/macros/visibility_control.hpp diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 9e5fd8986e..e6c4d50557 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -23,7 +23,20 @@ find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL REQUIRED "fcl>=0.5.0") # replace LIBFCL_LIBRARIES with full path to the library find_library(LIBFCL_LIBRARIES_FULL ${LIBFCL_LIBRARIES} ${LIBFCL_LIBRARY_DIRS}) -set(LIBFCL_LIBRARIES "${LIBFCL_LIBRARIES_FULL}") + +find_package(ASSIMP QUIET) +if(NOT ASSIMP_FOUND) + find_package(PkgConfig REQUIRED) + # assimp is required, so REQUIRE the second attempt + pkg_check_modules(ASSIMP_PC REQUIRED assimp) + set(ASSIMP_INCLUDE_DIRS ${ASSIMP_PC_INCLUDE_DIRS}) +endif() + +# find *absolute* paths to ASSIMP_LIBRARIES +# Both, pkg-config and assimp's cmake-config don't provide an absolute library path. +# For, pkg-config the path is in ASSIMP_PC_LIBRARY_DIRS, for cmake in ASSIMP_LIBRARY_DIRS. +find_library(ASSIMP_ABS_LIBRARIES NAMES ${ASSIMP_LIBRARIES} assimp HINTS ${ASSIMP_LIBRARY_DIRS} ${ASSIMP_PC_LIBRARY_DIRS}) +set(ASSIMP_LIBRARIES "${ASSIMP_ABS_LIBRARIES}") find_package(angles REQUIRED) find_package(OCTOMAP REQUIRED) diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt index 2976afd3f1..a14ec8c781 100644 --- a/moveit_core/exceptions/CMakeLists.txt +++ b/moveit_core/exceptions/CMakeLists.txt @@ -1,7 +1,13 @@ set(MOVEIT_LIB_NAME moveit_exceptions) add_library(${MOVEIT_LIB_NAME} SHARED src/exceptions.cpp) -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") + + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${MOVEIT_LIB_NAME} Boost rclcpp diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index 1e4d28ddb9..d4d778958f 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -37,19 +37,20 @@ #pragma once #include +#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 +class MOVEIT_CORE_PUBLIC 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 +class MOVEIT_CORE_PUBLIC Exception : public std::runtime_error { public: explicit Exception(const std::string& what_arg); diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index ef52feff78..5c0e98b9e3 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -4,7 +4,12 @@ set(MOVEIT_LIB_NAME moveit_kinematics_base) add_library(${MOVEIT_LIB_NAME} SHARED src/kinematics_base.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -# This line is needed to ensure that messages are done being built before this is built +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") + + # This line is needed to ensure that messages are done being built before this is built ament_target_dependencies( ${MOVEIT_LIB_NAME} rclcpp 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 9f55085f61..752136d7d7 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 @@ -42,6 +42,7 @@ #include "rclcpp/rclcpp.hpp" #include #include +#include namespace moveit { @@ -140,7 +141,7 @@ MOVEIT_CLASS_FORWARD(KinematicsBase) // Defines KinematicsBasePtr, ConstPtr, We * @class KinematicsBase * @brief Provides an interface for kinematics solvers. */ -class KinematicsBase +class MOVEIT_CORE_PUBLIC KinematicsBase { public: static const rclcpp::Logger LOGGER; diff --git a/moveit_core/macros/include/moveit/macros/visibility_control.hpp b/moveit_core/macros/include/moveit/macros/visibility_control.hpp new file mode 100644 index 0000000000..ac0141224b --- /dev/null +++ b/moveit_core/macros/include/moveit/macros/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MOVEIT_CORE__VISIBILITY_CONTROL_HPP_ +#define MOVEIT_CORE__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MOVEIT_CORE_EXPORT __attribute__ ((dllexport)) + #define MOVEIT_CORE_IMPORT __attribute__ ((dllimport)) + #else + #define MOVEIT_CORE_EXPORT __declspec(dllexport) + #define MOVEIT_CORE_IMPORT __declspec(dllimport) + #endif + #ifdef MOVEIT_CORE_BUILDING_LIBRARY + #define MOVEIT_CORE_PUBLIC MOVEIT_CORE_EXPORT + #else + #define MOVEIT_CORE_PUBLIC MOVEIT_CORE_IMPORT + #endif + #define MOVEIT_CORE_PUBLIC_TYPE MOVEIT_CORE_PUBLIC + #define MOVEIT_CORE_LOCAL +#else + #define MOVEIT_CORE_EXPORT __attribute__ ((visibility("default"))) + #define MOVEIT_CORE_IMPORT + #if __GNUC__ >= 4 + #define MOVEIT_CORE_PUBLIC __attribute__ ((visibility("default"))) + #define MOVEIT_CORE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MOVEIT_CORE_PUBLIC + #define MOVEIT_CORE_LOCAL + #endif + #define MOVEIT_CORE_PUBLIC_TYPE +#endif + +#endif // MOVEIT_CORE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/profiler/CMakeLists.txt b/moveit_core/profiler/CMakeLists.txt index 2953acbf01..7d4430bc0e 100644 --- a/moveit_core/profiler/CMakeLists.txt +++ b/moveit_core/profiler/CMakeLists.txt @@ -2,7 +2,12 @@ set(MOVEIT_LIB_NAME moveit_profiler) add_library(${MOVEIT_LIB_NAME} SHARED src/profiler.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(${MOVEIT_LIB_NAME} +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") + + ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp rmw_implementation urdfdom diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index 3ea38f58fe..348a5d6948 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -59,6 +59,7 @@ #include #include #include +#include namespace moveit { @@ -69,7 +70,7 @@ namespace tools external profiling tools in that it allows the user to count time spent in various bits of code (sub-function granularity) or count how many times certain pieces of code are executed.*/ -class Profiler : private boost::noncopyable +class MOVEIT_CORE_PUBLIC Profiler : private boost::noncopyable { public: /** \brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. @@ -324,7 +325,7 @@ namespace moveit { namespace tools { -class Profiler +class MOVEIT_CORE_PUBLIC Profiler { public: class ScopedBlock diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 2b0886fb07..f34597dfce 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -14,6 +14,12 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/robot_model.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} angles moveit_msgs 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..b9fd2cc2dc 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -37,13 +37,14 @@ #pragma once #include +#include namespace moveit { namespace core { /** \brief Represents an axis-aligned bounding box. */ -class AABB : public Eigen::AlignedBox3d +class MOVEIT_CORE_PUBLIC AABB : public Eigen::AlignedBox3d { public: /** \brief Extend with a box transformed by the given transform. */ 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 855cbec6c7..1c6c4e180c 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 @@ -37,13 +37,14 @@ #pragma once #include +#include namespace moveit { namespace core { /** \brief A fixed joint */ -class FixedJointModel : public JointModel +class MOVEIT_CORE_PUBLIC FixedJointModel : public JointModel { public: FixedJointModel(const std::string& name); 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 0d0e0a60cc..5ad278d4a1 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 @@ -37,13 +37,14 @@ #pragma once #include +#include namespace moveit { namespace core { /** \brief A floating joint */ -class FloatingJointModel : public JointModel +class MOVEIT_CORE_PUBLIC FloatingJointModel : public JointModel { public: FloatingJointModel(const std::string& name); 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 eba749c23e..64a4a0477e 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 @@ -44,6 +44,7 @@ #include #include #include +#include namespace moveit { @@ -104,7 +105,7 @@ using JointModelMapConst = std::map; 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 +class MOVEIT_CORE_PUBLIC JointModel { public: /** \brief The different types of joints we support */ @@ -124,7 +125,7 @@ class JointModel /** \brief Construct a joint named \e name */ JointModel(const std::string& name); - virtual ~JointModel(); + virtual ~JointModel() {} /** \brief Get the name of the joint */ const std::string& getName() const @@ -298,7 +299,11 @@ class JointModel /** 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; + virtual bool harmonizePosition(double* values, const Bounds& other_bounds) const + { + return false; + } + bool harmonizePosition(double* values) const { return harmonizePosition(values, variable_bounds_); @@ -311,7 +316,15 @@ class JointModel } /** \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; + virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const + { + for (std::size_t i = 0; i < other_bounds.size(); ++i) + if (other_bounds[i].max_velocity_ + margin < values[i]) + return false; + else if (other_bounds[i].min_velocity_ - margin > values[i]) + return false; + return true; + } /** \brief Force the specified velocities to be within bounds. Return true if changes were made. */ bool enforceVelocityBounds(double* values) const @@ -320,7 +333,22 @@ class JointModel } /** \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; + virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const + { + bool change = false; + for (std::size_t i = 0; i < other_bounds.size(); ++i) + if (other_bounds[i].max_velocity_ < values[i]) + { + values[i] = other_bounds[i].max_velocity_; + change = true; + } + else if (other_bounds[i].min_velocity_ > values[i]) + { + values[i] = other_bounds[i].min_velocity_; + change = true; + } + return change; + } /** \brief Get the bounds for a variable. Throw an exception if the variable was not found */ const VariableBounds& getVariableBounds(const std::string& variable) const; @@ -519,6 +547,7 @@ class JointModel }; /** \brief Operator overload for printing variable bounds to a stream */ +MOVEIT_CORE_PUBLIC 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 256370877b..609e9c3ebd 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 @@ -43,6 +43,7 @@ #include #include #include +#include namespace moveit { @@ -65,7 +66,7 @@ using JointModelGroupMapConst = std::map; using JointBoundsVector = std::vector; -class JointModelGroup +class MOVEIT_CORE_PUBLIC JointModelGroup { public: struct KinematicsSolver @@ -305,41 +306,24 @@ class JointModelGroup 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]); - } + void getVariableDefaultPositions(std::vector& values) const; /** \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_); - } + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const; /** \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_); - } + void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const; /** \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); - } + const double distance) const; + /** \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); - } + const std::vector& near, double distance) const; /** \brief Compute random values for the state of the joint group */ void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, 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 9c6d7ba85e..e2308be8da 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 @@ -44,6 +44,7 @@ #include #include #include +#include namespace shapes { @@ -68,7 +69,7 @@ using LinkTransformMap = std::map > >; /** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */ -class LinkModel +class MOVEIT_CORE_PUBLIC LinkModel { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 4d947f0530..45d74ba8e2 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 @@ -37,13 +37,14 @@ #pragma once #include +#include namespace moveit { namespace core { /** \brief A planar joint */ -class PlanarJointModel : public JointModel +class MOVEIT_CORE_PUBLIC PlanarJointModel : public JointModel { public: /** \brief different types of planar joints we support */ 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 ece60ffe9c..ac82b654d3 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 @@ -37,13 +37,14 @@ #pragma once #include +#include namespace moveit { namespace core { /** \brief A prismatic joint */ -class PrismaticJointModel : public JointModel +class MOVEIT_CORE_PUBLIC PrismaticJointModel : public JointModel { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 9d70cf0628..89093a68bf 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 @@ -37,13 +37,14 @@ #pragma once #include +#include namespace moveit { namespace core { /** \brief A revolute joint */ -class RevoluteJointModel : public JointModel +class MOVEIT_CORE_PUBLIC RevoluteJointModel : public JointModel { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 6a1e6f2fb2..1734d7ee5d 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 @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -61,7 +62,7 @@ MOVEIT_CLASS_FORWARD(RobotModel) // Defines RobotModelPtr, ConstPtr, WeakPtr... /** \brief Definition of a kinematic model. This class is not thread safe, however multiple instances can be created */ -class RobotModel +class MOVEIT_CORE_PUBLIC RobotModel { public: /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index ff4a549104..3fd71e7924 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -90,38 +90,6 @@ int JointModel::getLocalVariableIndex(const std::string& variable) const return it->second; } -bool JointModel::harmonizePosition(double* values, const Bounds& other_bounds) const -{ - return false; -} - -bool JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const -{ - bool change = false; - for (std::size_t i = 0; i < other_bounds.size(); ++i) - if (other_bounds[i].max_velocity_ < values[i]) - { - values[i] = other_bounds[i].max_velocity_; - change = true; - } - else if (other_bounds[i].min_velocity_ > values[i]) - { - values[i] = other_bounds[i].min_velocity_; - change = true; - } - return change; -} - -bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const -{ - for (std::size_t i = 0; i < other_bounds.size(); ++i) - if (other_bounds[i].max_velocity_ + margin < values[i]) - return false; - else if (other_bounds[i].min_velocity_ - margin > values[i]) - return false; - return true; -} - const VariableBounds& JointModel::getVariableBounds(const std::string& variable) const { return variable_bounds_[getLocalVariableIndex(variable)]; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index ec53d5b7e6..b8cc8fa405 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -313,6 +313,30 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const return it->second; } +void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const +{ + getVariableRandomPositions(rng, values, active_joint_models_bounds_); +} + +void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const +{ + values.resize(variable_count_); + getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_); +} + +void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, + const double distance) const +{ + getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance); +} + +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); +} + void JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds) const { @@ -462,6 +486,12 @@ void JointModelGroup::getVariableDefaultPositions(double* values) const updateMimicJoints(values); } +void JointModelGroup::getVariableDefaultPositions(std::vector& values) const +{ + values.resize(variable_count_); + getVariableDefaultPositions(&values[0]); +} + void JointModelGroup::getVariableDefaultPositions(std::map& values) const { std::vector tmp(variable_count_); diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index f3c066e0ba..9b6c2150d5 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -34,6 +34,7 @@ *********************************************************************/ /* Author: Ioan Sucan */ +#define _USE_MATH_DEFINES #include #include diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 028c9fee6a..fd7f7bfdcb 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -2,6 +2,11 @@ set(MOVEIT_LIB_NAME moveit_transforms) add_library(${MOVEIT_LIB_NAME} SHARED src/transforms.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} geometric_shapes tf2_eigen diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 7eda9311ef..27d0920e67 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -40,6 +40,7 @@ #include #include #include +#include #include namespace moveit @@ -56,7 +57,7 @@ using FixedTransformsMap = std::map Date: Tue, 7 Jul 2020 19:53:33 -0700 Subject: [PATCH 02/26] Visibiltiy fixes for robot model and robot state --- .../moveit/robot_model/joint_model_group.h | 41 +- moveit_core/robot_model/src/joint_model.cpp | 2 - .../robot_model/src/joint_model_group.cpp | 50 +- .../include/moveit/robot_state/conversions.h | 11 + .../include/moveit/robot_state/robot_state.h | 592 +++-------------- moveit_core/robot_state/src/robot_state.cpp | 602 ++++++++++++++++++ 6 files changed, 767 insertions(+), 531 deletions(-) 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 609e9c3ebd..91af82ca2f 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 @@ -328,25 +328,15 @@ class MOVEIT_CORE_PUBLIC JointModelGroup /** \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); - } + 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 double* near, - const std::vector& distances) const - { - getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distances); - } + const std::vector& distances) const; + /** \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); - } + const std::vector& near, const std::vector& distances) const; void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds) const; @@ -366,23 +356,17 @@ class MOVEIT_CORE_PUBLIC JointModelGroup 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; 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, double margin = 0.0) const; + 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; + double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const; double distance(const double* state1, const double* state2) const; @@ -510,10 +494,7 @@ class MOVEIT_CORE_PUBLIC JointModelGroup } void setSolverAllocators(const SolverAllocatorFn& solver, - const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn()) - { - setSolverAllocators(std::make_pair(solver, solver_map)); - } + const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn()); void setSolverAllocators(const std::pair& solvers); diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index 3fd71e7924..5adbcfc906 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -59,8 +59,6 @@ JointModel::JointModel(const std::string& name) { } -JointModel::~JointModel() = default; - std::string JointModel::getTypeName() const { switch (type_) diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index b8cc8fa405..ac8698b3f7 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -313,30 +313,51 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const return it->second; } -void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const +void JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const { getVariableRandomPositions(rng, values, active_joint_models_bounds_); } -void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const +void JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const { values.resize(variable_count_); getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_); } -void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, +void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, const double distance) const { getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance); } -void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector& values, +void JointModelGroup::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); } +void JointModelGroup::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); +} + +void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, + const std::vector& distances) const +{ + getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distances); +} + +void JointModelGroup::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 JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds) const { @@ -402,6 +423,11 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum updateMimicJoints(values); } +bool JointModelGroup::satisfiesPositionBounds(const double* state, double margin) const +{ + return satisfiesPositionBounds(state, active_joint_models_bounds_, margin); +} + bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds, double margin) const { @@ -413,6 +439,11 @@ bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBo return true; } +bool JointModelGroup::enforcePositionBounds(double* state) const +{ + return enforcePositionBounds(state, active_joint_models_bounds_); +} + bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); @@ -426,6 +457,11 @@ bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVect return change; } +double JointModelGroup::getMaximumExtent() const +{ + return getMaximumExtent(active_joint_models_bounds_); +} + double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const { double max_distance = 0.0; @@ -619,6 +655,12 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik return true; } +void JointModelGroup::setSolverAllocators(const SolverAllocatorFn& solver, + const SolverAllocatorMapFn& solver_map) +{ + setSolverAllocators(std::make_pair(solver, solver_map)); +} + void JointModelGroup::setSolverAllocators(const std::pair& solvers) { if (solvers.first) 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 5129e9b7a2..ffbc0c50ed 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -50,6 +50,7 @@ namespace core * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ +MOVEIT_CORE_PUBLIC bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state); /** @@ -60,6 +61,7 @@ bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, Rob * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ +MOVEIT_CORE_PUBLIC bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies = true); @@ -70,6 +72,7 @@ bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::Rob * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ +MOVEIT_CORE_PUBLIC bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies = true); @@ -79,6 +82,7 @@ bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, * @param robot_state The resultant RobotState *message * @param copy_attached_bodies Flag to include attached objects in robot state copy */ +MOVEIT_CORE_PUBLIC void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotState& robot_state, bool copy_attached_bodies = true); @@ -87,14 +91,17 @@ void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotS * @param attached_bodies The input MoveIt attached body objects * @param attached_collision_objs The resultant AttachedCollisionObject messages */ +MOVEIT_CORE_PUBLIC 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 */ +MOVEIT_CORE_PUBLIC void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state); /** @@ -104,6 +111,7 @@ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointS * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ +MOVEIT_CORE_PUBLIC bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id, RobotState& state); @@ -115,6 +123,7 @@ bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& tra * @param include_header - flag to prefix the output with a line of joint names. * @param separator - allows to override the comma seperator with any symbol, such as a white space */ +MOVEIT_CORE_PUBLIC void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header = true, const std::string& separator = ","); @@ -127,6 +136,7 @@ void robotStateToStream(const RobotState& state, std::ostream& out, bool include * @param include_header - flag to prefix the output with a line of joint names. * @param separator - allows to override the comma seperator with any symbol, such as a white space */ +MOVEIT_CORE_PUBLIC void robotStateToStream(const RobotState& state, std::ostream& out, const std::vector& joint_groups_ordering, bool include_header = true, const std::string& separator = ","); @@ -138,6 +148,7 @@ void robotStateToStream(const RobotState& state, std::ostream& out, * @param separator - allows to override the comma seperator with any symbol, such as a white space * \return true on success */ +MOVEIT_CORE_PUBLIC 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 84d2a070e7..49511c32c6 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 @@ -86,7 +86,7 @@ typedef boost::functiongetVariableCount(); - } + std::size_t getVariableCount() const; /** \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(); - } + const std::vector& getVariableNames() const; /** \brief Get the model of a particular link */ - const LinkModel* getLinkModel(const std::string& link) const - { - return robot_model_->getLinkModel(link); - } - + const LinkModel* getLinkModel(const std::string& link) const; /** \brief Get the model of a particular joint */ - const JointModel* getJointModel(const std::string& joint) const - { - return robot_model_->getJointModel(joint); - } + const JointModel* getJointModel(const std::string& joint) const; /** \brief Get the model of a particular joint group */ - const JointModelGroup* getJointModelGroup(const std::string& group) const - { - return robot_model_->getJointModelGroup(group); - } + const JointModelGroup* getJointModelGroup(const std::string& group) const; /** \name Getting and setting variable position * @{ @@ -164,11 +145,7 @@ class RobotState /** \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]); - } + void setVariablePositions(const std::vector& position); /** \brief Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. */ @@ -185,37 +162,19 @@ class RobotState 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); - } + void setVariablePosition(const std::string& variable, double 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); - } - } + void setVariablePosition(int index, double value); /** \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)]; - } + double getVariablePosition(const std::string& variable) const; /** \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]; - } + double getVariablePosition(int index) const; /** @} */ @@ -226,43 +185,24 @@ class RobotState /** \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_; - } + bool hasVelocities() const; /** \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_; - } + double* getVariableVelocities(); /** \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_; - } + const double* getVariableVelocities() const; /** \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_, velocity, robot_model_->getVariableCount() * sizeof(double)); - } + void setVariableVelocities(const double* velocity); /** \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]); - } + void setVariableVelocities(const std::vector& velocity); /** \brief Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. */ @@ -279,18 +219,11 @@ class RobotState 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); - } - + void setVariableVelocity(const std::string& variable, double 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; - } + void setVariableVelocity(int index, double 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 @@ -345,22 +278,11 @@ class RobotState /** \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(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double)); - } + void setVariableAccelerations(const double* acceleration); /** \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]); - } + void setVariableAccelerations(const std::vector& acceleration); /** \brief Set the accelerations of a set of variables. If unknown variable names are specified, an exception is * thrown. */ @@ -417,45 +339,25 @@ class RobotState /** \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_; - } + bool hasEffort() const; /** \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_; - } + double* getVariableEffort(); /** \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_; - } + const double* getVariableEffort() const; /** \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_, effort, robot_model_->getVariableCount() * sizeof(double)); - } + void setVariableEffort(const double* effort); /** \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]); - } + void setVariableEffort(const std::vector& effort); /** \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); @@ -469,32 +371,19 @@ class RobotState 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); - } + void setVariableEffort(const std::string& variable, double 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_[index] = value; - } + void setVariableEffort(int index, double 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_[robot_model_->getVariableIndex(variable)]; - } + double getVariableEffort(const std::string& variable) const; /** \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_[index]; - } + double getVariableEffort(int index) const; /** \brief Remove effort values from this state (this differs from setting them to zero) */ void dropEffort(); @@ -512,87 +401,37 @@ class RobotState * 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 double* position); - void setJointPositions(const std::string& joint_name, const std::vector& position) - { - setJointPositions(robot_model_->getJointModel(joint_name), &position[0]); - } + void setJointPositions(const std::string& joint_name, const std::vector& position); - void setJointPositions(const JointModel* joint, const std::vector& position) - { - setJointPositions(joint, &position[0]); - } + void setJointPositions(const JointModel* joint, const std::vector& position); - void setJointPositions(const JointModel* joint, const double* position) - { - memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double)); - markDirtyJointTransforms(joint); - updateMimicJoint(joint); - } + void setJointPositions(const JointModel* joint, const double* position); + + void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform); - 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 setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform) - { - joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex()); - markDirtyJointTransforms(joint); - updateMimicJoint(joint); - } - - void setJointVelocities(const JointModel* joint, const double* velocity) - { - has_velocity_ = true; - memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double)); - } + 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)); - } + const double* getJointPositions(const std::string& joint_name) const; - const double* getJointPositions(const JointModel* joint) const - { - return position_ + joint->getFirstVariableIndex(); - } + const double* getJointPositions(const JointModel* joint) const; - const double* getJointVelocities(const std::string& joint_name) const - { - return getJointVelocities(robot_model_->getJointModel(joint_name)); - } + const double* getJointVelocities(const std::string& joint_name) const; - const double* getJointVelocities(const JointModel* joint) const - { - return velocity_ + joint->getFirstVariableIndex(); - } + const double* getJointVelocities(const JointModel* joint) const; - const double* getJointAccelerations(const std::string& joint_name) const - { - return getJointAccelerations(robot_model_->getJointModel(joint_name)); - } + const double* getJointAccelerations(const std::string& joint_name) const; - const double* getJointAccelerations(const JointModel* joint) const - { - return acceleration_ + joint->getFirstVariableIndex(); - } + const double* getJointAccelerations(const JointModel* joint) const; - const double* getJointEffort(const std::string& joint_name) const - { - return getJointEffort(robot_model_->getJointModel(joint_name)); - } + const double* getJointEffort(const std::string& joint_name) const; - const double* getJointEffort(const JointModel* joint) const - { - return effort_ + joint->getFirstVariableIndex(); - } + const double* getJointEffort(const JointModel* joint) const; /** @} */ @@ -602,12 +441,7 @@ class RobotState /** \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); - } + void setJointGroupPositions(const std::string& joint_group_name, 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 */ @@ -688,34 +522,17 @@ class RobotState /** \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]); - } - } + void copyJointGroupPositions(const std::string& joint_group_name, std::vector& 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, double* gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupPositions(jmg, gstate); - } + void copyJointGroupPositions(const std::string& joint_group_name, 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 JointModelGroup* group, std::vector& gstate) const - { - gstate.resize(group->getVariableCount()); - copyJointGroupPositions(group, &gstate[0]); - } + void copyJointGroupPositions(const JointModelGroup* group, std::vector& 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 @@ -725,12 +542,7 @@ class RobotState /** \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); - } + void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) 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 @@ -745,28 +557,15 @@ class RobotState /** \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); - } + void setJointGroupVelocities(const std::string& joint_group_name, 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 std::vector& gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupVelocities(jmg, &gstate[0]); - } + void setJointGroupVelocities(const std::string& joint_group_name, const std::vector& 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 JointModelGroup* group, const std::vector& gstate) - { - setJointGroupVelocities(group, &gstate[0]); - } + void setJointGroupVelocities(const JointModelGroup* group, const std::vector& 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 */ @@ -774,13 +573,8 @@ class RobotState /** \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); - } - + void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& 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); @@ -788,34 +582,17 @@ class RobotState /** \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]); - } - } + void copyJointGroupVelocities(const std::string& joint_group_name, std::vector& 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, double* gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupVelocities(jmg, gstate); - } + void copyJointGroupVelocities(const std::string& joint_group_name, 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 JointModelGroup* group, std::vector& gstate) const - { - gstate.resize(group->getVariableCount()); - copyJointGroupVelocities(group, &gstate[0]); - } + void copyJointGroupVelocities(const JointModelGroup* group, std::vector& 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 @@ -825,12 +602,7 @@ class RobotState /** \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); - } + void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) 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 @@ -845,28 +617,15 @@ class RobotState /** \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); - } + void setJointGroupAccelerations(const std::string& joint_group_name, 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 std::vector& gstate) - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupAccelerations(jmg, &gstate[0]); - } + void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector& 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 JointModelGroup* group, const std::vector& gstate) - { - setJointGroupAccelerations(group, &gstate[0]); - } + void setJointGroupAccelerations(const JointModelGroup* group, const std::vector& 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 */ @@ -874,12 +633,7 @@ class RobotState /** \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); - } + void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& 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 */ @@ -888,34 +642,17 @@ class RobotState /** \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]); - } - } + void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector& 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, double* gstate) const - { - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupAccelerations(jmg, gstate); - } + void copyJointGroupAccelerations(const std::string& joint_group_name, 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 JointModelGroup* group, std::vector& gstate) const - { - gstate.resize(group->getVariableCount()); - copyJointGroupAccelerations(group, &gstate[0]); - } + void copyJointGroupAccelerations(const JointModelGroup* group, std::vector& 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 @@ -925,12 +662,7 @@ class RobotState /** \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); - } + void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) 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 @@ -1191,11 +923,7 @@ class RobotState /** \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); - } + const LinkModel* 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, @@ -1209,13 +937,7 @@ class RobotState * @{ */ - 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); - } + void setVariableValues(const sensor_msgs::msg::JointState& msg); /** \brief Set all joints to their default positions. The default position is 0, or if that is not within bounds then half way @@ -1293,27 +1015,13 @@ class RobotState * * 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 std::string& link_name); - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) - { - updateLinkTransforms(); - return global_link_transforms_[link->getLinkIndex()]; - } + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link); - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const - { - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); - } + const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const; - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const - { - BOOST_VERIFY(checkLinkTransforms()); - return global_link_transforms_[link->getLinkIndex()]; - } + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) 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. @@ -1325,55 +1033,21 @@ class RobotState * @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 std::string& link_name, std::size_t 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 LinkModel* link, std::size_t 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 std::string& link_name, std::size_t index) const; - const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const - { - BOOST_VERIFY(checkCollisionTransforms()); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; - } + const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const; - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) - { - return getJointTransform(robot_model_->getJointModel(joint_name)); - } + const Eigen::Isometry3d& getJointTransform(const std::string& joint_name); - const Eigen::Isometry3d& getJointTransform(const JointModel* joint) - { - const int idx = joint->getJointIndex(); - unsigned char& dirty = dirty_joint_transforms_[idx]; - if (dirty) - { - joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]); - dirty = 0; - } - return variable_joint_transforms_[idx]; - } + 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 std::string& joint_name) const; - const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const - { - BOOST_VERIFY(checkJointTransforms(joint)); - return variable_joint_transforms_[joint->getJointIndex()]; - } + const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const; bool dirtyJointTransform(const JointModel* joint) const { @@ -1458,50 +1132,22 @@ class RobotState 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) - { - if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex())) - { - markDirtyJointTransforms(joint); - updateMimicJoint(joint); - } - } + void enforceBounds(const JointModel* 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) - { - if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex())) - // no need to mark transforms dirty, as the transform hasn't changed - updateMimicJoint(joint); - } + void harmonizePosition(const JointModel* joint); - void enforceVelocityBounds(const JointModel* joint) - { - joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex()); - } + 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); - } + bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const; + bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const; + bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const; /** \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. */ @@ -1593,11 +1239,7 @@ class RobotState 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, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses); - } + const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()); /** \brief Get all bodies attached to the model corresponding to this state */ void getAttachedBodies(std::vector& attached_bodies) const; @@ -1643,12 +1285,7 @@ class RobotState } /** \brief Return the instance of a random number generator */ - random_numbers::RandomNumberGenerator& getRandomNumberGenerator() - { - if (!rng_) - rng_ = new random_numbers::RandomNumberGenerator(); - return *rng_; - } + random_numbers::RandomNumberGenerator& getRandomNumberGenerator(); /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * @@ -1763,55 +1400,20 @@ class RobotState 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 markDirtyJointTransforms(const JointModelGroup* group); void markVelocity(); void markAcceleration(); void markEffort(); - void updateMimicJoint(const JointModel* joint) - { - double v = position_[joint->getFirstVariableIndex()]; - for (const JointModel* jm : joint->getMimicRequests()) - { - position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset(); - markDirtyJointTransforms(jm); - } - } + void updateMimicJoint(const JointModel* joint); /** \brief Update a set of joints that are certain to be mimicking other joints */ /* use updateMimicJoints() instead, which also marks joints dirty */ - [[deprecated]] void updateMimicJoint(const std::vector& mim) - { - for (const JointModel* jm : mim) - { - const int fvi = jm->getFirstVariableIndex(); - position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); - // Only mark joint transform dirty, but not the associated link transform - // as this function is always used in combination of - // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); - dirty_joint_transforms_[jm->getJointIndex()] = 1; - } - } + [[deprecated]] void updateMimicJoint(const std::vector& mim); /** \brief Update all mimic joints within group */ - void updateMimicJoints(const JointModelGroup* group) - { - for (const JointModel* jm : group->getMimicJointModels()) - { - const int fvi = jm->getFirstVariableIndex(); - position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); - markDirtyJointTransforms(jm); - } - markDirtyJointTransforms(group); - } + void updateMimicJoints(const JointModelGroup* group); void updateLinkTransformsInternal(const JointModel* start); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 739956066b..22fee2b35d 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -83,6 +83,36 @@ RobotState::~RobotState() delete rng_; } +const RobotModelConstPtr& RobotState::getRobotModel() const +{ + return robot_model_; +} + +std::size_t RobotState::getVariableCount() const +{ + return robot_model_->getVariableCount(); +} + +const std::vector& RobotState::getVariableNames() const +{ + return robot_model_->getVariableNames(); +} + +const LinkModel* RobotState::getLinkModel(const std::string& link) const +{ + return robot_model_->getLinkModel(link); +} + +const JointModel* RobotState::getJointModel(const std::string& joint) const +{ + return robot_model_->getJointModel(joint); +} + +const JointModelGroup* RobotState::getJointModelGroup(const std::string& group) const +{ + return robot_model_->getJointModelGroup(group); +} + void RobotState::allocMemory() { static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == sizeof(Eigen::Isometry3d), @@ -359,6 +389,12 @@ void RobotState::setVariablePositions(const double* position) dirty_link_transforms_ = robot_model_->getRootJoint(); } +void RobotState::setVariablePositions(const std::vector& position) +{ + assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode + setVariablePositions(&position[0]); +} + void RobotState::setVariablePositions(const std::map& variable_map) { for (const std::pair& it : variable_map) @@ -402,6 +438,61 @@ void RobotState::setVariablePositions(const std::vector& variable_n } } +void RobotState::setVariablePosition(const std::string& variable, double value) +{ + setVariablePosition(robot_model_->getVariableIndex(variable), value); +} + +void RobotState::setVariablePosition(int index, double value) +{ + position_[index] = value; + const JointModel* jm = robot_model_->getJointOfVariable(index); + if (jm) + { + markDirtyJointTransforms(jm); + updateMimicJoint(jm); + } +} + +double RobotState::getVariablePosition(const std::string& variable) const +{ + return position_[robot_model_->getVariableIndex(variable)]; +} + +double RobotState::getVariablePosition(int index) const +{ + return position_[index]; +} + +bool RobotState::hasVelocities() const +{ + return has_velocity_; +} + +double* RobotState::getVariableVelocities() +{ + markVelocity(); + return velocity_; +} + +const double* RobotState::getVariableVelocities() const +{ + return velocity_; +} + +void RobotState::setVariableVelocities(const double* velocity) +{ + has_velocity_ = true; + // assume everything is in order in terms of array lengths (for efficiency reasons) + memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double)); +} + +void RobotState::setVariableVelocities(const std::vector& velocity) +{ + assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode + setVariableVelocities(&velocity[0]); +} + void RobotState::setVariableVelocities(const std::map& variable_map) { markVelocity(); @@ -425,6 +516,32 @@ void RobotState::setVariableVelocities(const std::vector& variable_ velocity_[robot_model_->getVariableIndex(variable_names[i])] = variable_velocity[i]; } +void RobotState::setVariableVelocity(const std::string& variable, double value) +{ + setVariableVelocity(robot_model_->getVariableIndex(variable), value); +} + +void RobotState::setVariableVelocity(int index, double value) +{ + markVelocity(); + velocity_[index] = value; +} + +void RobotState::setVariableAccelerations(const double* acceleration) +{ + has_acceleration_ = true; + has_effort_ = false; + + // assume everything is in order in terms of array lengths (for efficiency reasons) + memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double)); +} + +void RobotState::setVariableAccelerations(const std::vector& acceleration) +{ + assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode + setVariableAccelerations(&acceleration[0]); +} + void RobotState::setVariableAccelerations(const std::map& variable_map) { markAcceleration(); @@ -448,6 +565,36 @@ void RobotState::setVariableAccelerations(const std::vector& variab acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_acceleration[i]; } +bool RobotState::hasEffort() const +{ + return has_effort_; +} + +double* RobotState::getVariableEffort() +{ + markEffort(); + return effort_; +} + +const double* RobotState::getVariableEffort() const +{ + return effort_; +} + +void RobotState::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_, effort, robot_model_->getVariableCount() * sizeof(double)); +} + +void RobotState::setVariableEffort(const std::vector& effort) +{ + assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode + setVariableEffort(&effort[0]); +} + void RobotState::setVariableEffort(const std::map& variable_map) { markEffort(); @@ -471,6 +618,27 @@ void RobotState::setVariableEffort(const std::vector& variable_name effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i]; } +void RobotState::setVariableEffort(const std::string& variable, double value) +{ + setVariableEffort(robot_model_->getVariableIndex(variable), value); +} + +void RobotState::setVariableEffort(int index, double value) +{ + markEffort(); + effort_[index] = value; +} + +double RobotState::getVariableEffort(const std::string& variable) const +{ + return effort_[robot_model_->getVariableIndex(variable)]; +} + +double RobotState::getVariableEffort(int index) const +{ + return effort_[index]; +} + void RobotState::invertVelocity() { if (has_velocity_) @@ -480,6 +648,46 @@ void RobotState::invertVelocity() } } +void RobotState::setJointPositions(const std::string& joint_name, const double* position) +{ + setJointPositions(robot_model_->getJointModel(joint_name), position); +} + +void RobotState::setJointPositions(const std::string& joint_name, const std::vector& position) +{ + setJointPositions(robot_model_->getJointModel(joint_name), &position[0]); +} + +void RobotState::setJointPositions(const JointModel* joint, const std::vector& position) +{ + setJointPositions(joint, &position[0]); +} + +void RobotState::setJointPositions(const JointModel* joint, const double* position) +{ + memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double)); + markDirtyJointTransforms(joint); + updateMimicJoint(joint); +} + +void RobotState::setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform) +{ + setJointPositions(robot_model_->getJointModel(joint_name), transform); +} + +void RobotState::setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform) +{ + joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex()); + markDirtyJointTransforms(joint); + updateMimicJoint(joint); +} + +void RobotState::setJointVelocities(const JointModel* joint, const double* velocity) +{ + has_velocity_ = true; + memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double)); +} + void RobotState::setJointEfforts(const JointModel* joint, const double* effort) { if (has_acceleration_) @@ -492,6 +700,65 @@ void RobotState::setJointEfforts(const JointModel* joint, const double* effort) memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double)); } +const double* RobotState::getJointPositions(const std::string& joint_name) const +{ + return getJointPositions(robot_model_->getJointModel(joint_name)); +} + +const double* RobotState::getJointPositions(const JointModel* joint) const +{ + return position_ + joint->getFirstVariableIndex(); +} + +const double* RobotState::getJointVelocities(const std::string& joint_name) const +{ + return getJointVelocities(robot_model_->getJointModel(joint_name)); +} + +const double* RobotState::getJointVelocities(const JointModel* joint) const +{ + return velocity_ + joint->getFirstVariableIndex(); +} + +const double* RobotState::getJointAccelerations(const std::string& joint_name) const +{ + return getJointAccelerations(robot_model_->getJointModel(joint_name)); +} + +const double* RobotState::getJointAccelerations(const JointModel* joint) const +{ + return acceleration_ + joint->getFirstVariableIndex(); +} + +const double* RobotState::getJointEffort(const std::string& joint_name) const +{ + return getJointEffort(robot_model_->getJointModel(joint_name)); +} + +const double* RobotState::getJointEffort(const JointModel* joint) const +{ + return effort_ + joint->getFirstVariableIndex(); +} + +void RobotState::setJointGroupPositions(const std::string& joint_group_name, const double* gstate) +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupPositions(jmg, gstate); +} + +void RobotState::setJointGroupPositions(const std::string& joint_group_name, const std::vector& gstate) +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupPositions(jmg, &gstate[0]); +} + +void RobotState::setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) +{ + setJointGroupPositions(group, &gstate[0]); +} + void RobotState::setJointGroupPositions(const JointModelGroup* group, const double* gstate) { const std::vector& il = group->getVariableIndexList(); @@ -505,6 +772,13 @@ void RobotState::setJointGroupPositions(const JointModelGroup* group, const doub updateMimicJoints(group); } +void RobotState::setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values) +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupPositions(jmg, values); +} + void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values) { const std::vector& il = group->getVariableIndexList(); @@ -547,6 +821,13 @@ void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* g gstate[i] = position_[il[i]]; } +void RobotState::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); +} + void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const { const std::vector& il = group->getVariableIndexList(); @@ -555,6 +836,25 @@ void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::Ve values(i) = position_[il[i]]; } +void RobotState::setJointGroupVelocities(const std::string& joint_group_name, const double* gstate) +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupVelocities(jmg, gstate); +} + +void RobotState::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]); +} + +void RobotState::setJointGroupVelocities(const JointModelGroup* group, const std::vector& gstate) +{ + setJointGroupVelocities(group, &gstate[0]); +} + void RobotState::setJointGroupVelocities(const JointModelGroup* group, const double* gstate) { markVelocity(); @@ -568,6 +868,13 @@ void RobotState::setJointGroupVelocities(const JointModelGroup* group, const dou } } +void RobotState::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); +} + void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values) { markVelocity(); @@ -576,6 +883,29 @@ void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eig velocity_[il[i]] = values(i); } +void RobotState::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]); + } +} + +void RobotState::copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupVelocities(jmg, gstate); +} + +void RobotState::copyJointGroupVelocities(const JointModelGroup* group, std::vector& gstate) const +{ + gstate.resize(group->getVariableCount()); + copyJointGroupVelocities(group, &gstate[0]); +} + void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const { const std::vector& il = group->getVariableIndexList(); @@ -586,6 +916,13 @@ void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double* gstate[i] = velocity_[il[i]]; } +void RobotState::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); +} + void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const { const std::vector& il = group->getVariableIndexList(); @@ -594,6 +931,25 @@ void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::V values(i) = velocity_[il[i]]; } +void RobotState::setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate) +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + setJointGroupAccelerations(jmg, gstate); +} + +void RobotState::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]); +} + +void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const std::vector& gstate) +{ + setJointGroupAccelerations(group, &gstate[0]); +} + void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const double* gstate) { markAcceleration(); @@ -607,6 +963,13 @@ void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const } } +void RobotState::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); +} + void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values) { markAcceleration(); @@ -615,6 +978,29 @@ void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const acceleration_[il[i]] = values(i); } +void RobotState::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]); + } +} + +void RobotState::copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const +{ + const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); + if (jmg) + copyJointGroupAccelerations(jmg, gstate); +} + +void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, std::vector& gstate) const +{ + gstate.resize(group->getVariableCount()); + copyJointGroupAccelerations(group, &gstate[0]); +} + void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const { const std::vector& il = group->getVariableIndexList(); @@ -625,6 +1011,13 @@ void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, doubl gstate[i] = acceleration_[il[i]]; } +void RobotState::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); +} + void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const { const std::vector& il = group->getVariableIndexList(); @@ -674,6 +1067,56 @@ void RobotState::updateCollisionBodyTransforms() } } +void RobotState::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 RobotState::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 RobotState::updateMimicJoint(const JointModel* joint) +{ + double v = position_[joint->getFirstVariableIndex()]; + for (const JointModel* jm : joint->getMimicRequests()) + { + position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset(); + markDirtyJointTransforms(jm); + } +} + +[[deprecated]] void RobotState::updateMimicJoint(const std::vector& mim) +{ + for (const JointModel* jm : mim) + { + const int fvi = jm->getFirstVariableIndex(); + position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); + // Only mark joint transform dirty, but not the associated link transform + // as this function is always used in combination of + // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); + dirty_joint_transforms_[jm->getJointIndex()] = 1; + } +} + +void RobotState::updateMimicJoints(const JointModelGroup* group) +{ + for (const JointModel* jm : group->getMimicJointModels()) + { + const int fvi = jm->getFirstVariableIndex(); + position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); + markDirtyJointTransforms(jm); + } + markDirtyJointTransforms(group); +} + void RobotState::updateLinkTransforms() { if (dirty_link_transforms_ != nullptr) @@ -779,6 +1222,78 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); } +const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const std::string& link_name) +{ + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); +} + +const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const LinkModel* link) +{ + updateLinkTransforms(); + return global_link_transforms_[link->getLinkIndex()]; +} + +const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const std::string& link_name) const +{ + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); +} + +const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const LinkModel* link) const +{ + BOOST_VERIFY(checkLinkTransforms()); + return global_link_transforms_[link->getLinkIndex()]; +} + +const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const std::string& link_name, std::size_t index) +{ + return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); +} + +const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const LinkModel* link, std::size_t index) +{ + updateCollisionBodyTransforms(); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; +} + +const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const std::string& link_name, std::size_t index) const +{ + return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); +} + +const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const LinkModel* link, std::size_t index) const +{ + BOOST_VERIFY(checkCollisionTransforms()); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; +} + +const Eigen::Isometry3d& RobotState::getJointTransform(const std::string& joint_name) +{ + return getJointTransform(robot_model_->getJointModel(joint_name)); +} + +const Eigen::Isometry3d& RobotState::getJointTransform(const JointModel* joint) +{ + const int idx = joint->getJointIndex(); + unsigned char& dirty = dirty_joint_transforms_[idx]; + if (dirty) + { + joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]); + dirty = 0; + } + return variable_joint_transforms_[idx]; +} + +const Eigen::Isometry3d& RobotState::getJointTransform(const std::string& joint_name) const +{ + return getJointTransform(robot_model_->getJointModel(joint_name)); +} + +const Eigen::Isometry3d& RobotState::getJointTransform(const JointModel* joint) const +{ + BOOST_VERIFY(checkJointTransforms(joint)); + return variable_joint_transforms_[joint->getJointIndex()]; +} + bool RobotState::satisfiesBounds(double margin) const { const std::vector& jm = robot_model_->getActiveJointModels(); @@ -797,6 +1312,21 @@ bool RobotState::satisfiesBounds(const JointModelGroup* group, double margin) co return true; } +bool RobotState::satisfiesBounds(const JointModel* joint, double margin) const +{ + return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin)); +} + +bool RobotState::satisfiesPositionBounds(const JointModel* joint, double margin) const +{ + return joint->satisfiesPositionBounds(getJointPositions(joint), margin); +} + +bool RobotState::satisfiesVelocityBounds(const JointModel* joint, double margin) const +{ + return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin); +} + void RobotState::enforceBounds() { const std::vector& jm = robot_model_->getActiveJointModels(); @@ -811,6 +1341,22 @@ void RobotState::enforceBounds(const JointModelGroup* joint_group) enforceBounds(joint); } +void RobotState::enforceBounds(const JointModel* joint) +{ + enforcePositionBounds(joint); + if (has_velocity_) + enforceVelocityBounds(joint); +} + +void RobotState::enforcePositionBounds(const JointModel* joint) +{ + if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex())) + { + markDirtyJointTransforms(joint); + updateMimicJoint(joint); + } +} + void RobotState::harmonizePositions() { for (const JointModel* jm : robot_model_->getActiveJointModels()) @@ -823,6 +1369,18 @@ void RobotState::harmonizePositions(const JointModelGroup* joint_group) harmonizePosition(jm); } +void RobotState::harmonizePosition(const JointModel* joint) +{ + if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex())) + // no need to mark transforms dirty, as the transform hasn't changed + updateMimicJoint(joint); +} + +void RobotState::enforceVelocityBounds(const JointModel* joint) +{ + joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex()); +} + std::pair RobotState::getMinDistanceToPositionBounds() const { return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels()); @@ -890,6 +1448,11 @@ bool RobotState::isValidVelocityMove(const RobotState& other, const JointModelGr return true; } +double RobotState::distance(const RobotState& other) const +{ + return robot_model_->distance(position_, other.getVariablePositions()); +} + double RobotState::distance(const RobotState& other, const JointModelGroup* joint_group) const { double d = 0.0; @@ -902,6 +1465,12 @@ double RobotState::distance(const RobotState& other, const JointModelGroup* join return d; } +double RobotState::distance(const RobotState& other, const JointModel* joint) const +{ + const int idx = joint->getFirstVariableIndex(); + return joint->distance(position_ + idx, other.position_ + idx); +} + void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const { robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions()); @@ -963,6 +1532,16 @@ void RobotState::attachBody(const std::string& id, 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, + const moveit::core::FixedTransformsMap& subframe_poses) +{ + std::set touch_links_set(touch_links.begin(), touch_links.end()); + attachBody(id, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses); +} + void RobotState::getAttachedBodies(std::vector& attached_bodies) const { attached_bodies.clear(); @@ -1050,6 +1629,13 @@ bool RobotState::clearAttachedBody(const std::string& id) return false; } +random_numbers::RandomNumberGenerator& RobotState::getRandomNumberGenerator() +{ + if (!rng_) + rng_ = new random_numbers::RandomNumberGenerator(); + return *rng_; +} + const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_id, bool* frame_found) { updateLinkTransforms(); @@ -1370,6 +1956,13 @@ bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs:: return setFromDiffIK(jmg, t, tip, dt, constraint); } +void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, + const LinkModel* tip) +{ + updateLinkTransforms(); + static_cast(this)->computeVariableVelocity(jmg, qdot, twist, tip); +} + void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, const LinkModel* tip) const { @@ -1430,6 +2023,15 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig return true; } +void RobotState::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); +} + + bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) From bf07c178d2cedb34124161e4809161495c5e3958 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 14 Jul 2020 11:04:12 -0700 Subject: [PATCH 03/26] Windows bringup work in progress. Working on moveit_core/collision_detection --- moveit_core/CMakeLists.txt | 7 ++ .../collision_detection/CMakeLists.txt | 8 +++ .../collision_detection/collision_plugin.h | 3 +- .../visibility_control.hpp | 70 +++++++++++++++++++ .../collision_detection_bullet/CMakeLists.txt | 1 + .../collision_detection_fcl/CMakeLists.txt | 10 ++- .../collision_common.h | 8 +++ .../collision_detector_allocator_fcl.h | 3 +- .../collision_detector_fcl_plugin_loader.h | 3 +- .../collision_env_fcl.h | 2 +- .../visibility_control.hpp | 70 +++++++++++++++++++ moveit_core/distance_field/CMakeLists.txt | 2 + .../kinematic_constraints/CMakeLists.txt | 1 + .../moveit/kinematics_base/kinematics_base.h | 11 ++- moveit_core/package.xml | 1 + moveit_core/planning_interface/CMakeLists.txt | 1 + moveit_core/planning_scene/CMakeLists.txt | 1 + moveit_core/robot_trajectory/CMakeLists.txt | 5 ++ .../robot_trajectory/robot_trajectory.h | 3 +- .../robot_trajectory/visibility_control.hpp | 70 +++++++++++++++++++ .../trajectory_processing/CMakeLists.txt | 1 + .../time_optimal_trajectory_generation.h | 1 + moveit_core/utils/CMakeLists.txt | 1 + .../collision_plugin_loader/CMakeLists.txt | 1 + 24 files changed, 277 insertions(+), 7 deletions(-) create mode 100644 moveit_core/collision_detection/include/moveit/collision_detection/visibility_control.hpp create mode 100644 moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp create mode 100644 moveit_core/robot_trajectory/include/moveit/robot_trajectory/visibility_control.hpp diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index e6c4d50557..6293dc4133 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -23,6 +23,13 @@ find_package(PkgConfig REQUIRED) pkg_check_modules(LIBFCL REQUIRED "fcl>=0.5.0") # replace LIBFCL_LIBRARIES with full path to the library find_library(LIBFCL_LIBRARIES_FULL ${LIBFCL_LIBRARIES} ${LIBFCL_LIBRARY_DIRS}) +set(LIBFCL_LIBRARIES "${LIBFCL_LIBRARIES_FULL}") + +if(WIN32) +pkg_check_modules(LIBCCD REQUIRED ccd) +find_library(LIBCCD_LIBRARIES_FULL ${LIBCCD_LIBRARIES} ${LIBCCD_LIBRARY_DIRS}) +set(LIBCCD_LIBRARIES "${LIBCCD_LIBRARIES_FULL}") +endif() find_package(ASSIMP QUIET) if(NOT ASSIMP_FOUND) diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index a914eafb36..66a0b7444a 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -12,6 +12,14 @@ add_library(${MOVEIT_LIB_NAME} SHARED ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "COLLISION_DETECTION_BUILDING_LIBRARY") + + ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp rmw_implementation 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 799d90cf10..0e55f2c397 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 @@ -37,6 +37,7 @@ #include #include #include +#include namespace collision_detection { @@ -74,7 +75,7 @@ MOVEIT_CLASS_FORWARD(CollisionPlugin) // Defines CollisionPluginPtr, ConstPtr, * }; * */ -class CollisionPlugin +class COLLISION_DETECTION_PUBLIC CollisionPlugin { public: CollisionPlugin() diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/visibility_control.hpp b/moveit_core/collision_detection/include/moveit/collision_detection/visibility_control.hpp new file mode 100644 index 0000000000..852437ab3f --- /dev/null +++ b/moveit_core/collision_detection/include/moveit/collision_detection/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef COLLISION_DETECTION__VISIBILITY_CONTROL_HPP_ +#define COLLISION_DETECTION__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define COLLISION_DETECTION_EXPORT __attribute__ ((dllexport)) + #define COLLISION_DETECTION_IMPORT __attribute__ ((dllimport)) + #else + #define COLLISION_DETECTION_EXPORT __declspec(dllexport) + #define COLLISION_DETECTION_IMPORT __declspec(dllimport) + #endif + #ifdef COLLISION_DETECTION_BUILDING_LIBRARY + #define COLLISION_DETECTION_PUBLIC COLLISION_DETECTION_EXPORT + #else + #define COLLISION_DETECTION_PUBLIC COLLISION_DETECTION_IMPORT + #endif + #define COLLISION_DETECTION_PUBLIC_TYPE COLLISION_DETECTION_PUBLIC + #define COLLISION_DETECTION_LOCAL +#else + #define COLLISION_DETECTION_EXPORT __attribute__ ((visibility("default"))) + #define COLLISION_DETECTION_IMPORT + #if __GNUC__ >= 4 + #define COLLISION_DETECTION_PUBLIC __attribute__ ((visibility("default"))) + #define COLLISION_DETECTION_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define COLLISION_DETECTION_PUBLIC + #define COLLISION_DETECTION_LOCAL + #endif + #define COLLISION_DETECTION_PUBLIC_TYPE +#endif + +#endif // COLLISION_DETECTION__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 27fbd79504..ec43c5a9a3 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -10,6 +10,7 @@ add_library(${MOVEIT_LIB_NAME} src/bullet_integration/ros_bullet_utils.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index b6a6b839f6..5e3bc01969 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -12,6 +12,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdfdom urdfdom_headers LIBFCL + LIBCCD Boost visualization_msgs ) @@ -21,7 +22,14 @@ target_link_libraries(${MOVEIT_LIB_NAME} add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp) set_target_properties(collision_detector_fcl_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(collision_detector_fcl_plugin +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "COLLISION_DETECTION_FCL_BUILDING_LIBRARY") + + + ament_target_dependencies(collision_detector_fcl_plugin rclcpp urdf visualization_msgs 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 06063332e5..a91e2201d7 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 @@ -41,6 +41,7 @@ #include #include #include +#include #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) #include @@ -287,31 +288,38 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); /** \brief Create new FCLGeometry object out of robot link model. */ +COLLISION_DETECTION_FCL_PUBLIC FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link, int shape_index); /** \brief Create new FCLGeometry object out of attached body. */ +COLLISION_DETECTION_FCL_PUBLIC 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. */ +COLLISION_DETECTION_FCL_PUBLIC FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj); /** \brief Create new scaled and / or padded FCLGeometry object out of robot link model. */ +COLLISION_DETECTION_FCL_PUBLIC 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. */ +COLLISION_DETECTION_FCL_PUBLIC 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. */ +COLLISION_DETECTION_FCL_PUBLIC 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. */ +COLLISION_DETECTION_FCL_PUBLIC void cleanCollisionGeometryCache(); /** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ 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 fde03c4cb5..6c8389ef7f 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 @@ -38,11 +38,12 @@ #include #include +#include namespace collision_detection { /** \brief An allocator for FCL collision detectors */ -class CollisionDetectorAllocatorFCL +class COLLISION_DETECTION_FCL_PUBLIC CollisionDetectorAllocatorFCL : public CollisionDetectorAllocatorTemplate { public: 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..0f7924f9c7 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 @@ -38,10 +38,11 @@ #include #include +#include namespace collision_detection { -class CollisionDetectorFCLPluginLoader : public CollisionPlugin +class COLLISION_DETECTION_FCL_PUBLIC CollisionDetectorFCLPluginLoader : public CollisionPlugin { public: bool initialize(const planning_scene::PlanningScenePtr& scene) const override; 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 c3e81354c3..9588d5e648 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 @@ -50,7 +50,7 @@ namespace collision_detection { /** \brief FCL implementation of the CollisionEnv */ -class CollisionEnvFCL : public CollisionEnv +class COLLISION_DETECTION_FCL_PUBLIC CollisionEnvFCL : public CollisionEnv { public: CollisionEnvFCL() = delete; diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp new file mode 100644 index 0000000000..d09be6c45c --- /dev/null +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef COLLISION_DETECTION_FCL__VISIBILITY_CONTROL_HPP_ +#define COLLISION_DETECTION_FCL__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define COLLISION_DETECTION_FCL_EXPORT __attribute__ ((dllexport)) + #define COLLISION_DETECTION_FCL_IMPORT __attribute__ ((dllimport)) + #else + #define COLLISION_DETECTION_FCL_EXPORT __declspec(dllexport) + #define COLLISION_DETECTION_FCL_IMPORT __declspec(dllimport) + #endif + #ifdef COLLISION_DETECTION_FCL_BUILDING_LIBRARY + #define COLLISION_DETECTION_FCL_PUBLIC COLLISION_DETECTION_FCL_EXPORT + #else + #define COLLISION_DETECTION_FCL_PUBLIC COLLISION_DETECTION_FCL_IMPORT + #endif + #define COLLISION_DETECTION_FCL_PUBLIC_TYPE COLLISION_DETECTION_FCL_PUBLIC + #define COLLISION_DETECTION_FCL_LOCAL +#else + #define COLLISION_DETECTION_FCL_EXPORT __attribute__ ((visibility("default"))) + #define COLLISION_DETECTION_FCL_IMPORT + #if __GNUC__ >= 4 + #define COLLISION_DETECTION_FCL_PUBLIC __attribute__ ((visibility("default"))) + #define COLLISION_DETECTION_FCL_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define COLLISION_DETECTION_FCL_PUBLIC + #define COLLISION_DETECTION_FCL_LOCAL + #endif + #define COLLISION_DETECTION_FCL_PUBLIC_TYPE +#endif + +#endif // COLLISION_DETECTION_FCL__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index 0f4e5dd710..4fc58485d7 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(${MOVEIT_LIB_NAME} SHARED ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + ament_target_dependencies(${MOVEIT_LIB_NAME} Boost urdfdom diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 8d4a7201f3..9abaf6368a 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -12,6 +12,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} urdf 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 752136d7d7..2856d5698e 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 @@ -141,7 +141,7 @@ MOVEIT_CLASS_FORWARD(KinematicsBase) // Defines KinematicsBasePtr, ConstPtr, We * @class KinematicsBase * @brief Provides an interface for kinematics solvers. */ -class MOVEIT_CORE_PUBLIC KinematicsBase +class KinematicsBase { public: static const rclcpp::Logger LOGGER; @@ -420,6 +420,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase * @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) */ + MOVEIT_CORE_PUBLIC bool setRedundantJoints(const std::vector& redundant_joint_names); /** @@ -461,6 +462,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase /** * @brief Set the search discretization value for all the redundant joints */ + MOVEIT_CORE_PUBLIC void setSearchDiscretization(double sd) { redundant_joint_discretization_.clear(); @@ -475,6 +477,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase * * @param discretization a map of joint indices and discretization value pairs. */ + MOVEIT_CORE_PUBLIC void setSearchDiscretization(const std::map& discretization) { redundant_joint_discretization_.clear(); @@ -489,9 +492,11 @@ class MOVEIT_CORE_PUBLIC KinematicsBase /** * @brief Get the value of the search discretization */ + MOVEIT_CORE_PUBLIC double getSearchDiscretization(int joint_index = 0) const { if (redundant_joint_discretization_.count(joint_index) > 0) + { return redundant_joint_discretization_.at(joint_index); } @@ -505,6 +510,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase * @brief Returns the set of supported kinematics discretization search types. This implementation only supports * the DiscretizationMethods::ONE search. */ + MOVEIT_CORE_PUBLIC std::vector getSupportedDiscretizationMethods() const { return supported_methods_; @@ -512,6 +518,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase /** @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) */ + MOVEIT_CORE_PUBLIC void setDefaultTimeout(double timeout) { default_timeout_ = timeout; @@ -519,6 +526,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase /** @brief For functions that require a timeout specified but one is not specified using arguments, this default timeout is used */ + MOVEIT_CORE_PUBLIC double getDefaultTimeout() const { return default_timeout_; @@ -529,6 +537,7 @@ class MOVEIT_CORE_PUBLIC KinematicsBase */ virtual ~KinematicsBase(); + MOVEIT_CORE_PUBLIC KinematicsBase(); protected: diff --git a/moveit_core/package.xml b/moveit_core/package.xml index b8e9456950..d820c74256 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -51,6 +51,7 @@ tf2_geometry_msgs trajectory_msgs visualization_msgs + pluginlib moveit_resources_panda_moveit_config moveit_resources_pr2_description diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 46728f63b2..22bbb32ceb 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/planning_response.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_msgs urdf diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index fc1f7d788f..07dca2cffe 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -12,6 +12,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} OCTOMAP ) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_robot_state diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index b0c07c0896..17208c3500 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -2,6 +2,11 @@ set(MOVEIT_LIB_NAME moveit_robot_trajectory) add_library(${MOVEIT_LIB_NAME} SHARED src/robot_trajectory.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "ROBOT_TRAJECTORY_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp urdfdom 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 f5168b7fe8..7a48ce0abe 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 @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include #include @@ -55,7 +56,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory) // Defines RobotTrajectoryPtr, ConstPtr, /** \brief Maintain a sequence of waypoints and the time durations between these waypoints */ -class RobotTrajectory +class ROBOT_TRAJECTORY_PUBLIC RobotTrajectory { public: RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group); diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/visibility_control.hpp b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/visibility_control.hpp new file mode 100644 index 0000000000..b1f1133032 --- /dev/null +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef ROBOT_TRAJECTORY__VISIBILITY_CONTROL_HPP_ +#define ROBOT_TRAJECTORY__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROBOT_TRAJECTORY_EXPORT __attribute__ ((dllexport)) + #define ROBOT_TRAJECTORY_IMPORT __attribute__ ((dllimport)) + #else + #define ROBOT_TRAJECTORY_EXPORT __declspec(dllexport) + #define ROBOT_TRAJECTORY_IMPORT __declspec(dllimport) + #endif + #ifdef ROBOT_TRAJECTORY_BUILDING_LIBRARY + #define ROBOT_TRAJECTORY_PUBLIC ROBOT_TRAJECTORY_EXPORT + #else + #define ROBOT_TRAJECTORY_PUBLIC ROBOT_TRAJECTORY_IMPORT + #endif + #define ROBOT_TRAJECTORY_PUBLIC_TYPE ROBOT_TRAJECTORY_PUBLIC + #define ROBOT_TRAJECTORY_LOCAL +#else + #define ROBOT_TRAJECTORY_EXPORT __attribute__ ((visibility("default"))) + #define ROBOT_TRAJECTORY_IMPORT + #if __GNUC__ >= 4 + #define ROBOT_TRAJECTORY_PUBLIC __attribute__ ((visibility("default"))) + #define ROBOT_TRAJECTORY_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROBOT_TRAJECTORY_PUBLIC + #define ROBOT_TRAJECTORY_LOCAL + #endif + #define ROBOT_TRAJECTORY_PUBLIC_TYPE +#endif + +#endif // ROBOT_TRAJECTORY__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index 5a62f8d0ff..f7c03a1dc3 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp rmw_implementation 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 0ee4d13f41..3a19de41aa 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 @@ -37,6 +37,7 @@ */ #pragma once +#define _USE_MATH_DEFINES #include #include diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index f283b0084a..3776fbeec7 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED ) ament_target_dependencies(${MOVEIT_LIB_NAME} Boost moveit_msgs) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt index 50cbd7864f..d0a024ef28 100644 --- a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_collision_plugin_loader) add_library(${MOVEIT_LIB_NAME} SHARED src/collision_plugin_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core rclcpp From b6004c91c9096af403181547ecaf0600be153a3c Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 14 Jul 2020 16:03:01 -0700 Subject: [PATCH 04/26] collision_detection_fcl symbol visibility. Moveit_core now builds. --- .../collision_detection/collision_plugin.h | 2 +- .../collision_detection_fcl/CMakeLists.txt | 8 +++++- .../collision_common.h | 7 ----- .../collision_detector_allocator_fcl.h | 2 +- .../collision_detector_fcl_plugin_loader.h | 2 +- .../collision_env_fcl.h | 2 +- .../visibility_control.hpp | 28 +++++++++++++++++++ 7 files changed, 39 insertions(+), 12 deletions(-) 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 0e55f2c397..10b115bc47 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 @@ -75,7 +75,7 @@ MOVEIT_CLASS_FORWARD(CollisionPlugin) // Defines CollisionPluginPtr, ConstPtr, * }; * */ -class COLLISION_DETECTION_PUBLIC CollisionPlugin +class CollisionPlugin { public: CollisionPlugin() diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index 5e3bc01969..a1ea30ead4 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/collision_env_fcl.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp rmw_implementation @@ -20,6 +21,11 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection ) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "COLLISION_DETECTION_FCL_BUILDING_LIBRARY") + add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp) set_target_properties(collision_detector_fcl_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) @@ -29,7 +35,7 @@ target_compile_definitions(${MOVEIT_LIB_NAME} PRIVATE "COLLISION_DETECTION_FCL_BUILDING_LIBRARY") - ament_target_dependencies(collision_detector_fcl_plugin +ament_target_dependencies(collision_detector_fcl_plugin rclcpp urdf visualization_msgs 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 a91e2201d7..dfb32834b8 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 @@ -288,38 +288,31 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist); /** \brief Create new FCLGeometry object out of robot link model. */ -COLLISION_DETECTION_FCL_PUBLIC FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link, int shape_index); /** \brief Create new FCLGeometry object out of attached body. */ -COLLISION_DETECTION_FCL_PUBLIC 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. */ -COLLISION_DETECTION_FCL_PUBLIC FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj); /** \brief Create new scaled and / or padded FCLGeometry object out of robot link model. */ -COLLISION_DETECTION_FCL_PUBLIC 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. */ -COLLISION_DETECTION_FCL_PUBLIC 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. */ -COLLISION_DETECTION_FCL_PUBLIC 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. */ -COLLISION_DETECTION_FCL_PUBLIC void cleanCollisionGeometryCache(); /** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */ 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 6c8389ef7f..c9f98b3b47 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 @@ -47,6 +47,6 @@ class COLLISION_DETECTION_FCL_PUBLIC CollisionDetectorAllocatorFCL : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_env_fcl.cpp + static COLLISION_DETECTION_FCL_PUBLIC 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 0f7924f9c7..c1decc0041 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 @@ -42,7 +42,7 @@ namespace collision_detection { -class COLLISION_DETECTION_FCL_PUBLIC CollisionDetectorFCLPluginLoader : public CollisionPlugin +class COLLISION_DETECTION_FCL_PLUGIN_PUBLIC CollisionDetectorFCLPluginLoader : public CollisionPlugin { public: bool initialize(const planning_scene::PlanningScenePtr& scene) const override; 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 9588d5e648..c3e81354c3 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 @@ -50,7 +50,7 @@ namespace collision_detection { /** \brief FCL implementation of the CollisionEnv */ -class COLLISION_DETECTION_FCL_PUBLIC CollisionEnvFCL : public CollisionEnv +class CollisionEnvFCL : public CollisionEnv { public: CollisionEnvFCL() = delete; diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp index d09be6c45c..28ca4870b7 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/visibility_control.hpp @@ -67,4 +67,32 @@ #define COLLISION_DETECTION_FCL_PUBLIC_TYPE #endif +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define COLLISION_DETECTION_FCL_PLUGIN_EXPORT __attribute__ ((dllexport)) + #define COLLISION_DETECTION_FCL_PLUGIN_IMPORT __attribute__ ((dllimport)) + #else + #define COLLISION_DETECTION_FCL_PLUGIN_EXPORT __declspec(dllexport) + #define COLLISION_DETECTION_FCL_PLUGIN_IMPORT __declspec(dllimport) + #endif + #ifdef COLLISION_DETECTION_FCL_PLUGIN_BUILDING_LIBRARY + #define COLLISION_DETECTION_FCL_PLUGIN_PUBLIC COLLISION_DETECTION_FCL_PLUGIN_EXPORT + #else + #define COLLISION_DETECTION_FCL_PLUGIN_PUBLIC COLLISION_DETECTION_FCL_PLUGIN_IMPORT + #endif + #define COLLISION_DETECTION_FCL_PLUGIN_PUBLIC_TYPE COLLISION_DETECTION_FCL_PLUGIN_PUBLIC + #define COLLISION_DETECTION_FCL_PLUGIN_LOCAL +#else + #define COLLISION_DETECTION_FCL_PLUGIN_EXPORT __attribute__ ((visibility("default"))) + #define COLLISION_DETECTION_FCL_PLUGIN_IMPORT + #if __GNUC__ >= 4 + #define COLLISION_DETECTION_FCL_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) + #define COLLISION_DETECTION_FCL_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define COLLISION_DETECTION_FCL_PLUGIN_PUBLIC + #define COLLISION_DETECTION_FCL_PLUGIN_LOCAL + #endif + #define COLLISION_DETECTION_FCL_PLUGIN_PUBLIC_TYPE +#endif + #endif // COLLISION_DETECTION_FCL__VISIBILITY_CONTROL_HPP_ \ No newline at end of file From 0325aed4d15e6ea4896cf7e08e982a1ba3f4cfe6 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 21 Jul 2020 02:00:20 -0700 Subject: [PATCH 05/26] More symbol visibility fixes and cleanups of earlier commits. Milestone before team hack day. --- .../background_processing/CMakeLists.txt | 1 + .../collision_distance_field/CMakeLists.txt | 1 + .../constraint_samplers/CMakeLists.txt | 1 + moveit_core/dynamics_solver/CMakeLists.txt | 1 + moveit_core/exceptions/CMakeLists.txt | 3 +- .../include/moveit/exceptions/exceptions.h | 5 +- moveit_core/kinematics_base/CMakeLists.txt | 3 +- .../moveit/kinematics_base/kinematics_base.h | 13 +- .../kinematics_base/visibility_control.hpp | 98 +++ moveit_core/kinematics_metrics/CMakeLists.txt | 1 + .../planning_request_adapter/CMakeLists.txt | 1 + moveit_core/planning_scene/CMakeLists.txt | 7 +- .../moveit/planning_scene/planning_scene.h | 5 +- .../planning_scene/visibility_control.hpp | 98 +++ moveit_core/profiler/CMakeLists.txt | 4 +- .../include/moveit/profiler/profiler.h | 5 +- moveit_core/robot_model/CMakeLists.txt | 1 + .../include/moveit/robot_model/aabb.h | 3 +- .../moveit/robot_model/fixed_joint_model.h | 3 +- .../moveit/robot_model/floating_joint_model.h | 3 +- .../include/moveit/robot_model/joint_model.h | 39 +- .../moveit/robot_model/joint_model_group.h | 73 ++- .../include/moveit/robot_model/link_model.h | 3 +- .../moveit/robot_model/planar_joint_model.h | 3 +- .../robot_model/prismatic_joint_model.h | 3 +- .../moveit/robot_model/revolute_joint_model.h | 3 +- .../include/moveit/robot_model/robot_model.h | 3 +- moveit_core/robot_model/src/joint_model.cpp | 34 + .../robot_model/src/joint_model_group.cpp | 72 --- .../robot_model/src/revolute_joint_model.cpp | 2 +- moveit_core/robot_state/CMakeLists.txt | 5 + .../include/moveit/robot_state/conversions.h | 11 - .../include/moveit/robot_state/robot_state.h | 593 ++++++++++++++--- .../robot_state}/visibility_control.hpp | 38 +- moveit_core/robot_state/src/robot_state.cpp | 602 ------------------ moveit_core/transforms/CMakeLists.txt | 1 + .../include/moveit/transforms/transforms.h | 3 +- .../v2/kinematics_cache/CMakeLists.txt | 1 + .../lma_kinematics_plugin/CMakeLists.txt | 1 + .../srv_kinematics_plugin/CMakeLists.txt | 1 + .../ompl/ompl_interface/CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + moveit_ros/benchmarks/CMakeLists.txt | 1 + .../occupancy_map_monitor/CMakeLists.txt | 15 + .../CMakeLists.txt | 1 + .../kinematics_plugin_loader/CMakeLists.txt | 1 + .../planning/plan_execution/CMakeLists.txt | 1 + .../planning/planning_pipeline/CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + .../planning_scene_monitor/CMakeLists.txt | 6 + .../planning_scene_monitor.h | 14 +- .../visibility_control.hpp | 70 ++ moveit_ros/planning/rdf_loader/CMakeLists.txt | 1 + .../planning/rdf_loader/src/rdf_loader.cpp | 8 + .../robot_model_loader/CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + .../trajectory_execution_manager.h | 1 + 57 files changed, 971 insertions(+), 902 deletions(-) create mode 100644 moveit_core/kinematics_base/include/moveit/kinematics_base/visibility_control.hpp create mode 100644 moveit_core/planning_scene/include/moveit/planning_scene/visibility_control.hpp rename moveit_core/{macros/include/moveit/macros => robot_state/include/moveit/robot_state}/visibility_control.hpp (70%) create mode 100644 moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/visibility_control.hpp diff --git a/moveit_core/background_processing/CMakeLists.txt b/moveit_core/background_processing/CMakeLists.txt index f341c44af0..23b757bd54 100644 --- a/moveit_core/background_processing/CMakeLists.txt +++ b/moveit_core/background_processing/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_background_processing) add_library(${MOVEIT_LIB_NAME} SHARED src/background_processing.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} Boost rclcpp diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 5640978639..9f5f8be14d 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/collision_env_hybrid.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} urdf diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 370267389f..fdd5510622 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/union_constraint_sampler.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index d8ac1f4ebb..eeb09b3aae 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_dynamics_solver) add_library(${MOVEIT_LIB_NAME} SHARED src/dynamics_solver.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} urdf diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt index a14ec8c781..875a7ff02a 100644 --- a/moveit_core/exceptions/CMakeLists.txt +++ b/moveit_core/exceptions/CMakeLists.txt @@ -8,7 +8,8 @@ target_compile_definitions(${MOVEIT_LIB_NAME} PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(${MOVEIT_LIB_NAME} + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + ament_target_dependencies(${MOVEIT_LIB_NAME} Boost rclcpp urdfdom diff --git a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h index d4d778958f..1e4d28ddb9 100644 --- a/moveit_core/exceptions/include/moveit/exceptions/exceptions.h +++ b/moveit_core/exceptions/include/moveit/exceptions/exceptions.h @@ -37,20 +37,19 @@ #pragma once #include -#include /** \brief Main namespace for MoveIt */ namespace moveit { /** \brief This may be thrown during construction of objects if errors occur */ -class MOVEIT_CORE_PUBLIC ConstructException : public std::runtime_error +class ConstructException : public std::runtime_error { public: explicit ConstructException(const std::string& what_arg); }; /** \brief This may be thrown if unrecoverable errors occur */ -class MOVEIT_CORE_PUBLIC Exception : public std::runtime_error +class Exception : public std::runtime_error { public: explicit Exception(const std::string& what_arg); diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index 5c0e98b9e3..af68bd2de8 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -3,11 +3,12 @@ set(MOVEIT_LIB_NAME moveit_kinematics_base) add_library(${MOVEIT_LIB_NAME} SHARED src/kinematics_base.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${MOVEIT_LIB_NAME} - PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") + PRIVATE "KINEMATICS_BASE_BUILDING_LIBRARY") # This line is needed to ensure that messages are done being built before this is built ament_target_dependencies( 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 2856d5698e..158e710d47 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 @@ -39,10 +39,10 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include #include -#include namespace moveit { @@ -141,7 +141,7 @@ MOVEIT_CLASS_FORWARD(KinematicsBase) // Defines KinematicsBasePtr, ConstPtr, We * @class KinematicsBase * @brief Provides an interface for kinematics solvers. */ -class KinematicsBase +class KINEMATICS_BASE_PUBLIC KinematicsBase { public: static const rclcpp::Logger LOGGER; @@ -420,7 +420,6 @@ class KinematicsBase * @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) */ - MOVEIT_CORE_PUBLIC bool setRedundantJoints(const std::vector& redundant_joint_names); /** @@ -462,7 +461,6 @@ class KinematicsBase /** * @brief Set the search discretization value for all the redundant joints */ - MOVEIT_CORE_PUBLIC void setSearchDiscretization(double sd) { redundant_joint_discretization_.clear(); @@ -477,7 +475,6 @@ class KinematicsBase * * @param discretization a map of joint indices and discretization value pairs. */ - MOVEIT_CORE_PUBLIC void setSearchDiscretization(const std::map& discretization) { redundant_joint_discretization_.clear(); @@ -492,11 +489,9 @@ class KinematicsBase /** * @brief Get the value of the search discretization */ - MOVEIT_CORE_PUBLIC double getSearchDiscretization(int joint_index = 0) const { if (redundant_joint_discretization_.count(joint_index) > 0) - { return redundant_joint_discretization_.at(joint_index); } @@ -510,7 +505,6 @@ class KinematicsBase * @brief Returns the set of supported kinematics discretization search types. This implementation only supports * the DiscretizationMethods::ONE search. */ - MOVEIT_CORE_PUBLIC std::vector getSupportedDiscretizationMethods() const { return supported_methods_; @@ -518,7 +512,6 @@ class KinematicsBase /** @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) */ - MOVEIT_CORE_PUBLIC void setDefaultTimeout(double timeout) { default_timeout_ = timeout; @@ -526,7 +519,6 @@ class KinematicsBase /** @brief For functions that require a timeout specified but one is not specified using arguments, this default timeout is used */ - MOVEIT_CORE_PUBLIC double getDefaultTimeout() const { return default_timeout_; @@ -537,7 +529,6 @@ class KinematicsBase */ virtual ~KinematicsBase(); - MOVEIT_CORE_PUBLIC KinematicsBase(); protected: diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/visibility_control.hpp b/moveit_core/kinematics_base/include/moveit/kinematics_base/visibility_control.hpp new file mode 100644 index 0000000000..0624e858b7 --- /dev/null +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/visibility_control.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef KINEMATICS_BASE__VISIBILITY_CONTROL_HPP_ +#define KINEMATICS_BASE__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define KINEMATICS_BASE_EXPORT __attribute__ ((dllexport)) + #define KINEMATICS_BASE_IMPORT __attribute__ ((dllimport)) + #else + #define KINEMATICS_BASE_EXPORT __declspec(dllexport) + #define KINEMATICS_BASE_IMPORT __declspec(dllimport) + #endif + #ifdef KINEMATICS_BASE_BUILDING_LIBRARY + #define KINEMATICS_BASE_PUBLIC KINEMATICS_BASE_EXPORT + #else + #define KINEMATICS_BASE_PUBLIC KINEMATICS_BASE_IMPORT + #endif + #define KINEMATICS_BASE_PUBLIC_TYPE KINEMATICS_BASE_PUBLIC + #define KINEMATICS_BASE_LOCAL +#else + #define KINEMATICS_BASE_EXPORT __attribute__ ((visibility("default"))) + #define KINEMATICS_BASE_IMPORT + #if __GNUC__ >= 4 + #define KINEMATICS_BASE_PUBLIC __attribute__ ((visibility("default"))) + #define KINEMATICS_BASE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define KINEMATICS_BASE_PUBLIC + #define KINEMATICS_BASE_LOCAL + #endif + #define KINEMATICS_BASE_PUBLIC_TYPE +#endif + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define KINEMATICS_BASE_PLUGIN_EXPORT __attribute__ ((dllexport)) + #define KINEMATICS_BASE_PLUGIN_IMPORT __attribute__ ((dllimport)) + #else + #define KINEMATICS_BASE_PLUGIN_EXPORT __declspec(dllexport) + #define KINEMATICS_BASE_PLUGIN_IMPORT __declspec(dllimport) + #endif + #ifdef KINEMATICS_BASE_PLUGIN_BUILDING_LIBRARY + #define KINEMATICS_BASE_PLUGIN_PUBLIC KINEMATICS_BASE_PLUGIN_EXPORT + #else + #define KINEMATICS_BASE_PLUGIN_PUBLIC KINEMATICS_BASE_PLUGIN_IMPORT + #endif + #define KINEMATICS_BASE_PLUGIN_PUBLIC_TYPE KINEMATICS_BASE_PLUGIN_PUBLIC + #define KINEMATICS_BASE_PLUGIN_LOCAL +#else + #define KINEMATICS_BASE_PLUGIN_EXPORT __attribute__ ((visibility("default"))) + #define KINEMATICS_BASE_PLUGIN_IMPORT + #if __GNUC__ >= 4 + #define KINEMATICS_BASE_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) + #define KINEMATICS_BASE_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define KINEMATICS_BASE_PLUGIN_PUBLIC + #define KINEMATICS_BASE_PLUGIN_LOCAL + #endif + #define KINEMATICS_BASE_PLUGIN_PUBLIC_TYPE +#endif + +#endif // KINEMATICS_BASE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index 8f23eef19c..f630eca11d 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_kinematics_metrics) add_library(${MOVEIT_LIB_NAME} SHARED src/kinematics_metrics.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} urdf diff --git a/moveit_core/planning_request_adapter/CMakeLists.txt b/moveit_core/planning_request_adapter/CMakeLists.txt index 2dbb50caa2..76427b91d3 100644 --- a/moveit_core/planning_request_adapter/CMakeLists.txt +++ b/moveit_core/planning_request_adapter/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_planning_request_adapter) add_library(${MOVEIT_LIB_NAME} SHARED src/planning_request_adapter.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp rmw_implementation diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 07dca2cffe..931b984ac4 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -3,7 +3,12 @@ set(MOVEIT_LIB_NAME moveit_planning_scene) add_library(${MOVEIT_LIB_NAME} SHARED src/planning_scene.cpp) #TODO: Fix the versioning set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(${MOVEIT_LIB_NAME} +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "PLANNING_SCENE_BUILDING_LIBRARY") + + ament_target_dependencies(${MOVEIT_LIB_NAME} Boost rclcpp urdfdom 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 d499a6e7be..4b29dfcd38 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 @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -99,8 +100,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model, const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World())); - static const std::string OCTOMAP_NS; - static const std::string DEFAULT_SCENE_NAME; + PLANNING_SCENE_PUBLIC static const std::string OCTOMAP_NS; + PLANNING_SCENE_PUBLIC static const std::string DEFAULT_SCENE_NAME; ~PlanningScene(); diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/visibility_control.hpp b/moveit_core/planning_scene/include/moveit/planning_scene/visibility_control.hpp new file mode 100644 index 0000000000..5294b1b6cd --- /dev/null +++ b/moveit_core/planning_scene/include/moveit/planning_scene/visibility_control.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef PLANNING_SCENE__VISIBILITY_CONTROL_HPP_ +#define PLANNING_SCENE__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define PLANNING_SCENE_EXPORT __attribute__ ((dllexport)) + #define PLANNING_SCENE_IMPORT __attribute__ ((dllimport)) + #else + #define PLANNING_SCENE_EXPORT __declspec(dllexport) + #define PLANNING_SCENE_IMPORT __declspec(dllimport) + #endif + #ifdef PLANNING_SCENE_BUILDING_LIBRARY + #define PLANNING_SCENE_PUBLIC PLANNING_SCENE_EXPORT + #else + #define PLANNING_SCENE_PUBLIC PLANNING_SCENE_IMPORT + #endif + #define PLANNING_SCENE_PUBLIC_TYPE PLANNING_SCENE_PUBLIC + #define PLANNING_SCENE_LOCAL +#else + #define PLANNING_SCENE_EXPORT __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_IMPORT + #if __GNUC__ >= 4 + #define PLANNING_SCENE_PUBLIC __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define PLANNING_SCENE_PUBLIC + #define PLANNING_SCENE_LOCAL + #endif + #define PLANNING_SCENE_PUBLIC_TYPE +#endif + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define PLANNING_SCENE_PLUGIN_EXPORT __attribute__ ((dllexport)) + #define PLANNING_SCENE_PLUGIN_IMPORT __attribute__ ((dllimport)) + #else + #define PLANNING_SCENE_PLUGIN_EXPORT __declspec(dllexport) + #define PLANNING_SCENE_PLUGIN_IMPORT __declspec(dllimport) + #endif + #ifdef PLANNING_SCENE_PLUGIN_BUILDING_LIBRARY + #define PLANNING_SCENE_PLUGIN_PUBLIC PLANNING_SCENE_PLUGIN_EXPORT + #else + #define PLANNING_SCENE_PLUGIN_PUBLIC PLANNING_SCENE_PLUGIN_IMPORT + #endif + #define PLANNING_SCENE_PLUGIN_PUBLIC_TYPE PLANNING_SCENE_PLUGIN_PUBLIC + #define PLANNING_SCENE_PLUGIN_LOCAL +#else + #define PLANNING_SCENE_PLUGIN_EXPORT __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_PLUGIN_IMPORT + #if __GNUC__ >= 4 + #define PLANNING_SCENE_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define PLANNING_SCENE_PLUGIN_PUBLIC + #define PLANNING_SCENE_PLUGIN_LOCAL + #endif + #define PLANNING_SCENE_PLUGIN_PUBLIC_TYPE +#endif + +#endif // PLANNING_SCENE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/profiler/CMakeLists.txt b/moveit_core/profiler/CMakeLists.txt index 7d4430bc0e..042dc5a1e4 100644 --- a/moveit_core/profiler/CMakeLists.txt +++ b/moveit_core/profiler/CMakeLists.txt @@ -2,12 +2,14 @@ set(MOVEIT_LIB_NAME moveit_profiler) add_library(${MOVEIT_LIB_NAME} SHARED src/profiler.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${MOVEIT_LIB_NAME} PRIVATE "MOVEIT_CORE_BUILDING_LIBRARY") - ament_target_dependencies(${MOVEIT_LIB_NAME} +ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp rmw_implementation urdfdom diff --git a/moveit_core/profiler/include/moveit/profiler/profiler.h b/moveit_core/profiler/include/moveit/profiler/profiler.h index 348a5d6948..3ea38f58fe 100644 --- a/moveit_core/profiler/include/moveit/profiler/profiler.h +++ b/moveit_core/profiler/include/moveit/profiler/profiler.h @@ -59,7 +59,6 @@ #include #include #include -#include namespace moveit { @@ -70,7 +69,7 @@ namespace tools external profiling tools in that it allows the user to count time spent in various bits of code (sub-function granularity) or count how many times certain pieces of code are executed.*/ -class MOVEIT_CORE_PUBLIC Profiler : private boost::noncopyable +class Profiler : private boost::noncopyable { public: /** \brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. @@ -325,7 +324,7 @@ namespace moveit { namespace tools { -class MOVEIT_CORE_PUBLIC Profiler +class Profiler { public: class ScopedBlock diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index f34597dfce..c8770968fd 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -14,6 +14,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/robot_model.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. 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 b9fd2cc2dc..ce0315fdd2 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -37,14 +37,13 @@ #pragma once #include -#include namespace moveit { namespace core { /** \brief Represents an axis-aligned bounding box. */ -class MOVEIT_CORE_PUBLIC AABB : public Eigen::AlignedBox3d +class AABB : public Eigen::AlignedBox3d { public: /** \brief Extend with a box transformed by the given transform. */ 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 1c6c4e180c..855cbec6c7 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 @@ -37,14 +37,13 @@ #pragma once #include -#include namespace moveit { namespace core { /** \brief A fixed joint */ -class MOVEIT_CORE_PUBLIC FixedJointModel : public JointModel +class FixedJointModel : public JointModel { public: FixedJointModel(const std::string& name); 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 5ad278d4a1..0d0e0a60cc 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 @@ -37,14 +37,13 @@ #pragma once #include -#include namespace moveit { namespace core { /** \brief A floating joint */ -class MOVEIT_CORE_PUBLIC FloatingJointModel : public JointModel +class FloatingJointModel : public JointModel { public: FloatingJointModel(const std::string& name); 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 64a4a0477e..eba749c23e 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 @@ -44,7 +44,6 @@ #include #include #include -#include namespace moveit { @@ -105,7 +104,7 @@ using JointModelMapConst = std::map; and 'y' will store its full variable names as 'base/x' and 'base/y'). Local names are never used to reference variables directly. */ -class MOVEIT_CORE_PUBLIC JointModel +class JointModel { public: /** \brief The different types of joints we support */ @@ -125,7 +124,7 @@ class MOVEIT_CORE_PUBLIC JointModel /** \brief Construct a joint named \e name */ JointModel(const std::string& name); - virtual ~JointModel() {} + virtual ~JointModel(); /** \brief Get the name of the joint */ const std::string& getName() const @@ -299,11 +298,7 @@ class MOVEIT_CORE_PUBLIC JointModel /** 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 - { - return false; - } - + virtual bool harmonizePosition(double* values, const Bounds& other_bounds) const; bool harmonizePosition(double* values) const { return harmonizePosition(values, variable_bounds_); @@ -316,15 +311,7 @@ class MOVEIT_CORE_PUBLIC JointModel } /** \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 - { - for (std::size_t i = 0; i < other_bounds.size(); ++i) - if (other_bounds[i].max_velocity_ + margin < values[i]) - return false; - else if (other_bounds[i].min_velocity_ - margin > values[i]) - return false; - return true; - } + 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 @@ -333,22 +320,7 @@ class MOVEIT_CORE_PUBLIC JointModel } /** \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 - { - bool change = false; - for (std::size_t i = 0; i < other_bounds.size(); ++i) - if (other_bounds[i].max_velocity_ < values[i]) - { - values[i] = other_bounds[i].max_velocity_; - change = true; - } - else if (other_bounds[i].min_velocity_ > values[i]) - { - values[i] = other_bounds[i].min_velocity_; - change = true; - } - return change; - } + virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) 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; @@ -547,7 +519,6 @@ class MOVEIT_CORE_PUBLIC JointModel }; /** \brief Operator overload for printing variable bounds to a stream */ -MOVEIT_CORE_PUBLIC 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 91af82ca2f..256370877b 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 @@ -43,7 +43,6 @@ #include #include #include -#include namespace moveit { @@ -66,7 +65,7 @@ using JointModelGroupMapConst = std::map; using JointBoundsVector = std::vector; -class MOVEIT_CORE_PUBLIC JointModelGroup +class JointModelGroup { public: struct KinematicsSolver @@ -306,37 +305,64 @@ class MOVEIT_CORE_PUBLIC JointModelGroup void getVariableDefaultPositions(std::map& values) const; /** \brief Compute the default values for the joint group */ - void getVariableDefaultPositions(std::vector& values) const; + 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; + 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; + 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; - + 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; + 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; + 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; - + 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; + 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; @@ -356,17 +382,23 @@ class MOVEIT_CORE_PUBLIC JointModelGroup const JointBoundsVector& active_joint_bounds, const double* near, const std::vector& distances) const; - bool enforcePositionBounds(double* state) 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; - + 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; - + 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; @@ -494,7 +526,10 @@ class MOVEIT_CORE_PUBLIC JointModelGroup } void setSolverAllocators(const SolverAllocatorFn& solver, - const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn()); + const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn()) + { + setSolverAllocators(std::make_pair(solver, solver_map)); + } void setSolverAllocators(const std::pair& solvers); 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 e2308be8da..9c6d7ba85e 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 @@ -44,7 +44,6 @@ #include #include #include -#include namespace shapes { @@ -69,7 +68,7 @@ using LinkTransformMap = std::map > >; /** \brief A link from the robot. Contains the constant transform applied to the link and its geometry */ -class MOVEIT_CORE_PUBLIC LinkModel +class LinkModel { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 45d74ba8e2..4d947f0530 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 @@ -37,14 +37,13 @@ #pragma once #include -#include namespace moveit { namespace core { /** \brief A planar joint */ -class MOVEIT_CORE_PUBLIC PlanarJointModel : public JointModel +class PlanarJointModel : public JointModel { public: /** \brief different types of planar joints we support */ 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 ac82b654d3..ece60ffe9c 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 @@ -37,14 +37,13 @@ #pragma once #include -#include namespace moveit { namespace core { /** \brief A prismatic joint */ -class MOVEIT_CORE_PUBLIC PrismaticJointModel : public JointModel +class PrismaticJointModel : public JointModel { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 89093a68bf..9d70cf0628 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 @@ -37,14 +37,13 @@ #pragma once #include -#include namespace moveit { namespace core { /** \brief A revolute joint */ -class MOVEIT_CORE_PUBLIC RevoluteJointModel : public JointModel +class RevoluteJointModel : public JointModel { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 1734d7ee5d..6a1e6f2fb2 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 @@ -48,7 +48,6 @@ #include #include #include -#include #include #include @@ -62,7 +61,7 @@ MOVEIT_CLASS_FORWARD(RobotModel) // Defines RobotModelPtr, ConstPtr, WeakPtr... /** \brief Definition of a kinematic model. This class is not thread safe, however multiple instances can be created */ -class MOVEIT_CORE_PUBLIC RobotModel +class RobotModel { public: /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index 5adbcfc906..ff4a549104 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -59,6 +59,8 @@ JointModel::JointModel(const std::string& name) { } +JointModel::~JointModel() = default; + std::string JointModel::getTypeName() const { switch (type_) @@ -88,6 +90,38 @@ int JointModel::getLocalVariableIndex(const std::string& variable) const return it->second; } +bool JointModel::harmonizePosition(double* values, const Bounds& other_bounds) const +{ + return false; +} + +bool JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const +{ + bool change = false; + for (std::size_t i = 0; i < other_bounds.size(); ++i) + if (other_bounds[i].max_velocity_ < values[i]) + { + values[i] = other_bounds[i].max_velocity_; + change = true; + } + else if (other_bounds[i].min_velocity_ > values[i]) + { + values[i] = other_bounds[i].min_velocity_; + change = true; + } + return change; +} + +bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const +{ + for (std::size_t i = 0; i < other_bounds.size(); ++i) + if (other_bounds[i].max_velocity_ + margin < values[i]) + return false; + else if (other_bounds[i].min_velocity_ - margin > values[i]) + return false; + return true; +} + const VariableBounds& JointModel::getVariableBounds(const std::string& variable) const { return variable_bounds_[getLocalVariableIndex(variable)]; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index ac8698b3f7..ec53d5b7e6 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -313,51 +313,6 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const return it->second; } -void JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const -{ - getVariableRandomPositions(rng, values, active_joint_models_bounds_); -} - -void JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector& values) const -{ - values.resize(variable_count_); - getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_); -} - -void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, - const double distance) const -{ - getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance); -} - -void JointModelGroup::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); -} - -void JointModelGroup::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); -} - -void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near, - const std::vector& distances) const -{ - getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distances); -} - -void JointModelGroup::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 JointModelGroup::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds) const { @@ -423,11 +378,6 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum updateMimicJoints(values); } -bool JointModelGroup::satisfiesPositionBounds(const double* state, double margin) const -{ - return satisfiesPositionBounds(state, active_joint_models_bounds_, margin); -} - bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds, double margin) const { @@ -439,11 +389,6 @@ bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBo return true; } -bool JointModelGroup::enforcePositionBounds(double* state) const -{ - return enforcePositionBounds(state, active_joint_models_bounds_); -} - bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const { assert(active_joint_bounds.size() == active_joint_model_vector_.size()); @@ -457,11 +402,6 @@ bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVect return change; } -double JointModelGroup::getMaximumExtent() const -{ - return getMaximumExtent(active_joint_models_bounds_); -} - double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const { double max_distance = 0.0; @@ -522,12 +462,6 @@ void JointModelGroup::getVariableDefaultPositions(double* values) const updateMimicJoints(values); } -void JointModelGroup::getVariableDefaultPositions(std::vector& values) const -{ - values.resize(variable_count_); - getVariableDefaultPositions(&values[0]); -} - void JointModelGroup::getVariableDefaultPositions(std::map& values) const { std::vector tmp(variable_count_); @@ -655,12 +589,6 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik return true; } -void JointModelGroup::setSolverAllocators(const SolverAllocatorFn& solver, - const SolverAllocatorMapFn& solver_map) -{ - setSolverAllocators(std::make_pair(solver, solver_map)); -} - void JointModelGroup::setSolverAllocators(const std::pair& solvers) { if (solvers.first) diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 9b6c2150d5..11651cd597 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -34,7 +34,7 @@ *********************************************************************/ /* Author: Ioan Sucan */ -#define _USE_MATH_DEFINES +#define _USE_MATH_DEFINES #include #include diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 789926e82f..794ef13e10 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -12,6 +12,11 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/cartesian_interpolator.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "ROBOT_STATE_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} urdf tf2_geometry_msgs 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 ffbc0c50ed..5129e9b7a2 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h +++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h @@ -50,7 +50,6 @@ namespace core * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ -MOVEIT_CORE_PUBLIC bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state); /** @@ -61,7 +60,6 @@ bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, Rob * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ -MOVEIT_CORE_PUBLIC bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies = true); @@ -72,7 +70,6 @@ bool robotStateMsgToRobotState(const Transforms& tf, const moveit_msgs::msg::Rob * @param copy_attached_bodies Flag to include attached objects in robot state copy * @return True if successful, false if failed for any reason */ -MOVEIT_CORE_PUBLIC bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, RobotState& state, bool copy_attached_bodies = true); @@ -82,7 +79,6 @@ bool robotStateMsgToRobotState(const moveit_msgs::msg::RobotState& robot_state, * @param robot_state The resultant RobotState *message * @param copy_attached_bodies Flag to include attached objects in robot state copy */ -MOVEIT_CORE_PUBLIC void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotState& robot_state, bool copy_attached_bodies = true); @@ -91,17 +87,14 @@ void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::msg::RobotS * @param attached_bodies The input MoveIt attached body objects * @param attached_collision_objs The resultant AttachedCollisionObject messages */ -MOVEIT_CORE_PUBLIC 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 */ -MOVEIT_CORE_PUBLIC void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state); /** @@ -111,7 +104,6 @@ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointS * @param state The resultant MoveIt robot state * @return True if successful, false if failed for any reason */ -MOVEIT_CORE_PUBLIC bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id, RobotState& state); @@ -123,7 +115,6 @@ bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& tra * @param include_header - flag to prefix the output with a line of joint names. * @param separator - allows to override the comma seperator with any symbol, such as a white space */ -MOVEIT_CORE_PUBLIC void robotStateToStream(const RobotState& state, std::ostream& out, bool include_header = true, const std::string& separator = ","); @@ -136,7 +127,6 @@ void robotStateToStream(const RobotState& state, std::ostream& out, bool include * @param include_header - flag to prefix the output with a line of joint names. * @param separator - allows to override the comma seperator with any symbol, such as a white space */ -MOVEIT_CORE_PUBLIC void robotStateToStream(const RobotState& state, std::ostream& out, const std::vector& joint_groups_ordering, bool include_header = true, const std::string& separator = ","); @@ -148,7 +138,6 @@ void robotStateToStream(const RobotState& state, std::ostream& out, * @param separator - allows to override the comma seperator with any symbol, such as a white space * \return true on success */ -MOVEIT_CORE_PUBLIC 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 49511c32c6..f4cb64c293 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 @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -86,7 +87,7 @@ typedef boost::functiongetVariableCount(); + } /** \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; + 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; + 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; + 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; + const JointModelGroup* getJointModelGroup(const std::string& group) const + { + return robot_model_->getJointModelGroup(group); + } /** \name Getting and setting variable position * @{ @@ -145,7 +165,11 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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); + 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. */ @@ -162,19 +186,37 @@ class MOVEIT_CORE_PUBLIC RobotState 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); + 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); + 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; + 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; + double getVariablePosition(int index) const + { + return position_[index]; + } /** @} */ @@ -185,24 +227,43 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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(); + double* getVariableVelocities() + { + markVelocity(); + return velocity_; + } /** \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; + const double* getVariableVelocities() const + { + return velocity_; + } /** \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); + void setVariableVelocities(const double* velocity) + { + has_velocity_ = true; + // assume everything is in order in terms of array lengths (for efficiency reasons) + memcpy(velocity_, 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); + 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. */ @@ -219,11 +280,18 @@ class MOVEIT_CORE_PUBLIC RobotState 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); - + 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); + 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 @@ -278,11 +346,22 @@ class MOVEIT_CORE_PUBLIC RobotState /** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this * state */ - void setVariableAccelerations(const double* acceleration); + 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(acceleration_, 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); + 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. */ @@ -339,25 +418,45 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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(); + double* getVariableEffort() + { + markEffort(); + return effort_; + } /** \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; + const double* getVariableEffort() const + { + return effort_; + } /** \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); + 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_, 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); + 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); @@ -371,19 +470,32 @@ class MOVEIT_CORE_PUBLIC RobotState 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); + 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); + void setVariableEffort(int index, double value) + { + markEffort(); + effort_[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; + double getVariableEffort(const std::string& variable) const + { + return effort_[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; + double getVariableEffort(int index) const + { + return effort_[index]; + } /** \brief Remove effort values from this state (this differs from setting them to zero) */ void dropEffort(); @@ -401,37 +513,87 @@ class MOVEIT_CORE_PUBLIC RobotState * See setVariablePositions(), setVariableVelocities(), setVariableEffort() to handle multiple joints. * @{ */ - void setJointPositions(const std::string& joint_name, const double* position); + 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); + 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); + 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); + void setJointPositions(const JointModel* joint, const double* position) + { + memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double)); + markDirtyJointTransforms(joint); + updateMimicJoint(joint); + } - void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform); + void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform) + { + setJointPositions(robot_model_->getJointModel(joint_name), transform); + } - void setJointVelocities(const JointModel* joint, const double* velocity); + void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform) + { + joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex()); + markDirtyJointTransforms(joint); + updateMimicJoint(joint); + } + + void setJointVelocities(const JointModel* joint, const double* velocity) + { + has_velocity_ = true; + memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double)); + } void setJointEfforts(const JointModel* joint, const double* effort); - const double* getJointPositions(const std::string& joint_name) const; + const double* getJointPositions(const std::string& joint_name) const + { + return getJointPositions(robot_model_->getJointModel(joint_name)); + } - const double* getJointPositions(const JointModel* joint) const; + const double* getJointPositions(const JointModel* joint) const + { + return position_ + joint->getFirstVariableIndex(); + } - const double* getJointVelocities(const std::string& joint_name) const; + const double* getJointVelocities(const std::string& joint_name) const + { + return getJointVelocities(robot_model_->getJointModel(joint_name)); + } - const double* getJointVelocities(const JointModel* joint) const; + const double* getJointVelocities(const JointModel* joint) const + { + return velocity_ + joint->getFirstVariableIndex(); + } - const double* getJointAccelerations(const std::string& joint_name) const; + const double* getJointAccelerations(const std::string& joint_name) const + { + return getJointAccelerations(robot_model_->getJointModel(joint_name)); + } - const double* getJointAccelerations(const JointModel* joint) const; + const double* getJointAccelerations(const JointModel* joint) const + { + return acceleration_ + joint->getFirstVariableIndex(); + } - const double* getJointEffort(const std::string& joint_name) const; + const double* getJointEffort(const std::string& joint_name) const + { + return getJointEffort(robot_model_->getJointModel(joint_name)); + } - const double* getJointEffort(const JointModel* joint) const; + const double* getJointEffort(const JointModel* joint) const + { + return effort_ + joint->getFirstVariableIndex(); + } /** @} */ @@ -441,7 +603,12 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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); + 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 */ @@ -522,17 +689,34 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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; + 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; + 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 @@ -542,7 +726,12 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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 @@ -557,15 +746,28 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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); + 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); + 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); + 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 */ @@ -573,8 +775,13 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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); - + 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); @@ -582,17 +789,34 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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; + 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; + 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 @@ -602,7 +826,12 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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 @@ -617,15 +846,28 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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); + 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); + 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); + 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 */ @@ -633,7 +875,12 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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); + 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 */ @@ -642,17 +889,34 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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; + 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; + 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 @@ -662,7 +926,12 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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; + 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 @@ -923,7 +1192,11 @@ class MOVEIT_CORE_PUBLIC RobotState /** \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 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, @@ -937,7 +1210,13 @@ class MOVEIT_CORE_PUBLIC RobotState * @{ */ - void setVariableValues(const sensor_msgs::msg::JointState& msg); + 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 @@ -1015,13 +1294,27 @@ class MOVEIT_CORE_PUBLIC RobotState * * The returned transformation is always a valid isometry. */ - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name); + const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) + { + return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); + } - const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link); + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) + { + updateLinkTransforms(); + return global_link_transforms_[link->getLinkIndex()]; + } - const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const; + 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; + const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const + { + BOOST_VERIFY(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. @@ -1033,21 +1326,55 @@ class MOVEIT_CORE_PUBLIC RobotState * @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); + 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); + 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; + 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; + const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const + { + BOOST_VERIFY(checkCollisionTransforms()); + return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; + } - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name); + 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 JointModel* joint) + { + const int idx = joint->getJointIndex(); + unsigned char& dirty = dirty_joint_transforms_[idx]; + if (dirty) + { + joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]); + dirty = 0; + } + return variable_joint_transforms_[idx]; + } - const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const; + 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; + const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const + { + BOOST_VERIFY(checkJointTransforms(joint)); + return variable_joint_transforms_[joint->getJointIndex()]; + } bool dirtyJointTransform(const JointModel* joint) const { @@ -1132,22 +1459,50 @@ class MOVEIT_CORE_PUBLIC RobotState void enforceBounds(); void enforceBounds(const JointModelGroup* joint_group); - void enforceBounds(const JointModel* joint); - - void enforcePositionBounds(const JointModel* joint); + void enforceBounds(const JointModel* joint) + { + enforcePositionBounds(joint); + if (has_velocity_) + enforceVelocityBounds(joint); + } + void enforcePositionBounds(const JointModel* joint) + { + if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex())) + { + markDirtyJointTransforms(joint); + updateMimicJoint(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 harmonizePosition(const JointModel* joint) + { + if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex())) + // no need to mark transforms dirty, as the transform hasn't changed + updateMimicJoint(joint); + } - void enforceVelocityBounds(const JointModel* joint); + void enforceVelocityBounds(const JointModel* joint) + { + joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex()); + } 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; - bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const; - bool satisfiesVelocityBounds(const JointModel* joint, 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. */ @@ -1239,7 +1594,11 @@ class MOVEIT_CORE_PUBLIC RobotState 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()); + const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap()) + { + std::set touch_links_set(touch_links.begin(), touch_links.end()); + attachBody(id, 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; @@ -1285,7 +1644,12 @@ class MOVEIT_CORE_PUBLIC RobotState } /** \brief Return the instance of a random number generator */ - random_numbers::RandomNumberGenerator& getRandomNumberGenerator(); + random_numbers::RandomNumberGenerator& getRandomNumberGenerator() + { + if (!rng_) + rng_ = new random_numbers::RandomNumberGenerator(); + return *rng_; + } /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * @@ -1400,20 +1764,55 @@ class MOVEIT_CORE_PUBLIC RobotState dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint); } - void markDirtyJointTransforms(const JointModelGroup* group); + 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); + void updateMimicJoint(const JointModel* joint) + { + double v = position_[joint->getFirstVariableIndex()]; + for (const JointModel* jm : joint->getMimicRequests()) + { + position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset(); + markDirtyJointTransforms(jm); + } + } /** \brief Update a set of joints that are certain to be mimicking other joints */ /* use updateMimicJoints() instead, which also marks joints dirty */ - [[deprecated]] void updateMimicJoint(const std::vector& mim); + [[deprecated]] void updateMimicJoint(const std::vector& mim) + { + for (const JointModel* jm : mim) + { + const int fvi = jm->getFirstVariableIndex(); + position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); + // Only mark joint transform dirty, but not the associated link transform + // as this function is always used in combination of + // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); + dirty_joint_transforms_[jm->getJointIndex()] = 1; + } + } /** \brief Update all mimic joints within group */ - void updateMimicJoints(const JointModelGroup* group); + void updateMimicJoints(const JointModelGroup* group) + { + for (const JointModel* jm : group->getMimicJointModels()) + { + const int fvi = jm->getFirstVariableIndex(); + position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); + markDirtyJointTransforms(jm); + } + markDirtyJointTransforms(group); + } void updateLinkTransformsInternal(const JointModel* start); diff --git a/moveit_core/macros/include/moveit/macros/visibility_control.hpp b/moveit_core/robot_state/include/moveit/robot_state/visibility_control.hpp similarity index 70% rename from moveit_core/macros/include/moveit/macros/visibility_control.hpp rename to moveit_core/robot_state/include/moveit/robot_state/visibility_control.hpp index ac0141224b..3a2bc064f4 100644 --- a/moveit_core/macros/include/moveit/macros/visibility_control.hpp +++ b/moveit_core/robot_state/include/moveit/robot_state/visibility_control.hpp @@ -33,38 +33,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOVEIT_CORE__VISIBILITY_CONTROL_HPP_ -#define MOVEIT_CORE__VISIBILITY_CONTROL_HPP_ +#ifndef ROBOT_STATE__VISIBILITY_CONTROL_HPP_ +#define ROBOT_STATE__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ - #define MOVEIT_CORE_EXPORT __attribute__ ((dllexport)) - #define MOVEIT_CORE_IMPORT __attribute__ ((dllimport)) + #define ROBOT_STATE_EXPORT __attribute__ ((dllexport)) + #define ROBOT_STATE_IMPORT __attribute__ ((dllimport)) #else - #define MOVEIT_CORE_EXPORT __declspec(dllexport) - #define MOVEIT_CORE_IMPORT __declspec(dllimport) + #define ROBOT_STATE_EXPORT __declspec(dllexport) + #define ROBOT_STATE_IMPORT __declspec(dllimport) #endif - #ifdef MOVEIT_CORE_BUILDING_LIBRARY - #define MOVEIT_CORE_PUBLIC MOVEIT_CORE_EXPORT + #ifdef ROBOT_STATE_BUILDING_LIBRARY + #define ROBOT_STATE_PUBLIC ROBOT_STATE_EXPORT #else - #define MOVEIT_CORE_PUBLIC MOVEIT_CORE_IMPORT + #define ROBOT_STATE_PUBLIC ROBOT_STATE_IMPORT #endif - #define MOVEIT_CORE_PUBLIC_TYPE MOVEIT_CORE_PUBLIC - #define MOVEIT_CORE_LOCAL + #define ROBOT_STATE_PUBLIC_TYPE ROBOT_STATE_PUBLIC + #define ROBOT_STATE_LOCAL #else - #define MOVEIT_CORE_EXPORT __attribute__ ((visibility("default"))) - #define MOVEIT_CORE_IMPORT + #define ROBOT_STATE_EXPORT __attribute__ ((visibility("default"))) + #define ROBOT_STATE_IMPORT #if __GNUC__ >= 4 - #define MOVEIT_CORE_PUBLIC __attribute__ ((visibility("default"))) - #define MOVEIT_CORE_LOCAL __attribute__ ((visibility("hidden"))) + #define ROBOT_STATE_PUBLIC __attribute__ ((visibility("default"))) + #define ROBOT_STATE_LOCAL __attribute__ ((visibility("hidden"))) #else - #define MOVEIT_CORE_PUBLIC - #define MOVEIT_CORE_LOCAL + #define ROBOT_STATE_PUBLIC + #define ROBOT_STATE_LOCAL #endif - #define MOVEIT_CORE_PUBLIC_TYPE + #define ROBOT_STATE_PUBLIC_TYPE #endif -#endif // MOVEIT_CORE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file +#endif // ROBOT_STATE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 22fee2b35d..739956066b 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -83,36 +83,6 @@ RobotState::~RobotState() delete rng_; } -const RobotModelConstPtr& RobotState::getRobotModel() const -{ - return robot_model_; -} - -std::size_t RobotState::getVariableCount() const -{ - return robot_model_->getVariableCount(); -} - -const std::vector& RobotState::getVariableNames() const -{ - return robot_model_->getVariableNames(); -} - -const LinkModel* RobotState::getLinkModel(const std::string& link) const -{ - return robot_model_->getLinkModel(link); -} - -const JointModel* RobotState::getJointModel(const std::string& joint) const -{ - return robot_model_->getJointModel(joint); -} - -const JointModelGroup* RobotState::getJointModelGroup(const std::string& group) const -{ - return robot_model_->getJointModelGroup(group); -} - void RobotState::allocMemory() { static_assert((sizeof(Eigen::Isometry3d) / EIGEN_MAX_ALIGN_BYTES) * EIGEN_MAX_ALIGN_BYTES == sizeof(Eigen::Isometry3d), @@ -389,12 +359,6 @@ void RobotState::setVariablePositions(const double* position) dirty_link_transforms_ = robot_model_->getRootJoint(); } -void RobotState::setVariablePositions(const std::vector& position) -{ - assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode - setVariablePositions(&position[0]); -} - void RobotState::setVariablePositions(const std::map& variable_map) { for (const std::pair& it : variable_map) @@ -438,61 +402,6 @@ void RobotState::setVariablePositions(const std::vector& variable_n } } -void RobotState::setVariablePosition(const std::string& variable, double value) -{ - setVariablePosition(robot_model_->getVariableIndex(variable), value); -} - -void RobotState::setVariablePosition(int index, double value) -{ - position_[index] = value; - const JointModel* jm = robot_model_->getJointOfVariable(index); - if (jm) - { - markDirtyJointTransforms(jm); - updateMimicJoint(jm); - } -} - -double RobotState::getVariablePosition(const std::string& variable) const -{ - return position_[robot_model_->getVariableIndex(variable)]; -} - -double RobotState::getVariablePosition(int index) const -{ - return position_[index]; -} - -bool RobotState::hasVelocities() const -{ - return has_velocity_; -} - -double* RobotState::getVariableVelocities() -{ - markVelocity(); - return velocity_; -} - -const double* RobotState::getVariableVelocities() const -{ - return velocity_; -} - -void RobotState::setVariableVelocities(const double* velocity) -{ - has_velocity_ = true; - // assume everything is in order in terms of array lengths (for efficiency reasons) - memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double)); -} - -void RobotState::setVariableVelocities(const std::vector& velocity) -{ - assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode - setVariableVelocities(&velocity[0]); -} - void RobotState::setVariableVelocities(const std::map& variable_map) { markVelocity(); @@ -516,32 +425,6 @@ void RobotState::setVariableVelocities(const std::vector& variable_ velocity_[robot_model_->getVariableIndex(variable_names[i])] = variable_velocity[i]; } -void RobotState::setVariableVelocity(const std::string& variable, double value) -{ - setVariableVelocity(robot_model_->getVariableIndex(variable), value); -} - -void RobotState::setVariableVelocity(int index, double value) -{ - markVelocity(); - velocity_[index] = value; -} - -void RobotState::setVariableAccelerations(const double* acceleration) -{ - has_acceleration_ = true; - has_effort_ = false; - - // assume everything is in order in terms of array lengths (for efficiency reasons) - memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double)); -} - -void RobotState::setVariableAccelerations(const std::vector& acceleration) -{ - assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode - setVariableAccelerations(&acceleration[0]); -} - void RobotState::setVariableAccelerations(const std::map& variable_map) { markAcceleration(); @@ -565,36 +448,6 @@ void RobotState::setVariableAccelerations(const std::vector& variab acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_acceleration[i]; } -bool RobotState::hasEffort() const -{ - return has_effort_; -} - -double* RobotState::getVariableEffort() -{ - markEffort(); - return effort_; -} - -const double* RobotState::getVariableEffort() const -{ - return effort_; -} - -void RobotState::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_, effort, robot_model_->getVariableCount() * sizeof(double)); -} - -void RobotState::setVariableEffort(const std::vector& effort) -{ - assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode - setVariableEffort(&effort[0]); -} - void RobotState::setVariableEffort(const std::map& variable_map) { markEffort(); @@ -618,27 +471,6 @@ void RobotState::setVariableEffort(const std::vector& variable_name effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i]; } -void RobotState::setVariableEffort(const std::string& variable, double value) -{ - setVariableEffort(robot_model_->getVariableIndex(variable), value); -} - -void RobotState::setVariableEffort(int index, double value) -{ - markEffort(); - effort_[index] = value; -} - -double RobotState::getVariableEffort(const std::string& variable) const -{ - return effort_[robot_model_->getVariableIndex(variable)]; -} - -double RobotState::getVariableEffort(int index) const -{ - return effort_[index]; -} - void RobotState::invertVelocity() { if (has_velocity_) @@ -648,46 +480,6 @@ void RobotState::invertVelocity() } } -void RobotState::setJointPositions(const std::string& joint_name, const double* position) -{ - setJointPositions(robot_model_->getJointModel(joint_name), position); -} - -void RobotState::setJointPositions(const std::string& joint_name, const std::vector& position) -{ - setJointPositions(robot_model_->getJointModel(joint_name), &position[0]); -} - -void RobotState::setJointPositions(const JointModel* joint, const std::vector& position) -{ - setJointPositions(joint, &position[0]); -} - -void RobotState::setJointPositions(const JointModel* joint, const double* position) -{ - memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double)); - markDirtyJointTransforms(joint); - updateMimicJoint(joint); -} - -void RobotState::setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform) -{ - setJointPositions(robot_model_->getJointModel(joint_name), transform); -} - -void RobotState::setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform) -{ - joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex()); - markDirtyJointTransforms(joint); - updateMimicJoint(joint); -} - -void RobotState::setJointVelocities(const JointModel* joint, const double* velocity) -{ - has_velocity_ = true; - memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double)); -} - void RobotState::setJointEfforts(const JointModel* joint, const double* effort) { if (has_acceleration_) @@ -700,65 +492,6 @@ void RobotState::setJointEfforts(const JointModel* joint, const double* effort) memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double)); } -const double* RobotState::getJointPositions(const std::string& joint_name) const -{ - return getJointPositions(robot_model_->getJointModel(joint_name)); -} - -const double* RobotState::getJointPositions(const JointModel* joint) const -{ - return position_ + joint->getFirstVariableIndex(); -} - -const double* RobotState::getJointVelocities(const std::string& joint_name) const -{ - return getJointVelocities(robot_model_->getJointModel(joint_name)); -} - -const double* RobotState::getJointVelocities(const JointModel* joint) const -{ - return velocity_ + joint->getFirstVariableIndex(); -} - -const double* RobotState::getJointAccelerations(const std::string& joint_name) const -{ - return getJointAccelerations(robot_model_->getJointModel(joint_name)); -} - -const double* RobotState::getJointAccelerations(const JointModel* joint) const -{ - return acceleration_ + joint->getFirstVariableIndex(); -} - -const double* RobotState::getJointEffort(const std::string& joint_name) const -{ - return getJointEffort(robot_model_->getJointModel(joint_name)); -} - -const double* RobotState::getJointEffort(const JointModel* joint) const -{ - return effort_ + joint->getFirstVariableIndex(); -} - -void RobotState::setJointGroupPositions(const std::string& joint_group_name, const double* gstate) -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupPositions(jmg, gstate); -} - -void RobotState::setJointGroupPositions(const std::string& joint_group_name, const std::vector& gstate) -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupPositions(jmg, &gstate[0]); -} - -void RobotState::setJointGroupPositions(const JointModelGroup* group, const std::vector& gstate) -{ - setJointGroupPositions(group, &gstate[0]); -} - void RobotState::setJointGroupPositions(const JointModelGroup* group, const double* gstate) { const std::vector& il = group->getVariableIndexList(); @@ -772,13 +505,6 @@ void RobotState::setJointGroupPositions(const JointModelGroup* group, const doub updateMimicJoints(group); } -void RobotState::setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values) -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupPositions(jmg, values); -} - void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values) { const std::vector& il = group->getVariableIndexList(); @@ -821,13 +547,6 @@ void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* g gstate[i] = position_[il[i]]; } -void RobotState::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); -} - void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const { const std::vector& il = group->getVariableIndexList(); @@ -836,25 +555,6 @@ void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::Ve values(i) = position_[il[i]]; } -void RobotState::setJointGroupVelocities(const std::string& joint_group_name, const double* gstate) -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupVelocities(jmg, gstate); -} - -void RobotState::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]); -} - -void RobotState::setJointGroupVelocities(const JointModelGroup* group, const std::vector& gstate) -{ - setJointGroupVelocities(group, &gstate[0]); -} - void RobotState::setJointGroupVelocities(const JointModelGroup* group, const double* gstate) { markVelocity(); @@ -868,13 +568,6 @@ void RobotState::setJointGroupVelocities(const JointModelGroup* group, const dou } } -void RobotState::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); -} - void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values) { markVelocity(); @@ -883,29 +576,6 @@ void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eig velocity_[il[i]] = values(i); } -void RobotState::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]); - } -} - -void RobotState::copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupVelocities(jmg, gstate); -} - -void RobotState::copyJointGroupVelocities(const JointModelGroup* group, std::vector& gstate) const -{ - gstate.resize(group->getVariableCount()); - copyJointGroupVelocities(group, &gstate[0]); -} - void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const { const std::vector& il = group->getVariableIndexList(); @@ -916,13 +586,6 @@ void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double* gstate[i] = velocity_[il[i]]; } -void RobotState::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); -} - void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const { const std::vector& il = group->getVariableIndexList(); @@ -931,25 +594,6 @@ void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::V values(i) = velocity_[il[i]]; } -void RobotState::setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate) -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - setJointGroupAccelerations(jmg, gstate); -} - -void RobotState::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]); -} - -void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const std::vector& gstate) -{ - setJointGroupAccelerations(group, &gstate[0]); -} - void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const double* gstate) { markAcceleration(); @@ -963,13 +607,6 @@ void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const } } -void RobotState::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); -} - void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values) { markAcceleration(); @@ -978,29 +615,6 @@ void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const acceleration_[il[i]] = values(i); } -void RobotState::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]); - } -} - -void RobotState::copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const -{ - const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name); - if (jmg) - copyJointGroupAccelerations(jmg, gstate); -} - -void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, std::vector& gstate) const -{ - gstate.resize(group->getVariableCount()); - copyJointGroupAccelerations(group, &gstate[0]); -} - void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const { const std::vector& il = group->getVariableIndexList(); @@ -1011,13 +625,6 @@ void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, doubl gstate[i] = acceleration_[il[i]]; } -void RobotState::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); -} - void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const { const std::vector& il = group->getVariableIndexList(); @@ -1067,56 +674,6 @@ void RobotState::updateCollisionBodyTransforms() } } -void RobotState::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 RobotState::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 RobotState::updateMimicJoint(const JointModel* joint) -{ - double v = position_[joint->getFirstVariableIndex()]; - for (const JointModel* jm : joint->getMimicRequests()) - { - position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset(); - markDirtyJointTransforms(jm); - } -} - -[[deprecated]] void RobotState::updateMimicJoint(const std::vector& mim) -{ - for (const JointModel* jm : mim) - { - const int fvi = jm->getFirstVariableIndex(); - position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); - // Only mark joint transform dirty, but not the associated link transform - // as this function is always used in combination of - // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group); - dirty_joint_transforms_[jm->getJointIndex()] = 1; - } -} - -void RobotState::updateMimicJoints(const JointModelGroup* group) -{ - for (const JointModel* jm : group->getMimicJointModels()) - { - const int fvi = jm->getFirstVariableIndex(); - position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset(); - markDirtyJointTransforms(jm); - } - markDirtyJointTransforms(group); -} - void RobotState::updateLinkTransforms() { if (dirty_link_transforms_ != nullptr) @@ -1222,78 +779,6 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]); } -const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const std::string& link_name) -{ - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); -} - -const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const LinkModel* link) -{ - updateLinkTransforms(); - return global_link_transforms_[link->getLinkIndex()]; -} - -const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const std::string& link_name) const -{ - return getGlobalLinkTransform(robot_model_->getLinkModel(link_name)); -} - -const Eigen::Isometry3d& RobotState::getGlobalLinkTransform(const LinkModel* link) const -{ - BOOST_VERIFY(checkLinkTransforms()); - return global_link_transforms_[link->getLinkIndex()]; -} - -const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const std::string& link_name, std::size_t index) -{ - return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); -} - -const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const LinkModel* link, std::size_t index) -{ - updateCollisionBodyTransforms(); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; -} - -const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const std::string& link_name, std::size_t index) const -{ - return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index); -} - -const Eigen::Isometry3d& RobotState::getCollisionBodyTransform(const LinkModel* link, std::size_t index) const -{ - BOOST_VERIFY(checkCollisionTransforms()); - return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index]; -} - -const Eigen::Isometry3d& RobotState::getJointTransform(const std::string& joint_name) -{ - return getJointTransform(robot_model_->getJointModel(joint_name)); -} - -const Eigen::Isometry3d& RobotState::getJointTransform(const JointModel* joint) -{ - const int idx = joint->getJointIndex(); - unsigned char& dirty = dirty_joint_transforms_[idx]; - if (dirty) - { - joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]); - dirty = 0; - } - return variable_joint_transforms_[idx]; -} - -const Eigen::Isometry3d& RobotState::getJointTransform(const std::string& joint_name) const -{ - return getJointTransform(robot_model_->getJointModel(joint_name)); -} - -const Eigen::Isometry3d& RobotState::getJointTransform(const JointModel* joint) const -{ - BOOST_VERIFY(checkJointTransforms(joint)); - return variable_joint_transforms_[joint->getJointIndex()]; -} - bool RobotState::satisfiesBounds(double margin) const { const std::vector& jm = robot_model_->getActiveJointModels(); @@ -1312,21 +797,6 @@ bool RobotState::satisfiesBounds(const JointModelGroup* group, double margin) co return true; } -bool RobotState::satisfiesBounds(const JointModel* joint, double margin) const -{ - return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin)); -} - -bool RobotState::satisfiesPositionBounds(const JointModel* joint, double margin) const -{ - return joint->satisfiesPositionBounds(getJointPositions(joint), margin); -} - -bool RobotState::satisfiesVelocityBounds(const JointModel* joint, double margin) const -{ - return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin); -} - void RobotState::enforceBounds() { const std::vector& jm = robot_model_->getActiveJointModels(); @@ -1341,22 +811,6 @@ void RobotState::enforceBounds(const JointModelGroup* joint_group) enforceBounds(joint); } -void RobotState::enforceBounds(const JointModel* joint) -{ - enforcePositionBounds(joint); - if (has_velocity_) - enforceVelocityBounds(joint); -} - -void RobotState::enforcePositionBounds(const JointModel* joint) -{ - if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex())) - { - markDirtyJointTransforms(joint); - updateMimicJoint(joint); - } -} - void RobotState::harmonizePositions() { for (const JointModel* jm : robot_model_->getActiveJointModels()) @@ -1369,18 +823,6 @@ void RobotState::harmonizePositions(const JointModelGroup* joint_group) harmonizePosition(jm); } -void RobotState::harmonizePosition(const JointModel* joint) -{ - if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex())) - // no need to mark transforms dirty, as the transform hasn't changed - updateMimicJoint(joint); -} - -void RobotState::enforceVelocityBounds(const JointModel* joint) -{ - joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex()); -} - std::pair RobotState::getMinDistanceToPositionBounds() const { return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels()); @@ -1448,11 +890,6 @@ bool RobotState::isValidVelocityMove(const RobotState& other, const JointModelGr return true; } -double RobotState::distance(const RobotState& other) const -{ - return robot_model_->distance(position_, other.getVariablePositions()); -} - double RobotState::distance(const RobotState& other, const JointModelGroup* joint_group) const { double d = 0.0; @@ -1465,12 +902,6 @@ double RobotState::distance(const RobotState& other, const JointModelGroup* join return d; } -double RobotState::distance(const RobotState& other, const JointModel* joint) const -{ - const int idx = joint->getFirstVariableIndex(); - return joint->distance(position_ + idx, other.position_ + idx); -} - void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const { robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions()); @@ -1532,16 +963,6 @@ void RobotState::attachBody(const std::string& id, 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, - const moveit::core::FixedTransformsMap& subframe_poses) -{ - std::set touch_links_set(touch_links.begin(), touch_links.end()); - attachBody(id, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses); -} - void RobotState::getAttachedBodies(std::vector& attached_bodies) const { attached_bodies.clear(); @@ -1629,13 +1050,6 @@ bool RobotState::clearAttachedBody(const std::string& id) return false; } -random_numbers::RandomNumberGenerator& RobotState::getRandomNumberGenerator() -{ - if (!rng_) - rng_ = new random_numbers::RandomNumberGenerator(); - return *rng_; -} - const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_id, bool* frame_found) { updateLinkTransforms(); @@ -1956,13 +1370,6 @@ bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs:: return setFromDiffIK(jmg, t, tip, dt, constraint); } -void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, - const LinkModel* tip) -{ - updateLinkTransforms(); - static_cast(this)->computeVariableVelocity(jmg, qdot, twist, tip); -} - void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist, const LinkModel* tip) const { @@ -2023,15 +1430,6 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig return true; } -void RobotState::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); -} - - bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, double timeout, const GroupStateValidityCallbackFn& constraint, const kinematics::KinematicsQueryOptions& options) diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index fd7f7bfdcb..52f23b2e42 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_transforms) add_library(${MOVEIT_LIB_NAME} SHARED src/transforms.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${MOVEIT_LIB_NAME} diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 27d0920e67..7eda9311ef 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -40,7 +40,6 @@ #include #include #include -#include #include namespace moveit @@ -57,7 +56,7 @@ using FixedTransformsMap = std::map #include #include +#include #include +#include #include #include #include @@ -82,23 +84,23 @@ class PlanningSceneMonitor : private boost::noncopyable }; /// The name of the topic used by default for receiving joint states - static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states" + PLANNING_SCENE_MONITOR_PUBLIC 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" + PLANNING_SCENE_MONITOR_PUBLIC 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" + PLANNING_SCENE_MONITOR_PUBLIC 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" + PLANNING_SCENE_MONITOR_PUBLIC 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" + PLANNING_SCENE_MONITOR_PUBLIC 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" + PLANNING_SCENE_MONITOR_PUBLIC 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) diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/visibility_control.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/visibility_control.hpp new file mode 100644 index 0000000000..fc95d46022 --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef PLANNING_SCENE_MONITOR__VISIBILITY_CONTROL_HPP_ +#define PLANNING_SCENE_MONITOR__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define PLANNING_SCENE_MONITOR_EXPORT __attribute__ ((dllexport)) + #define PLANNING_SCENE_MONITOR_IMPORT __attribute__ ((dllimport)) + #else + #define PLANNING_SCENE_MONITOR_EXPORT __declspec(dllexport) + #define PLANNING_SCENE_MONITOR_IMPORT __declspec(dllimport) + #endif + #ifdef PLANNING_SCENE_MONITOR_BUILDING_LIBRARY + #define PLANNING_SCENE_MONITOR_PUBLIC PLANNING_SCENE_MONITOR_EXPORT + #else + #define PLANNING_SCENE_MONITOR_PUBLIC PLANNING_SCENE_MONITOR_IMPORT + #endif + #define PLANNING_SCENE_MONITOR_PUBLIC_TYPE PLANNING_SCENE_MONITOR_PUBLIC + #define PLANNING_SCENE_MONITOR_LOCAL +#else + #define PLANNING_SCENE_MONITOR_EXPORT __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_MONITOR_IMPORT + #if __GNUC__ >= 4 + #define PLANNING_SCENE_MONITOR_PUBLIC __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_MONITOR_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define PLANNING_SCENE_MONITOR_PUBLIC + #define PLANNING_SCENE_MONITOR_LOCAL + #endif + #define PLANNING_SCENE_MONITOR_PUBLIC_TYPE +#endif + +#endif // PLANNING_SCENE_MONITOR__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/planning/rdf_loader/CMakeLists.txt b/moveit_ros/planning/rdf_loader/CMakeLists.txt index a78045621c..33046461c9 100644 --- a/moveit_ros/planning/rdf_loader/CMakeLists.txt +++ b/moveit_ros/planning/rdf_loader/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_rdf_loader) add_library(${MOVEIT_LIB_NAME} SHARED src/rdf_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp ament_index_cpp diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index 2af090bf7d..ab64407cb8 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -192,7 +192,11 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa cmd += xacro_arg + " "; cmd += path; +#ifdef _WIN32 + FILE* pipe = _popen(cmd.c_str(), "r"); +#else FILE* pipe = popen(cmd.c_str(), "r"); +#endif if (!pipe) { RCLCPP_ERROR(LOGGER, "Unable to load path"); @@ -205,7 +209,11 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa if (fgets(pipe_buffer, 128, pipe) != nullptr) buffer += pipe_buffer; } +#ifdef _WIN32 + _pclose(pipe); +#else pclose(pipe); +#endif return true; } diff --git a/moveit_ros/planning/robot_model_loader/CMakeLists.txt b/moveit_ros/planning/robot_model_loader/CMakeLists.txt index 7ea0234fd2..775b5a6f83 100644 --- a/moveit_ros/planning/robot_model_loader/CMakeLists.txt +++ b/moveit_ros/planning/robot_model_loader/CMakeLists.txt @@ -6,6 +6,7 @@ endif() add_library(${MOVEIT_LIB_NAME} SHARED src/robot_model_loader.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp urdf diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index baf0dca83e..6ebb4ae8cc 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_trajectory_execution_manager) add_library(${MOVEIT_LIB_NAME} SHARED src/trajectory_execution_manager.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core moveit_ros_occupancy_map_monitor 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 e35164c9be..19d93dd8b7 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 @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include From 87d34b915ce963666aade78842fc5be156da8dfc Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Tue, 21 Jul 2020 11:32:44 -0700 Subject: [PATCH 06/26] Disable warnings about stl dll link --- moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt b/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt index 3e79756021..bce1cc7057 100644 --- a/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt +++ b/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt @@ -9,6 +9,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wpedantic) endif() +if (WIN32) + add_compile_options(/wd4251 /wd4068 /wd4275) +endif() + + # find dependencies find_package(ament_cmake REQUIRED) find_package(moveit_common REQUIRED) From 246181d7fd0c6711bd9434d8f5f22c54ca438147 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 21 Jul 2020 11:01:22 -0700 Subject: [PATCH 07/26] modified .repos file to point to forked dependencies. --- moveit2.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit2.repos b/moveit2.repos index b7c72382ca..ba55e52e63 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -13,7 +13,7 @@ repositories: version: ros2 srdfdom: type: git - url: https://github.com/ros-planning/srdfdom + url: https://github.com/lilustga/srdfdom version: ros2 ros2_control: type: git From b11a6dbfb943f31abd98350ef33b64ea2e5eab4a Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 21 Jul 2020 11:33:02 -0700 Subject: [PATCH 08/26] Symbol visibility for robot state. --- .../collision_detector_allocator_allvalid.h | 3 +- moveit_core/robot_model/CMakeLists.txt | 5 + .../include/moveit/robot_model/robot_model.h | 3 +- .../moveit/robot_model/visibility_control.hpp | 98 +++++++++++++++++++ 4 files changed, 107 insertions(+), 2 deletions(-) create mode 100644 moveit_core/robot_model/include/moveit/robot_model/visibility_control.hpp 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 fdc9aec03b..a23bcb8afb 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 @@ -38,6 +38,7 @@ #include #include +#include namespace collision_detection { @@ -46,6 +47,6 @@ class CollisionDetectorAllocatorAllValid : public CollisionDetectorAllocatorTemplate { public: - static const std::string NAME; // defined in collision_env_allvalid.cpp + COLLISION_DETECTION_PUBLIC static const std::string NAME; // defined in collision_env_allvalid.cpp }; } // namespace collision_detection diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index c8770968fd..b052500fdf 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -37,6 +37,11 @@ target_link_libraries(${MOVEIT_LIB_NAME} moveit_kinematics_base ) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "ROBOT_MODEL_BUILDING_LIBRARY") + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_robot_model test/test.cpp) 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 6a1e6f2fb2..d5dce58622 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 @@ -39,6 +39,7 @@ #include #include +#include #include // joint types @@ -61,7 +62,7 @@ MOVEIT_CLASS_FORWARD(RobotModel) // Defines RobotModelPtr, ConstPtr, WeakPtr... /** \brief Definition of a kinematic model. This class is not thread safe, however multiple instances can be created */ -class RobotModel +class ROBOT_MODEL_PUBLIC RobotModel { public: /** \brief Construct a kinematic model from a parsed description and a list of planning groups */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/visibility_control.hpp b/moveit_core/robot_model/include/moveit/robot_model/visibility_control.hpp new file mode 100644 index 0000000000..ada0f68260 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/visibility_control.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef ROBOT_MODEL__VISIBILITY_CONTROL_HPP_ +#define ROBOT_MODEL__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROBOT_MODEL_EXPORT __attribute__ ((dllexport)) + #define ROBOT_MODEL_IMPORT __attribute__ ((dllimport)) + #else + #define ROBOT_MODEL_EXPORT __declspec(dllexport) + #define ROBOT_MODEL_IMPORT __declspec(dllimport) + #endif + #ifdef ROBOT_MODEL_BUILDING_LIBRARY + #define ROBOT_MODEL_PUBLIC ROBOT_MODEL_EXPORT + #else + #define ROBOT_MODEL_PUBLIC ROBOT_MODEL_IMPORT + #endif + #define ROBOT_MODEL_PUBLIC_TYPE ROBOT_MODEL_PUBLIC + #define ROBOT_MODEL_LOCAL +#else + #define ROBOT_MODEL_EXPORT __attribute__ ((visibility("default"))) + #define ROBOT_MODEL_IMPORT + #if __GNUC__ >= 4 + #define ROBOT_MODEL_PUBLIC __attribute__ ((visibility("default"))) + #define ROBOT_MODEL_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROBOT_MODEL_PUBLIC + #define ROBOT_MODEL_LOCAL + #endif + #define ROBOT_MODEL_PUBLIC_TYPE +#endif + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROBOT_MODEL_PLUGIN_EXPORT __attribute__ ((dllexport)) + #define ROBOT_MODEL_PLUGIN_IMPORT __attribute__ ((dllimport)) + #else + #define ROBOT_MODEL_PLUGIN_EXPORT __declspec(dllexport) + #define ROBOT_MODEL_PLUGIN_IMPORT __declspec(dllimport) + #endif + #ifdef ROBOT_MODEL_PLUGIN_BUILDING_LIBRARY + #define ROBOT_MODEL_PLUGIN_PUBLIC ROBOT_MODEL_PLUGIN_EXPORT + #else + #define ROBOT_MODEL_PLUGIN_PUBLIC ROBOT_MODEL_PLUGIN_IMPORT + #endif + #define ROBOT_MODEL_PLUGIN_PUBLIC_TYPE ROBOT_MODEL_PLUGIN_PUBLIC + #define ROBOT_MODEL_PLUGIN_LOCAL +#else + #define ROBOT_MODEL_PLUGIN_EXPORT __attribute__ ((visibility("default"))) + #define ROBOT_MODEL_PLUGIN_IMPORT + #if __GNUC__ >= 4 + #define ROBOT_MODEL_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) + #define ROBOT_MODEL_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROBOT_MODEL_PLUGIN_PUBLIC + #define ROBOT_MODEL_PLUGIN_LOCAL + #endif + #define ROBOT_MODEL_PLUGIN_PUBLIC_TYPE +#endif + +#endif // ROBOT_MODEL__VISIBILITY_CONTROL_HPP_ \ No newline at end of file From 73c350cf880a87f4b58d043ed3a5f0da8ef8e55f Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Tue, 21 Jul 2020 12:31:18 -0700 Subject: [PATCH 09/26] MoveIt2 on Windows fixes --- moveit_core/robot_state/CMakeLists.txt | 2 +- moveit_core/utils/CMakeLists.txt | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 794ef13e10..892fa94637 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -39,7 +39,7 @@ if(BUILD_TESTING) find_package(resource_retriever REQUIRED) if(WIN32) - # TODO add windows paths + add_compile_definitions(_USE_MATH_DEFINES /wd4251 /wd4068 /wd4275) else() set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") endif() diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 3776fbeec7..e936efd3a5 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -1,13 +1,12 @@ set(MOVEIT_LIB_NAME moveit_utils) -add_library(${MOVEIT_LIB_NAME} SHARED +add_library(${MOVEIT_LIB_NAME} src/lexical_casts.cpp src/message_checks.cpp src/rclcpp_utils.cpp ) ament_target_dependencies(${MOVEIT_LIB_NAME} Boost moveit_msgs) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) install(DIRECTORY include/ DESTINATION include) From 406f106b43dd060ab2c00a6554dc6b82f8914eeb Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 21 Jul 2020 13:16:59 -0700 Subject: [PATCH 10/26] fix. --- moveit_core/constraint_samplers/test/pr2_arm_ik.cpp | 2 +- .../constraint_samplers/test/pr2_arm_kinematics_plugin.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 77080ee9ef..d172392822 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -789,6 +789,6 @@ bool PR2ArmIK::checkJointLimits(const double& joint_value, const int& joint_num) else jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]); - return not(jv < min_angles_[joint_num] || jv > max_angles_[joint_num]); + return !(jv < min_angles_[joint_num] || jv > max_angles_[joint_num]); } } // namespace pr2_arm_kinematics 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 5b2a7b6716..5d2b716cc5 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include #include "pr2_arm_kinematics_plugin.h" @@ -240,7 +242,7 @@ double computeEuclideanDistance(const std::vector& array_1, const KDL::J { distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i)); } - return sqrt(distance); + return std::sqrt(distance); } void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::msg::KinematicSolverInfo& chain_info) From 4e5a4744b8b37c7eb7c1f7f898b1a2dc01836191 Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 21 Jul 2020 13:54:54 -0700 Subject: [PATCH 11/26] fix. From a9c07ec0d1a0e0a57b6b2bdb8a2fefda736f0ed1 Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 21 Jul 2020 14:34:14 -0700 Subject: [PATCH 12/26] fix. --- moveit_planners/ompl/CMakeLists.txt | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index eb63af3c6b..cbc65332b3 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -5,6 +5,20 @@ project(moveit_planners_ompl) find_package(moveit_common REQUIRED) moveit_package() +find_package(ASSIMP QUIET) +if(NOT ASSIMP_FOUND) + find_package(PkgConfig REQUIRED) + # assimp is required, so REQUIRE the second attempt + pkg_check_modules(ASSIMP_PC REQUIRED assimp) + set(ASSIMP_INCLUDE_DIRS ${ASSIMP_PC_INCLUDE_DIRS}) +endif() + +# find *absolute* paths to ASSIMP_LIBRARIES +# Both, pkg-config and assimp's cmake-config don't provide an absolute library path. +# For, pkg-config the path is in ASSIMP_PC_LIBRARY_DIRS, for cmake in ASSIMP_LIBRARY_DIRS. +find_library(ASSIMP_ABS_LIBRARIES NAMES ${ASSIMP_LIBRARIES} assimp HINTS ${ASSIMP_LIBRARY_DIRS} ${ASSIMP_PC_LIBRARY_DIRS}) +set(ASSIMP_LIBRARIES "${ASSIMP_ABS_LIBRARIES}") + find_package(Boost REQUIRED system filesystem date_time thread serialization) find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) From 858b38e8bc201cb6e9649dd8b5caad0832ce248e Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 21 Jul 2020 16:03:55 -0700 Subject: [PATCH 13/26] remove all ASSIMP things. --- moveit_planners/ompl/CMakeLists.txt | 14 -------------- moveit_ros/occupancy_map_monitor/CMakeLists.txt | 14 -------------- 2 files changed, 28 deletions(-) diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index cbc65332b3..eb63af3c6b 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -5,20 +5,6 @@ project(moveit_planners_ompl) find_package(moveit_common REQUIRED) moveit_package() -find_package(ASSIMP QUIET) -if(NOT ASSIMP_FOUND) - find_package(PkgConfig REQUIRED) - # assimp is required, so REQUIRE the second attempt - pkg_check_modules(ASSIMP_PC REQUIRED assimp) - set(ASSIMP_INCLUDE_DIRS ${ASSIMP_PC_INCLUDE_DIRS}) -endif() - -# find *absolute* paths to ASSIMP_LIBRARIES -# Both, pkg-config and assimp's cmake-config don't provide an absolute library path. -# For, pkg-config the path is in ASSIMP_PC_LIBRARY_DIRS, for cmake in ASSIMP_LIBRARY_DIRS. -find_library(ASSIMP_ABS_LIBRARIES NAMES ${ASSIMP_LIBRARIES} assimp HINTS ${ASSIMP_LIBRARY_DIRS} ${ASSIMP_PC_LIBRARY_DIRS}) -set(ASSIMP_LIBRARIES "${ASSIMP_ABS_LIBRARIES}") - find_package(Boost REQUIRED system filesystem date_time thread serialization) find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index 8dc5cd4501..753b8fb4e3 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -19,20 +19,6 @@ if(APPLE) find_package(X11 REQUIRED) endif() -find_package(ASSIMP QUIET) -if(NOT ASSIMP_FOUND) - find_package(PkgConfig REQUIRED) - # assimp is required, so REQUIRE the second attempt - pkg_check_modules(ASSIMP_PC REQUIRED assimp) - set(ASSIMP_INCLUDE_DIRS ${ASSIMP_PC_INCLUDE_DIRS}) -endif() - -# find *absolute* paths to ASSIMP_LIBRARIES -# Both, pkg-config and assimp's cmake-config don't provide an absolute library path. -# For, pkg-config the path is in ASSIMP_PC_LIBRARY_DIRS, for cmake in ASSIMP_LIBRARY_DIRS. -find_library(ASSIMP_ABS_LIBRARIES NAMES ${ASSIMP_LIBRARIES} assimp HINTS ${ASSIMP_LIBRARY_DIRS} ${ASSIMP_PC_LIBRARY_DIRS}) -set(ASSIMP_LIBRARIES "${ASSIMP_ABS_LIBRARIES}") - find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) find_package(pluginlib REQUIRED) From 4778da77de640dc8f95077e05d085bf2d0d99d19 Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 21 Jul 2020 16:21:41 -0700 Subject: [PATCH 14/26] Fixing type deduction errors. --- .../ompl/ompl_interface/scripts/generate_state_database.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 ea8a551d7e..e0e315ad65 100644 --- a/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp +++ b/moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp @@ -84,17 +84,17 @@ struct GenerateStateDatabaseParameters node->get_parameter_or("use_current_scene", use_current_scene, false); // number of states in joint space approximation - get_uint_parameter_or(node, "state_cnt", construction_opts.samples, 10000); + get_uint_parameter_or(node, "state_cnt", static_cast(construction_opts.samples), 10000); // generate edges together with states? - get_uint_parameter_or(node, "edges_per_sample", construction_opts.edges_per_sample, 0); + get_uint_parameter_or(node, "edges_per_sample", static_cast(construction_opts.edges_per_sample), 0); node->get_parameter_or("max_edge_length", construction_opts.max_edge_length, 0.2); // verify constraint validity on edges node->get_parameter_or("explicit_motions", construction_opts.explicit_motions, true); node->get_parameter_or("explicit_points_resolution", construction_opts.explicit_points_resolution, 0.05); - get_uint_parameter_or(node, "max_explicit_points", construction_opts.max_explicit_points, 200); + get_uint_parameter_or(node, "max_explicit_points", static_cast(construction_opts.max_explicit_points), 200); // local planning in JointModel state space node->get_parameter_or("state_space_parameterization", construction_opts.state_space_parameterization, From 188ff0a9dfb4e90aa30f95612d1ce1ecb3381cde Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Tue, 21 Jul 2020 16:32:27 -0700 Subject: [PATCH 15/26] Fix Boost --- .../common_planning_interface_objects/CMakeLists.txt | 3 +++ moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt | 3 +++ 2 files changed, 6 insertions(+) diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt index dd0fd34d16..c59da493e0 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt +++ b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt @@ -1,8 +1,11 @@ set(MOVEIT_LIB_NAME moveit_common_planning_interface_objects) +find_package(Boost REQUIRED COMPONENTS thread) + add_library(${MOVEIT_LIB_NAME} SHARED src/common_objects.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${MOVEIT_LIB_NAME} + Boost rclcpp moveit_ros_planning tf2_ros diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index c2158c0b0a..6f008f2652 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,11 +1,14 @@ set(MOVEIT_LIB_NAME moveit_cpp) +find_package(Boost REQUIRED COMPONENTS thread) + add_library(${MOVEIT_LIB_NAME} SHARED src/moveit_cpp.cpp src/planning_component.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${MOVEIT_LIB_NAME} + Boost rclcpp moveit_core moveit_ros_planning From d8d9406266cdd3e32bf1038df1e53a18917fbbbf Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 21 Jul 2020 17:16:18 -0700 Subject: [PATCH 16/26] moveit_kinematics boost fixes --- moveit_kinematics/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 499c487387..63a934ae17 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED program_options system filesystem regex) find_package(Eigen3 REQUIRED) find_package(orocos_kdl REQUIRED) find_package(tf2_kdl REQUIRED) From 3d81711a9462617f9da487e9a0f133e4c5cee04e Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Tue, 21 Jul 2020 17:14:16 -0700 Subject: [PATCH 17/26] More Windows Fixes --- .../planning_scene_monitor.h | 15 ++++++++------- .../CMakeLists.txt | 6 ++++++ .../planning_interface/moveit_cpp/CMakeLists.txt | 2 ++ 3 files changed, 16 insertions(+), 7 deletions(-) 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 3b447e99a1..e73899f686 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 @@ -53,6 +53,7 @@ #include #include #include +#include "visibility_control.hpp" namespace planning_scene_monitor { @@ -61,7 +62,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneMonitor) // Defines PlanningSceneMonitorPtr, /** * @brief PlanningSceneMonitor * Subscribes to the topic \e planning_scene */ -class PlanningSceneMonitor : private boost::noncopyable +class PLANNING_SCENE_MONITOR_PUBLIC PlanningSceneMonitor : private boost::noncopyable { public: enum SceneUpdateType @@ -84,23 +85,23 @@ class PlanningSceneMonitor : private boost::noncopyable }; /// The name of the topic used by default for receiving joint states - PLANNING_SCENE_MONITOR_PUBLIC static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states" + static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states" /// The name of the topic used by default for attached collision objects - PLANNING_SCENE_MONITOR_PUBLIC static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object" + 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 - PLANNING_SCENE_MONITOR_PUBLIC static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object" + 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) - PLANNING_SCENE_MONITOR_PUBLIC static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world" + 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 - PLANNING_SCENE_MONITOR_PUBLIC static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene" + static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene" /// The name of the service used by default for requesting full planning scene state - PLANNING_SCENE_MONITOR_PUBLIC static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene" + 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) diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt index c59da493e0..2d864c040c 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt +++ b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt @@ -1,9 +1,15 @@ set(MOVEIT_LIB_NAME moveit_common_planning_interface_objects) +if (WIN32) + add_compile_options(/wd4251 /wd4068 /wd4275) +endif() + find_package(Boost REQUIRED COMPONENTS thread) add_library(${MOVEIT_LIB_NAME} SHARED src/common_objects.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + ament_target_dependencies(${MOVEIT_LIB_NAME} Boost rclcpp diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index 6f008f2652..419a28d023 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/planning_component.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + ament_target_dependencies(${MOVEIT_LIB_NAME} Boost rclcpp From 1e2c28ff48e1c78bcd16f7ec90aa2201384db25d Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 21 Jul 2020 18:29:12 -0700 Subject: [PATCH 18/26] moveit_kinematics and run_moveit_cpp fix --- moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt | 2 +- moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt | 3 +++ moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt | 2 ++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt b/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt index bce1cc7057..c59f935966 100644 --- a/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt +++ b/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(moveit_common REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) # This shouldn't be necessary (required by moveit_simple_controller_manager) find_package(rosidl_default_runtime REQUIRED) -find_package(Boost REQUIRED COMPONENTS system) +find_package(Boost REQUIRED COMPONENTS system thread) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt index 9263b4aabc..c9957076d7 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(Boost COMPONENTS filesystem program_options REQUIRED) set(MOVEIT_LIB_NAME moveit_cached_ik_kinematics_base) add_library(${MOVEIT_LIB_NAME} SHARED src/ik_cache.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core @@ -20,6 +21,7 @@ endif() set(MOVEIT_LIB_NAME moveit_cached_ik_kinematics_plugin) add_library(${MOVEIT_LIB_NAME} SHARED src/cached_ik_kinematics_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core @@ -42,6 +44,7 @@ if(ur_kinematics_FOUND) set(MOVEIT_LIB_NAME moveit_cached_ur${UR}_kinematics_plugin) add_library(${MOVEIT_LIB_NAME} SHARED src/cached_ur_kinematics_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) find_library(ur${UR}_pluginlib ur${UR}_moveit_plugin PATHS ${ur_kinematics_LIBRARY_DIRS}) target_link_libraries(${MOVEIT_LIB_NAME} moveit_cached_ik_kinematics_base diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt index 603c7db8dc..8b49bacd21 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt @@ -4,6 +4,8 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/kdl_kinematics_plugin.cpp src/chainiksolver_vel_mimic_svd.cpp) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp random_numbers From d0d4fa5178705a1a3abeac4e68f111443e8b8342 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Tue, 21 Jul 2020 19:10:30 -0700 Subject: [PATCH 19/26] moveit_ros_visualization symbol visibility. --- .../visualization/motion_planning_rviz_plugin/CMakeLists.txt | 2 ++ .../visualization/planning_scene_rviz_plugin/CMakeLists.txt | 2 ++ moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt | 2 ++ .../visualization/rviz_plugin_render_tools/CMakeLists.txt | 1 + moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt | 2 ++ 5 files changed, 9 insertions(+) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index d93996d6af..f36bf89f3c 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -31,6 +31,7 @@ set(SOURCE_FILES set(MOVEIT_LIB_NAME moveit_motion_planning_rviz_plugin) add_library(${MOVEIT_LIB_NAME}_core SHARED ${SOURCE_FILES} ${HEADERS} ${UIC_FILES}) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin) ament_target_dependencies(${MOVEIT_LIB_NAME}_core Boost @@ -47,6 +48,7 @@ target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/i add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) ament_target_dependencies(${MOVEIT_LIB_NAME} Boost diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 5b65ecdba6..201b92c3ca 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -4,6 +4,7 @@ add_library(${MOVEIT_LIB_NAME}_core SHARED src/planning_scene_display.cpp include/moveit/planning_scene_rviz_plugin/planning_scene_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools) ament_target_dependencies(${MOVEIT_LIB_NAME}_core rclcpp @@ -19,6 +20,7 @@ target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/i add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core Qt5::Widgets) ament_target_dependencies(${MOVEIT_LIB_NAME} pluginlib diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index f94c9b65ba..4bad296b4c 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(${MOVEIT_LIB_NAME}_core SHARED src/robot_state_display.cpp include/moveit/robot_state_rviz_plugin/robot_state_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools ) @@ -19,6 +20,7 @@ target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/i add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index fc5d381e5f..2ab5d8b546 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -25,6 +25,7 @@ add_library(${MOVEIT_LIB_NAME} SHARED ${HEADERS} ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME} Qt5::Widgets diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index eff468f987..e072f20501 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -13,6 +13,7 @@ add_library(${MOVEIT_LIB_NAME}_core SHARED ${HEADERS} ) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin_core @@ -32,6 +33,7 @@ target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/i # Plugin Initializer add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp From 0e5240310e65b79c8a4a7d84d0c887d1756ccebc Mon Sep 17 00:00:00 2001 From: lilustga Date: Thu, 30 Jul 2020 10:53:09 -0700 Subject: [PATCH 20/26] Removed unnecessary deps in .repos file. Added compiler flags --- moveit2.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit2.repos b/moveit2.repos index ba55e52e63..b7c72382ca 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -13,7 +13,7 @@ repositories: version: ros2 srdfdom: type: git - url: https://github.com/lilustga/srdfdom + url: https://github.com/ros-planning/srdfdom version: ros2 ros2_control: type: git From 71888758eae448f1e3335bd6479e6f73476627e1 Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Thu, 3 Jun 2021 11:18:53 -0700 Subject: [PATCH 21/26] Update moveit2.repos --- moveit_core/ConfigExtras.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/ConfigExtras.cmake b/moveit_core/ConfigExtras.cmake index f208f771f9..28e8451f4f 100644 --- a/moveit_core/ConfigExtras.cmake +++ b/moveit_core/ConfigExtras.cmake @@ -2,7 +2,7 @@ # boost::iostreams on Windows depends on boost::zlib if(WIN32) - set(EXTRA_BOOST_COMPONENTS zlib) +# set(EXTRA_BOOST_COMPONENTS zlib) endif() find_package(Boost REQUIRED chrono From ae0e1d786fb2880356bd3e5ebe15e9226f87fb2b Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Tue, 22 Jun 2021 14:04:01 -0700 Subject: [PATCH 22/26] Mopve the needle --- moveit_core/CMakeLists.txt | 4 ++++ .../collision_detection_bullet/CMakeLists.txt | 21 +++++++++++++------ .../collision_detection_fcl/CMakeLists.txt | 13 ++++++++---- .../collision_detector_allocator_fcl.h | 2 +- .../kinematic_constraints/CMakeLists.txt | 5 +++++ 5 files changed, 34 insertions(+), 11 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 6293dc4133..672dfa31d0 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.10.2) project(moveit_core VERSION 2.0.0 LANGUAGES CXX) +set(CMAKE_VERBOSE_MAKEFILE ON) + # Common cmake code applied to all moveit packages find_package(moveit_common REQUIRED) moveit_package() @@ -13,7 +15,9 @@ find_package(Eigen3 REQUIRED) # Finds Boost Components include(ConfigExtras.cmake) +if(NOT MSVC) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") +endif() # TODO: Move collision detection into separate packages find_package(Bullet 2.87 REQUIRED) diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index ec43c5a9a3..a9373aeef9 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -59,16 +59,25 @@ if(BUILD_TESTING) ament_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp) target_link_libraries(test_bullet_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME}) - # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished - target_compile_options(test_bullet_collision_detection PRIVATE -Wno-deprecated-declarations) + + if(NOT WIN32) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_collision_detection PRIVATE -Wno-deprecated-declarations) + endif() ament_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME}) - # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished - target_compile_options(test_bullet_collision_detection_panda PRIVATE -Wno-deprecated-declarations) + + if(NOT WIN32) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_collision_detection_panda PRIVATE -Wno-deprecated-declarations) + endif() ament_add_gtest(test_bullet_continuous_collision_checking test/test_bullet_continuous_collision_checking.cpp) target_link_libraries(test_bullet_continuous_collision_checking moveit_test_utils ${MOVEIT_LIB_NAME}) - # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished - target_compile_options(test_bullet_continuous_collision_checking PRIVATE -Wno-deprecated-declarations) + + if(NOT WIN32) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_bullet_continuous_collision_checking PRIVATE -Wno-deprecated-declarations) + endif() endif() diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index a1ea30ead4..f81e7061f1 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -61,11 +61,16 @@ if(BUILD_TESTING) ament_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp) target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME}) - # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished - target_compile_options(test_fcl_collision_detection PRIVATE -Wno-deprecated-declarations) + if (NOT WIN32) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_fcl_collision_detection PRIVATE -Wno-deprecated-declarations) + endif() ament_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils ${MOVEIT_LIB_NAME}) - # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished - target_compile_options(test_fcl_collision_detection_panda PRIVATE -Wno-deprecated-declarations) + + if (NOT WIN32) + # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished + target_compile_options(test_fcl_collision_detection PRIVATE -Wno-deprecated-declarations) + endif() endif() diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h index c9f98b3b47..6c8389ef7f 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 @@ -47,6 +47,6 @@ class COLLISION_DETECTION_FCL_PUBLIC CollisionDetectorAllocatorFCL : public CollisionDetectorAllocatorTemplate { public: - static COLLISION_DETECTION_FCL_PUBLIC const std::string NAME; // defined in collision_env_fcl.cpp + static const std::string NAME; // defined in collision_env_fcl.cpp }; } // namespace collision_detection diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 9abaf6368a..b0efbe5c9c 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -34,14 +34,19 @@ target_link_libraries(${MOVEIT_LIB_NAME} install(DIRECTORY include/ DESTINATION include) +if(NOT MSVC) +# reenable once https://github.com/ros2/geometry2/pull/427 is in distribution + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_constraints test/test_constraints.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" ) + target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) ament_add_gtest(test_orientation_constraints test/test_orientation_constraints.cpp) target_link_libraries(test_orientation_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) endif() +endif() \ No newline at end of file From cfad43a86ea128027f56bf161953fdc1999a3a0a Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Wed, 23 Jun 2021 18:10:56 -0700 Subject: [PATCH 23/26] Added symbol visibility headers and replaced GLEW_LIBRARIES with GLEW::glew. --- .../collision_detection_fcl/CMakeLists.txt | 6 +- .../moveit/planning_scene/planning_scene.h | 1 + .../manipulation/pick_place/CMakeLists.txt | 5 ++ .../include/moveit/pick_place/pick_place.h | 7 +- .../moveit/pick_place/visibility_control.hpp | 70 +++++++++++++++++++ .../CMakeLists.txt | 2 + .../perception/mesh_filter/CMakeLists.txt | 9 ++- .../moveit/mesh_filter/stereo_camera_model.h | 3 +- .../moveit/mesh_filter/visibility_control.hpp | 70 +++++++++++++++++++ .../planning/planning_pipeline/CMakeLists.txt | 5 ++ .../planning_pipeline/planning_pipeline.h | 3 +- .../planning_pipeline/visibility_control.hpp | 70 +++++++++++++++++++ .../CMakeLists.txt | 6 ++ .../trajectory_execution_manager.h | 3 +- .../visibility_control.hpp | 70 +++++++++++++++++++ .../move_group_interface/CMakeLists.txt | 6 ++ .../move_group_interface.h | 4 +- .../visibility_control.hpp | 70 +++++++++++++++++++ moveit_ros/robot_interaction/CMakeLists.txt | 6 ++ .../robot_interaction/kinematic_options_map.h | 3 +- .../robot_interaction/visibility_control.hpp | 70 +++++++++++++++++++ .../CMakeLists.txt | 2 +- .../planning_scene_rviz_plugin/CMakeLists.txt | 12 ++++ .../planning_scene_display.h | 3 +- .../visibility_control.hpp | 70 +++++++++++++++++++ moveit_ros/warehouse/warehouse/CMakeLists.txt | 6 ++ .../moveit/warehouse/constraints_storage.h | 9 +-- .../moveit/warehouse/planning_scene_storage.h | 3 +- .../include/moveit/warehouse/state_storage.h | 3 +- .../moveit/warehouse/visibility_control.hpp | 70 +++++++++++++++++++ 30 files changed, 647 insertions(+), 20 deletions(-) create mode 100644 moveit_ros/manipulation/pick_place/include/moveit/pick_place/visibility_control.hpp create mode 100644 moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/visibility_control.hpp create mode 100644 moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/visibility_control.hpp create mode 100644 moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/visibility_control.hpp create mode 100644 moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/visibility_control.hpp create mode 100644 moveit_ros/robot_interaction/include/moveit/robot_interaction/visibility_control.hpp create mode 100644 moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp create mode 100644 moveit_ros/warehouse/warehouse/include/moveit/warehouse/visibility_control.hpp diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index f81e7061f1..49cebde599 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -28,11 +28,11 @@ target_compile_definitions(${MOVEIT_LIB_NAME} add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp) set_target_properties(collision_detector_fcl_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) +set_target_properties(collision_detector_fcl_plugin PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${MOVEIT_LIB_NAME} - PRIVATE "COLLISION_DETECTION_FCL_BUILDING_LIBRARY") +target_compile_definitions(collision_detector_fcl_plugin + PRIVATE "COLLISION_DETECTION_FCL_PLUGIN_BUILDING_LIBRARY") ament_target_dependencies(collision_detector_fcl_plugin 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 4b29dfcd38..f6b37d06ae 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 @@ -234,6 +234,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from */ /**@{*/ + PLANNING_SCENE_PUBLIC const std::string getCollisionDetectorName() const { // If no collision detector is allocated, return an empty string diff --git a/moveit_ros/manipulation/pick_place/CMakeLists.txt b/moveit_ros/manipulation/pick_place/CMakeLists.txt index c2e8edc6ee..f08975db69 100644 --- a/moveit_ros/manipulation/pick_place/CMakeLists.txt +++ b/moveit_ros/manipulation/pick_place/CMakeLists.txt @@ -12,6 +12,11 @@ add_library(${MOVEIT_LIB_NAME} ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_PICK_PLACE_PLANNER_BUILDING_LIBRARY") + target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h index 1e171c8f8b..a5dcd35df2 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -113,11 +114,11 @@ class PlacePlan : public PickPlacePlanBase class PickPlace : private boost::noncopyable, public std::enable_shared_from_this { public: - static const std::string DISPLAY_PATH_TOPIC; - static const std::string DISPLAY_GRASP_TOPIC; + MOVEIT_PICK_PLACE_PLANNER_PUBLIC static const std::string DISPLAY_PATH_TOPIC; + MOVEIT_PICK_PLACE_PLANNER_PUBLIC static const std::string DISPLAY_GRASP_TOPIC; // the amount of time (maximum) to wait for achieving a grasp posture - static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION; // seconds + MOVEIT_PICK_PLACE_PLANNER_PUBLIC static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION; // seconds PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/visibility_control.hpp b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/visibility_control.hpp new file mode 100644 index 0000000000..0e634a9c94 --- /dev/null +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MOVEIT_PICK_PLACE_PLANNER__VISIBILITY_CONTROL_HPP_ +#define MOVEIT_PICK_PLACE_PLANNER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MOVEIT_PICK_PLACE_PLANNER_EXPORT __attribute__ ((dllexport)) + #define MOVEIT_PICK_PLACE_PLANNER_IMPORT __attribute__ ((dllimport)) + #else + #define MOVEIT_PICK_PLACE_PLANNER_EXPORT __declspec(dllexport) + #define MOVEIT_PICK_PLACE_PLANNER_IMPORT __declspec(dllimport) + #endif + #ifdef MOVEIT_PICK_PLACE_PLANNER_BUILDING_LIBRARY + #define MOVEIT_PICK_PLACE_PLANNER_PUBLIC MOVEIT_PICK_PLACE_PLANNER_EXPORT + #else + #define MOVEIT_PICK_PLACE_PLANNER_PUBLIC MOVEIT_PICK_PLACE_PLANNER_IMPORT + #endif + #define MOVEIT_PICK_PLACE_PLANNER_PUBLIC_TYPE MOVEIT_PICK_PLACE_PLANNER_PUBLIC + #define MOVEIT_PICK_PLACE_PLANNER_LOCAL +#else + #define MOVEIT_PICK_PLACE_PLANNER_EXPORT __attribute__ ((visibility("default"))) + #define MOVEIT_PICK_PLACE_PLANNER_IMPORT + #if __GNUC__ >= 4 + #define MOVEIT_PICK_PLACE_PLANNER_PUBLIC __attribute__ ((visibility("default"))) + #define MOVEIT_PICK_PLACE_PLANNER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MOVEIT_PICK_PLACE_PLANNER_PUBLIC + #define MOVEIT_PICK_PLACE_PLANNER_LOCAL + #endif + #define MOVEIT_PICK_PLACE_PLANNER_PUBLIC_TYPE +#endif + +#endif // MOVEIT_PICK_PLACE_PLANNER__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt index 0146061207..fa3fb8b94d 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_depth_image_octomap_updater) add_library(${MOVEIT_LIB_NAME}_core SHARED src/depth_image_octomap_updater.cpp) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME}_core rclcpp moveit_core @@ -17,6 +18,7 @@ target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_lazy_free_space_updater mov add_library(${MOVEIT_LIB_NAME} SHARED src/updater_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index 0fcee736f9..b3ac3ae0a1 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -8,6 +8,13 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/gl_mesh.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MESH_FILTER_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core @@ -15,7 +22,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} Eigen3 Boost ) -target_link_libraries(${MOVEIT_LIB_NAME} ${gl_LIBS} GLUT::GLUT ${GLEW_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} ${gl_LIBS} GLUT::GLUT GLEW::glew) # TODO: enable testing # if(CATKIN_ENABLE_TESTING) 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 5ffd8174d3..89ba6a721f 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 @@ -38,6 +38,7 @@ #include #include +#include namespace mesh_filter { @@ -45,7 +46,7 @@ namespace mesh_filter * \brief Model for Disparity based devices. E.g stereo camera systems or OpenNI compatible devices * \author Suat Gedikli */ -class StereoCameraModel : public SensorModel +class MESH_FILTER_PUBLIC StereoCameraModel : public SensorModel { public: /** diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/visibility_control.hpp b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/visibility_control.hpp new file mode 100644 index 0000000000..577895943c --- /dev/null +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MESH_FILTER__VISIBILITY_CONTROL_HPP_ +#define MESH_FILTER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MESH_FILTER_EXPORT __attribute__ ((dllexport)) + #define MESH_FILTER_IMPORT __attribute__ ((dllimport)) + #else + #define MESH_FILTER_EXPORT __declspec(dllexport) + #define MESH_FILTER_IMPORT __declspec(dllimport) + #endif + #ifdef MESH_FILTER_BUILDING_LIBRARY + #define MESH_FILTER_PUBLIC MESH_FILTER_EXPORT + #else + #define MESH_FILTER_PUBLIC MESH_FILTER_IMPORT + #endif + #define MESH_FILTER_PUBLIC_TYPE MESH_FILTER_PUBLIC + #define MESH_FILTER_LOCAL +#else + #define MESH_FILTER_EXPORT __attribute__ ((visibility("default"))) + #define MESH_FILTER_IMPORT + #if __GNUC__ >= 4 + #define MESH_FILTER_PUBLIC __attribute__ ((visibility("default"))) + #define MESH_FILTER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MESH_FILTER_PUBLIC + #define MESH_FILTER_LOCAL + #endif + #define MESH_FILTER_PUBLIC_TYPE +#endif + +#endif // MESH_FILTER__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 1bb43ff99a..41ab2f398d 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -4,6 +4,11 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/planning_pipeline.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_PLANNING_PIPELINE_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core moveit_msgs 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 edf3bfa298..4533546346 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 @@ -42,6 +42,7 @@ #include #include #include +#include #include @@ -54,7 +55,7 @@ namespace planning_pipeline planning plugin and the planning_request_adapter::PlanningRequestAdapter plugins, in the specified order. */ -class PlanningPipeline +class MOVEIT_PLANNING_PIPELINE_PUBLIC PlanningPipeline { public: /** \brief When motion plans are computed and they are supposed to be automatically displayed, they are sent to this diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/visibility_control.hpp b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/visibility_control.hpp new file mode 100644 index 0000000000..e995ae2f6b --- /dev/null +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MOVEIT_PLANNING_PIPELINE__VISIBILITY_CONTROL_HPP_ +#define MOVEIT_PLANNING_PIPELINE__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MOVEIT_PLANNING_PIPELINE_EXPORT __attribute__ ((dllexport)) + #define MOVEIT_PLANNING_PIPELINE_IMPORT __attribute__ ((dllimport)) + #else + #define MOVEIT_PLANNING_PIPELINE_EXPORT __declspec(dllexport) + #define MOVEIT_PLANNING_PIPELINE_IMPORT __declspec(dllimport) + #endif + #ifdef MOVEIT_PLANNING_PIPELINE_BUILDING_LIBRARY + #define MOVEIT_PLANNING_PIPELINE_PUBLIC MOVEIT_PLANNING_PIPELINE_EXPORT + #else + #define MOVEIT_PLANNING_PIPELINE_PUBLIC MOVEIT_PLANNING_PIPELINE_IMPORT + #endif + #define MOVEIT_PLANNING_PIPELINE_PUBLIC_TYPE MOVEIT_PLANNING_PIPELINE_PUBLIC + #define MOVEIT_PLANNING_PIPELINE_LOCAL +#else + #define MOVEIT_PLANNING_PIPELINE_EXPORT __attribute__ ((visibility("default"))) + #define MOVEIT_PLANNING_PIPELINE_IMPORT + #if __GNUC__ >= 4 + #define MOVEIT_PLANNING_PIPELINE_PUBLIC __attribute__ ((visibility("default"))) + #define MOVEIT_PLANNING_PIPELINE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MOVEIT_PLANNING_PIPELINE_PUBLIC + #define MOVEIT_PLANNING_PIPELINE_LOCAL + #endif + #define MOVEIT_PLANNING_PIPELINE_PUBLIC_TYPE +#endif + +#endif // MOVEIT_PLANNING_PIPELINE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index 6ebb4ae8cc..fd2836e585 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -3,6 +3,12 @@ set(MOVEIT_LIB_NAME moveit_trajectory_execution_manager) add_library(${MOVEIT_LIB_NAME} SHARED src/trajectory_execution_manager.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_TRAJECTORY_EXECUTION_MANAGER_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core moveit_ros_occupancy_map_monitor 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 19d93dd8b7..f82ea25537 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 @@ -47,6 +47,7 @@ #include #include #include +#include #include @@ -57,7 +58,7 @@ MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager) // Defines TrajectoryExecution // Two modes: // Managed controllers // Unmanaged controllers: given the trajectory, -class TrajectoryExecutionManager +class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC TrajectoryExecutionManager { public: static const std::string EXECUTION_EVENT_TOPIC; diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/visibility_control.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/visibility_control.hpp new file mode 100644 index 0000000000..1647a35b4e --- /dev/null +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER__VISIBILITY_CONTROL_HPP_ +#define MOVEIT_TRAJECTORY_EXECUTION_MANAGER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT __attribute__ ((dllexport)) + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_IMPORT __attribute__ ((dllimport)) + #else + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT __declspec(dllexport) + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_IMPORT __declspec(dllimport) + #endif + #ifdef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_BUILDING_LIBRARY + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT + #else + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC MOVEIT_TRAJECTORY_EXECUTION_MANAGER_IMPORT + #endif + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC_TYPE MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_LOCAL +#else + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT __attribute__ ((visibility("default"))) + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_IMPORT + #if __GNUC__ >= 4 + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC __attribute__ ((visibility("default"))) + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_LOCAL + #endif + #define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_PUBLIC_TYPE +#endif + +#endif // MOVEIT_TRAJECTORY_EXECUTION_MANAGER__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index 523d74f259..1f5e327441 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -2,6 +2,12 @@ set(MOVEIT_LIB_NAME moveit_move_group_interface) add_library(${MOVEIT_LIB_NAME} SHARED src/move_group_interface.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVE_GROUP_INTERFACE_BUILDING_LIBRARY") + target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${Boost_THREAD_LIBRARY}) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core 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 2758f2099b..97e9fde291 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 @@ -56,6 +56,8 @@ #include +#include + #include #include #include @@ -101,7 +103,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface) // Defines MoveGroupInterfacePtr, Cons \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 MoveGroupInterface +class MOVE_GROUP_INTERFACE_PUBLIC MoveGroupInterface { public: /** \brief Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description' */ diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/visibility_control.hpp b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/visibility_control.hpp new file mode 100644 index 0000000000..da5875e4c9 --- /dev/null +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MOVE_GROUP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define MOVE_GROUP_INTERFACE__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MOVE_GROUP_INTERFACE_EXPORT __attribute__ ((dllexport)) + #define MOVE_GROUP_INTERFACE_IMPORT __attribute__ ((dllimport)) + #else + #define MOVE_GROUP_INTERFACE_EXPORT __declspec(dllexport) + #define MOVE_GROUP_INTERFACE_IMPORT __declspec(dllimport) + #endif + #ifdef MOVE_GROUP_INTERFACE_BUILDING_LIBRARY + #define MOVE_GROUP_INTERFACE_PUBLIC MOVE_GROUP_INTERFACE_EXPORT + #else + #define MOVE_GROUP_INTERFACE_PUBLIC MOVE_GROUP_INTERFACE_IMPORT + #endif + #define MOVE_GROUP_INTERFACE_PUBLIC_TYPE MOVE_GROUP_INTERFACE_PUBLIC + #define MOVE_GROUP_INTERFACE_LOCAL +#else + #define MOVE_GROUP_INTERFACE_EXPORT __attribute__ ((visibility("default"))) + #define MOVE_GROUP_INTERFACE_IMPORT + #if __GNUC__ >= 4 + #define MOVE_GROUP_INTERFACE_PUBLIC __attribute__ ((visibility("default"))) + #define MOVE_GROUP_INTERFACE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MOVE_GROUP_INTERFACE_PUBLIC + #define MOVE_GROUP_INTERFACE_LOCAL + #endif + #define MOVE_GROUP_INTERFACE_PUBLIC_TYPE +#endif + +#endif // MOVE_GROUP_INTERFACE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index 9491f51480..7f3a66fd00 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -36,6 +36,12 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/robot_interaction.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "ROBOT_INTERACTION_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) if(BUILD_TESTING) 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 12a9c9203d..5674d46e3d 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 @@ -37,6 +37,7 @@ #pragma once #include +#include #include #include @@ -44,7 +45,7 @@ namespace robot_interaction { // Maintains a set of KinematicOptions with a key/value mapping and a default // value. -class KinematicOptionsMap +class ROBOT_INTERACTION_PUBLIC KinematicOptionsMap { public: /// Constructor - set all options to reasonable default values. diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/visibility_control.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/visibility_control.hpp new file mode 100644 index 0000000000..8a8f5a099a --- /dev/null +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef ROBOT_INTERACTION__VISIBILITY_CONTROL_HPP_ +#define ROBOT_INTERACTION__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROBOT_INTERACTION_EXPORT __attribute__ ((dllexport)) + #define ROBOT_INTERACTION_IMPORT __attribute__ ((dllimport)) + #else + #define ROBOT_INTERACTION_EXPORT __declspec(dllexport) + #define ROBOT_INTERACTION_IMPORT __declspec(dllimport) + #endif + #ifdef ROBOT_INTERACTION_BUILDING_LIBRARY + #define ROBOT_INTERACTION_PUBLIC ROBOT_INTERACTION_EXPORT + #else + #define ROBOT_INTERACTION_PUBLIC ROBOT_INTERACTION_IMPORT + #endif + #define ROBOT_INTERACTION_PUBLIC_TYPE ROBOT_INTERACTION_PUBLIC + #define ROBOT_INTERACTION_LOCAL +#else + #define ROBOT_INTERACTION_EXPORT __attribute__ ((visibility("default"))) + #define ROBOT_INTERACTION_IMPORT + #if __GNUC__ >= 4 + #define ROBOT_INTERACTION_PUBLIC __attribute__ ((visibility("default"))) + #define ROBOT_INTERACTION_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROBOT_INTERACTION_PUBLIC + #define ROBOT_INTERACTION_LOCAL + #endif + #define ROBOT_INTERACTION_PUBLIC_TYPE +#endif + +#endif // ROBOT_INTERACTION__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index f36bf89f3c..73035b769b 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -31,7 +31,7 @@ set(SOURCE_FILES set(MOVEIT_LIB_NAME moveit_motion_planning_rviz_plugin) add_library(${MOVEIT_LIB_NAME}_core SHARED ${SOURCE_FILES} ${HEADERS} ${UIC_FILES}) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) +set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin) ament_target_dependencies(${MOVEIT_LIB_NAME}_core Boost diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 201b92c3ca..e77461a8c6 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -5,6 +5,12 @@ add_library(${MOVEIT_LIB_NAME}_core SHARED include/moveit/planning_scene_rviz_plugin/planning_scene_display.h) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME}_core + PRIVATE "PLANNING_SCENE_RVIZ_PLUGIN_BUILDING_LIBRARY") + target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools) ament_target_dependencies(${MOVEIT_LIB_NAME}_core rclcpp @@ -21,6 +27,12 @@ target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/i add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "PLANNING_SCENE_RVIZ_PLUGIN_BUILDING_LIBRARY") + target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core Qt5::Widgets) ament_target_dependencies(${MOVEIT_LIB_NAME} pluginlib 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 7447ab81bb..4ecec51f55 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 @@ -45,6 +45,7 @@ #include #include #include +#include #endif namespace Ogre @@ -66,7 +67,7 @@ class EnumProperty; namespace moveit_rviz_plugin { -class PlanningSceneDisplay : public rviz_common::Display +class PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC PlanningSceneDisplay : public rviz_common::Display { Q_OBJECT diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp new file mode 100644 index 0000000000..34a9442b7b --- /dev/null +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef PLANNING_SCENE_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define PLANNING_SCENE_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define PLANNING_SCENE_RVIZ_PLUGIN_EXPORT __attribute__ ((dllexport)) + #define PLANNING_SCENE_RVIZ_PLUGIN_IMPORT __attribute__ ((dllimport)) + #else + #define PLANNING_SCENE_RVIZ_PLUGIN_EXPORT __declspec(dllexport) + #define PLANNING_SCENE_RVIZ_PLUGIN_IMPORT __declspec(dllimport) + #endif + #ifdef PLANNING_SCENE_RVIZ_PLUGIN_BUILDING_LIBRARY + #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC PLANNING_SCENE_RVIZ_PLUGIN_EXPORT + #else + #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC PLANNING_SCENE_RVIZ_PLUGIN_IMPORT + #endif + #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC_TYPE PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC + #define PLANNING_SCENE_RVIZ_PLUGIN_LOCAL +#else + #define PLANNING_SCENE_RVIZ_PLUGIN_EXPORT __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_RVIZ_PLUGIN_IMPORT + #if __GNUC__ >= 4 + #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_RVIZ_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC + #define PLANNING_SCENE_RVIZ_PLUGIN_LOCAL + #endif + #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC_TYPE +#endif + +#endif // PLANNING_SCENE_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/moveit_ros/warehouse/warehouse/CMakeLists.txt b/moveit_ros/warehouse/warehouse/CMakeLists.txt index b407850d35..7d875d1856 100644 --- a/moveit_ros/warehouse/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/warehouse/CMakeLists.txt @@ -10,6 +10,12 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/warehouse_connector.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} + PRIVATE "MOVEIT_WAREHOUSE_BUILDING_LIBRARY") + ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp Boost moveit_core warehouse_ros moveit_ros_planning) target_link_libraries(${MOVEIT_LIB_NAME} ${OPENSSL_CRYPTO_LIBRARY}) diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h index 29e25095ea..070d1e92e4 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/constraints_storage.h @@ -37,6 +37,7 @@ #pragma once #include "moveit/warehouse/moveit_message_storage.h" +#include "moveit/warehouse/visibility_control.hpp" #include #include @@ -50,11 +51,11 @@ MOVEIT_CLASS_FORWARD(ConstraintsStorage) // Defines ConstraintsStoragePtr, Cons class ConstraintsStorage : public MoveItMessageStorage { public: - static const std::string DATABASE_NAME; + MOVEIT_WAREHOUSE_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; + MOVEIT_WAREHOUSE_PUBLIC static const std::string CONSTRAINTS_ID_NAME; + MOVEIT_WAREHOUSE_PUBLIC static const std::string CONSTRAINTS_GROUP_NAME; + MOVEIT_WAREHOUSE_PUBLIC static const std::string ROBOT_NAME; ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn); diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h index cde1593eed..7106474639 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/planning_scene_storage.h @@ -37,6 +37,7 @@ #pragma once #include "moveit/warehouse/moveit_message_storage.h" +#include "moveit/warehouse/visibility_control.hpp" #include #include #include @@ -54,7 +55,7 @@ typedef warehouse_ros::MessageCollection::Ptr MOVEIT_CLASS_FORWARD(PlanningSceneStorage) // Defines PlanningSceneStoragePtr, ConstPtr, WeakPtr... etc -class PlanningSceneStorage : public MoveItMessageStorage +class MOVEIT_WAREHOUSE_PUBLIC PlanningSceneStorage : public MoveItMessageStorage { public: static const std::string DATABASE_NAME; diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h index f97da7974d..230cadb337 100644 --- a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/state_storage.h @@ -39,6 +39,7 @@ #include #include #include +#include "moveit/warehouse/visibility_control.hpp" namespace moveit_warehouse { @@ -47,7 +48,7 @@ typedef warehouse_ros::MessageCollection::Ptr Robo MOVEIT_CLASS_FORWARD(RobotStateStorage) // Defines RobotStateStoragePtr, ConstPtr, WeakPtr... etc -class RobotStateStorage : public MoveItMessageStorage +class MOVEIT_WAREHOUSE_PUBLIC RobotStateStorage : public MoveItMessageStorage { public: static const std::string DATABASE_NAME; diff --git a/moveit_ros/warehouse/warehouse/include/moveit/warehouse/visibility_control.hpp b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/visibility_control.hpp new file mode 100644 index 0000000000..e298255b95 --- /dev/null +++ b/moveit_ros/warehouse/warehouse/include/moveit/warehouse/visibility_control.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2019, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation, 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. +// 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. + +#ifndef MOVEIT_WAREHOUSE__VISIBILITY_CONTROL_HPP_ +#define MOVEIT_WAREHOUSE__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MOVEIT_WAREHOUSE_EXPORT __attribute__ ((dllexport)) + #define MOVEIT_WAREHOUSE_IMPORT __attribute__ ((dllimport)) + #else + #define MOVEIT_WAREHOUSE_EXPORT __declspec(dllexport) + #define MOVEIT_WAREHOUSE_IMPORT __declspec(dllimport) + #endif + #ifdef MOVEIT_WAREHOUSE_BUILDING_LIBRARY + #define MOVEIT_WAREHOUSE_PUBLIC MOVEIT_WAREHOUSE_EXPORT + #else + #define MOVEIT_WAREHOUSE_PUBLIC MOVEIT_WAREHOUSE_IMPORT + #endif + #define MOVEIT_WAREHOUSE_PUBLIC_TYPE MOVEIT_WAREHOUSE_PUBLIC + #define MOVEIT_WAREHOUSE_LOCAL +#else + #define MOVEIT_WAREHOUSE_EXPORT __attribute__ ((visibility("default"))) + #define MOVEIT_WAREHOUSE_IMPORT + #if __GNUC__ >= 4 + #define MOVEIT_WAREHOUSE_PUBLIC __attribute__ ((visibility("default"))) + #define MOVEIT_WAREHOUSE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MOVEIT_WAREHOUSE_PUBLIC + #define MOVEIT_WAREHOUSE_LOCAL + #endif + #define MOVEIT_WAREHOUSE_PUBLIC_TYPE +#endif + +#endif // MOVEIT_WAREHOUSE__VISIBILITY_CONTROL_HPP_ \ No newline at end of file From 734a8b7e1c83e7ab5bbe91b823f431159f8879d4 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Wed, 23 Jun 2021 21:48:13 -0700 Subject: [PATCH 24/26] undef near, far and max from windows.h, changed type from uint to uint32_t, added boost::placeholders --- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 1 + .../moveit_servo/include/moveit_servo/servo_calcs.h | 2 +- .../src/depth_image_octomap_updater.cpp | 4 ++-- moveit_ros/perception/mesh_filter/src/gl_renderer.cpp | 4 ++++ .../src/pointcloud_octomap_updater.cpp | 8 ++++---- 5 files changed, 12 insertions(+), 7 deletions(-) diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index bbd774a25c..96d34fee3a 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -53,6 +53,7 @@ #include #else #include +#undef max #endif using namespace moveit_ros_benchmarks; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index c020749db9..98772d8fa5 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -321,7 +321,7 @@ class ServoCalcs const int gazebo_redundant_message_count_ = 30; - uint num_joints_; + uint32_t num_joints_; // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] std::array drift_dimensions_ = { { false, false, false, false, false, false } }; 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 4fb7b83ce2..b75ceda77d 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 @@ -120,7 +120,7 @@ bool DepthImageOctomapUpdater::initialize(const rclcpp::Node::SharedPtr& node) mesh_filter_->setShadowThreshold(shadow_threshold_); mesh_filter_->setPaddingOffset(padding_offset_); mesh_filter_->setPaddingScale(padding_scale_); - mesh_filter_->setTransformCallback(boost::bind(&DepthImageOctomapUpdater::getShapeTransform, this, _1, _2)); + mesh_filter_->setTransformCallback(boost::bind(&DepthImageOctomapUpdater::getShapeTransform, this, boost::placeholders::_1, boost::placeholders::_2)); // init rclcpp time default value last_update_time_ = node_->now(); @@ -141,7 +141,7 @@ void DepthImageOctomapUpdater::start() pub_filtered_label_image_ = filtered_label_transport_->advertiseCamera("filtered_label", 1); sub_depth_image_ = image_transport::create_camera_subscription( - node_.get(), image_topic_, boost::bind(&DepthImageOctomapUpdater::depthImageCallback, this, _1, _2), "raw", + node_.get(), image_topic_, boost::bind(&DepthImageOctomapUpdater::depthImageCallback, this, boost::placeholders::_1, boost::placeholders::_2), "raw", custom_qos); } diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 15d91e1366..ff8d8d60ae 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -49,6 +49,10 @@ #include #include #include +#ifdef _WIN32 +#undef near +#undef far +#endif using namespace std; 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 6edef43044..6703550216 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 @@ -43,7 +43,7 @@ #include #include #include - +#include #include namespace occupancy_map_monitor @@ -86,7 +86,7 @@ bool PointCloudOctomapUpdater::initialize(const rclcpp::Node::SharedPtr& node) tf_buffer_->setCreateTimerInterface(create_timer_interface); tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); shape_mask_.reset(new point_containment_filter::ShapeMask()); - shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2)); + shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, boost::placeholders::_1, boost::placeholders::_2)); last_update_time_ = node_->now(); return true; } @@ -104,13 +104,13 @@ void PointCloudOctomapUpdater::start() { point_cloud_filter_ = new tf2_ros::MessageFilter( *point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, node_); - point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); + point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, boost::placeholders::_1)); RCLCPP_INFO(LOGGER, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str()); } else { - point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1)); + point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, boost::placeholders::_1)); RCLCPP_INFO(LOGGER, "Listening to '%s'", point_cloud_topic_.c_str()); } } From d61357f113a95e0fc67f90fd931206fb24ba9b66 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Thu, 24 Jun 2021 20:46:01 -0700 Subject: [PATCH 25/26] Fix for planning scene rviz plugin symbol visibility. --- .../planning_scene_rviz_plugin/CMakeLists.txt | 2 +- .../planning_scene_display.h | 2 +- .../visibility_control.hpp | 28 +++++++++++++++++++ 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index e77461a8c6..b5e256e8f7 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -9,7 +9,7 @@ set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES WINDOWS_EXPORT_ALL_SYMB # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${MOVEIT_LIB_NAME}_core - PRIVATE "PLANNING_SCENE_RVIZ_PLUGIN_BUILDING_LIBRARY") + PRIVATE "PLANNING_SCENE_RVIZ_PLUGIN_CORE_BUILDING_LIBRARY") target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools) ament_target_dependencies(${MOVEIT_LIB_NAME}_core 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 4ecec51f55..c8a539e9c7 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 @@ -67,7 +67,7 @@ class EnumProperty; namespace moveit_rviz_plugin { -class PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC PlanningSceneDisplay : public rviz_common::Display +class PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC PlanningSceneDisplay : public rviz_common::Display { Q_OBJECT diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp index 34a9442b7b..9542fd2430 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/visibility_control.hpp @@ -67,4 +67,32 @@ #define PLANNING_SCENE_RVIZ_PLUGIN_PUBLIC_TYPE #endif +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT __attribute__ ((dllexport)) + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_IMPORT __attribute__ ((dllimport)) + #else + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT __declspec(dllexport) + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_IMPORT __declspec(dllimport) + #endif + #ifdef PLANNING_SCENE_RVIZ_PLUGIN_CORE_BUILDING_LIBRARY + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT + #else + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC PLANNING_SCENE_RVIZ_PLUGIN_CORE_IMPORT + #endif + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC_TYPE PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_LOCAL +#else + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_IMPORT + #if __GNUC__ >= 4 + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC __attribute__ ((visibility("default"))) + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_LOCAL + #endif + #define PLANNING_SCENE_RVIZ_PLUGIN_CORE_PUBLIC_TYPE +#endif + #endif // PLANNING_SCENE_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ \ No newline at end of file From b56eba13e5fb7ab594f774b0b51cdd8f03dad5e2 Mon Sep 17 00:00:00 2001 From: Lior Lustgarten Date: Mon, 28 Jun 2021 16:02:32 -0700 Subject: [PATCH 26/26] Added ifndef WIN32 for kill, fork, execv commands in warehouse_connector.cpp --- .../warehouse/warehouse/src/warehouse_connector.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/moveit_ros/warehouse/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/warehouse/src/warehouse_connector.cpp index 17096723b3..369c4fda36 100644 --- a/moveit_ros/warehouse/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/warehouse/src/warehouse_connector.cpp @@ -36,7 +36,9 @@ #include #include +#ifndef _WIN32 #include +#endif #include #include @@ -50,12 +52,15 @@ WarehouseConnector::WarehouseConnector(const std::string& dbexec) : dbexec_(dbex WarehouseConnector::~WarehouseConnector() { + #ifndef _WIN32 if (child_pid_ != 0) kill(child_pid_, SIGTERM); + #endif } bool WarehouseConnector::connectToDatabase(const std::string& dirname) { + #ifndef _WIN32 if (child_pid_ != 0) kill(child_pid_, SIGTERM); @@ -102,5 +107,9 @@ bool WarehouseConnector::connectToDatabase(const std::string& dirname) rclcpp::sleep_for(1s); } return true; + #else + return false; + #endif + } } // namespace moveit_warehouse