establish utils namespace

leaves us a place to put free helper functions

Author: v4hn

core/include/moveit/task_constructor/moveit_compat.h

@@ -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

core/include/moveit/task_constructor/stage.h

@@ -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

core/include/moveit/task_constructor/utils.h

@@ -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 @@ class Flags
 };
 
 #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

core/src/CMakeLists.txt

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

core/src/stages/compute_ik.cpp

@@ -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());

core/src/stages/move_to.cpp

@@ -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;

core/src/utils.cpp

@@ -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