diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 7ce3162ea..ed4f8c72d 100644 --- 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 index 19c7db09c..151ddb110 100644 --- 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 index 89389ebc7..8fbd60e6b 100644 --- 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 index 979e228df..d1d51a89d 100644 --- 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 index d77204280..660b8a46a 100644 --- 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 index 78fa8ee33..fd7dabb9a 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -123,14 +123,8 @@ 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, [] { @@ -138,7 +132,6 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string 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 index 2ccecc094..bf27da69f 100644 --- 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;