Remove macros to check supported features

JafarAbdi · May 26, 2022 · d6284ea · d6284ea
1 parent 11b8185
commit d6284ea
Show file tree

Hide file tree

Showing 7 changed files with 2 additions and 46 deletions.
diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h
@@ -45,11 +45,6 @@
 	(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
 	 major * 1'000'000 + minor * 1'000 + patch)
 
-// use object shape poses relative to a single object pose
-#define MOVEIT_HAS_OBJECT_POSE 1
-
-#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 1
-
 #if !MOVEIT_VERSION_GE(3, 0, 0)
 // the pointers are not yet available
 namespace trajectory_processing {

diff --git a/core/src/stage.cpp b/core/src/stage.cpp
@@ -895,12 +895,10 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
 			return false;
 		}
 
-#if MOVEIT_HAS_OBJECT_POSE
 		if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
 			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object_name);
 			return false;  // transforms do not match
 		}
-#endif
 
 		if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
 			RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object_name);
@@ -947,15 +945,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
 			return false;  // shapes not matching
 		}
 
-#if MOVEIT_HAS_OBJECT_POSE
 		auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
 		auto from_end = from_object->getShapePosesInLinkFrame().cend();
 		auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
-#else
-		auto from_it = from_object->getFixedTransforms().cbegin();
-		auto from_end = from_object->getFixedTransforms().cend();
-		auto to_it = to_object->getFixedTransforms().cbegin();
-#endif
 		for (; from_it != from_end; ++from_it, ++to_it)
 			if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
 				RCLCPP_DEBUG_STREAM(LOGGER, name()

diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
@@ -299,7 +299,7 @@ void ComputeIK::compute() {
 		}
 		ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
 
-		link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
+		link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(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());

diff --git a/core/src/utils.cpp b/core/src/utils.cpp
@@ -54,18 +54,7 @@ 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
 }
 
 bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,

diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp
@@ -64,12 +64,7 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
 	geometry_msgs::msg::Pose p;
 	p.position.x = 0.1;
 	p.orientation.w = 1.0;
-#if MOVEIT_HAS_OBJECT_POSE
 	aco.object.pose = p;
-#else
-	aco.object.primitive_poses.resize(1, p);
-	aco.object.primitive_poses[0] = p;
-#endif
 	return aco;
 }
 

diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp
@@ -123,22 +123,15 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
 		return p;
 	}());
 	aco.object.primitive_poses.resize(1);
-#if MOVEIT_HAS_OBJECT_POSE
 	aco.object.pose.position.z = 0.2;
 	aco.object.pose.orientation.w = 1.0;
-#else
-	aco.object.primitive_poses[0].position.z = 0.2;
-	aco.object.primitive_poses[0].orientation.w = 1.0;
-#endif
-#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
 	// If we don't have this, we also don't have subframe support
 	aco.object.subframe_names.resize(1, "subframe");
 	aco.object.subframe_poses.resize(1, [] {
 		geometry_msgs::msg::Pose p;
 		p.orientation.w = 1.0;
 		return p;
 	}());
-#endif
 	return aco;
 }
 
@@ -151,7 +144,6 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
 	EXPECT_ONE_SOLUTION;
 }
 
-#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
 // If we don't have this, we also don't have subframe support
 TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
 	const std::string ATTACHED_OBJECT{ "attached_object" };
@@ -163,7 +155,6 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
 	move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
 	EXPECT_ONE_SOLUTION;
 }
-#endif
 
 int main(int argc, char** argv) {
 	testing::InitGoogleTest(&argc, argv);

diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp
@@ -84,13 +84,7 @@ int main(int argc, char** argv) {
 			co.id = "box";
 			co.header.frame_id = "panda_link0";
 			co.operation = co.ADD;
-#if MOVEIT_HAS_OBJECT_POSE
-			auto& pose{ co.pose };
-#else
-			co.primitive_poses.emplace_back();
-			auto& pose{ co.primitive_poses[0] };
-#endif
-			pose = []() {
+			co.pose = []() {
 				geometry_msgs::msg::Pose p;
 				p.position.x = 0.3;
 				p.position.y = 0.0;