establish utils namespace

leaves us a place to put free helper functions
This commit is contained in:
v4hn 2021-11-10 14:36:58 +01:00
parent 48959c6806
commit f1fc447e3b
7 changed files with 90 additions and 30 deletions

View File

@ -57,18 +57,3 @@
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)
#if !MOVEIT_HAS_STATE_RIGID_PARENT_LINK
#include <moveit/robot_state/robot_state.h>
inline const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame) {
const moveit::core::LinkModel* link{ nullptr };
if (state.hasAttachedBody(frame)) {
link = state.getAttachedBody(frame)->getAttachedLink();
} else if (state.getRobotModel()->hasLinkModel(frame))
link = state.getLinkModel(frame);
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
}
#endif

View File

@ -84,7 +84,7 @@ enum InterfaceFlag
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
using InterfaceFlags = Flags<InterfaceFlag>;
using InterfaceFlags = utils::Flags<InterfaceFlag>;
/** invert interface such that
* - new end can connect to old start

View File

@ -32,15 +32,27 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: Project-agnostic utility classes
/* Authors: Robert Haschke, Michael 'v4hn' Goerner
Desc: Miscellaneous utilities
*/
#pragma once
#include <string>
#include <type_traits>
#include <initializer_list>
namespace moveit {
namespace core {
class LinkModel;
class RobotState;
} // namespace core
namespace task_constructor {
namespace utils {
/** template class to compose flags from enums in a type-safe fashion */
template <typename Enum>
class Flags
@ -118,3 +130,9 @@ private:
};
#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame);
} // namespace utils
} // namespace task_constructor
} // namespace moveit

View File

@ -28,6 +28,7 @@ add_library(${PROJECT_NAME}
stage.cpp
storage.cpp
task.cpp
utils.cpp
solvers/planner_interface.cpp
solvers/cartesian_path.cpp

View File

@ -294,11 +294,7 @@ void ComputeIK::compute() {
}
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
#else
link = getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
#endif
link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
// transform target pose such that ik frame will reach there if link does
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());

View File

@ -43,6 +43,7 @@
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/utils.h>
#include <rviz_marker_tools/marker_creation.h>
@ -262,13 +263,8 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
add_frame(target, "target frame");
add_frame(ik_pose_world, "ik frame");
const moveit::core::LinkModel* parent {
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id)
#else
getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id)
#endif
};
const moveit::core::LinkModel* parent{ utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(),
ik_pose_msg.header.frame_id) };
// transform target pose such that ik frame will reach there if link does
Eigen::Isometry3d ik_pose;

64
core/src/utils.cpp Normal file
View File

@ -0,0 +1,64 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Hamburg University
* Copyright (c) 2017, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke */
#include <moveit/robot_state/robot_state.h>
#include <moveit/task_constructor/moveit_compat.h>
namespace moveit {
namespace task_constructor {
namespace utils {
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame) {
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
return state.getRigidlyConnectedParentLinkModel(frame);
#else
const moveit::core::LinkModel* link{ nullptr };
if (state.hasAttachedBody(frame)) {
link = state.getAttachedBody(frame)->getAttachedLink();
} else if (state.getRobotModel()->hasLinkModel(frame))
link = state.getLinkModel(frame);
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
#endif
}
} // namespace utils
} // namespace task_constructor
} // namespace moveit