diff --git a/benchmark_suite/include/moveit_benchmark_suite/scene.h b/benchmark_suite/include/moveit_benchmark_suite/scene.h index b2f050ac..7ed170be 100644 --- a/benchmark_suite/include/moveit_benchmark_suite/scene.h +++ b/benchmark_suite/include/moveit_benchmark_suite/scene.h @@ -46,10 +46,7 @@ class CollisionPluginLoader std::map plugins_; ///< Loaded plugins. }; -void getTransformsFromTf(std::vector& transforms, - const robot_model::RobotModelConstPtr& rm); - -void addTransformsToSceneMsg(const std::vector& transforms, - moveit_msgs::PlanningScene& scene_msg); +void getVirtualModelTransform(std::vector& transforms, + const robot_model::RobotModelConstPtr& robot, double timeout); } // namespace moveit_benchmark_suite diff --git a/benchmark_suite/src/benchmarks/motion_planning.cpp b/benchmark_suite/src/benchmarks/motion_planning.cpp index 1f73015e..85cbaa82 100644 --- a/benchmark_suite/src/benchmarks/motion_planning.cpp +++ b/benchmark_suite/src/benchmarks/motion_planning.cpp @@ -65,9 +65,9 @@ int main(int argc, char** argv) auto robot = std::make_shared("robot", "robot_description"); robot->initialize(); - // Get transforms from tf listener + // Get virtual transforms from robot model or tf listener std::vector transforms; - getTransformsFromTf(transforms, robot->getModelConst()); + getVirtualModelTransform(transforms, robot->getModelConst(), 1.0); // Prepare query setup QuerySetup query_setup; @@ -100,8 +100,9 @@ int main(int argc, char** argv) scene_msgs.back().is_diff = true; parser.getCollisionObjects(scene_msgs.back().world.collision_objects); - // If tf add it to the planning scene - addTransformsToSceneMsg(transforms, scene_msgs.back()); + // Add virtual transform + for (const auto& transform : transforms) + scene_msgs.back().fixed_frame_transforms.push_back(transform); query_setup.addQuery("scene", scene.first, ""); } diff --git a/benchmark_suite/src/scene.cpp b/benchmark_suite/src/scene.cpp index b1d7037f..bf085595 100644 --- a/benchmark_suite/src/scene.cpp +++ b/benchmark_suite/src/scene.cpp @@ -61,43 +61,73 @@ bool CollisionPluginLoader::activate(const std::string& name, const planning_sce return false; } -void moveit_benchmark_suite::getTransformsFromTf(std::vector& transforms, - const robot_model::RobotModelConstPtr& rm) +void moveit_benchmark_suite::getVirtualModelTransform(std::vector& transforms, + const robot_model::RobotModelConstPtr& robot, double timeout) { - const std::string& target = rm->getModelFrame(); + auto tf_buffer = std::make_shared(); + auto tf_listener = std::make_shared(*tf_buffer); + planning_scene::PlanningScene scene(robot); - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tfListener(tf_buffer_); + const std::string& root_link = robot->getRootLinkName(); + std::string virtual_frame; + geometry_msgs::TransformStamped transform; - ros::Duration(1.0).sleep(); + // SRDF has virtual joints + if (robot->getSRDF() && !robot->getSRDF()->getVirtualJoints().empty()) + { + // Must parse virtual joints for the case where the virtual joint type is `fixed` + for (const auto& virtual_joint : robot->getSRDF()->getVirtualJoints()) + { + if (virtual_joint.child_link_.compare(root_link) == 0) + { + virtual_frame = virtual_joint.parent_frame_; + break; + } + } + + // Prioritize virtual joint from srdf, then check tf listener. A fixed joint type will + // never find a transform from the scene. + if (!scene.knowsFrameTransform(virtual_frame) && + !tf_buffer->canTransform(virtual_frame, root_link, ros::Time{ 0 }, ros::Duration{ timeout })) + { + ROS_FATAL("can't transform to model frame"); + return; + } + + try + { + transform = tf_buffer->lookupTransform(virtual_frame, root_link, ros::Time(0), ros::Duration{ timeout }); + transforms.push_back(transform); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + return; + } + + // No virtual joints, check all tf listener names (that are not part of the robot) against root link + ros::Duration(timeout).sleep(); // Needed because tf names must be available std::vector all_frame_names; - tf_buffer_._getFrameStrings(all_frame_names); - for (const std::string& all_frame_name : all_frame_names) + tf_buffer->_getFrameStrings(all_frame_names); + + for (const std::string& tf_frame_name : all_frame_names) { - if (all_frame_name == target || rm->hasLinkModel(all_frame_name)) + if (robot->hasLinkModel(tf_frame_name)) continue; - geometry_msgs::TransformStamped f; try { - f = tf_buffer_.lookupTransform(target, all_frame_name, ros::Time(0)); + transform = tf_buffer->lookupTransform(tf_frame_name, root_link, ros::Time(0)); + transforms.push_back(transform); + return; } catch (tf2::TransformException& ex) { - ROS_WARN_STREAM("Unable to transform object from frame '" << all_frame_name << "' to planning frame '" << target + ROS_WARN_STREAM("Unable to transform object from frame '" << tf_frame_name << "' to planning frame '" << root_link << "' (" << ex.what() << ")"); continue; } - f.header.frame_id = all_frame_name; - f.child_frame_id = target; - transforms.push_back(f); } } - -void moveit_benchmark_suite::addTransformsToSceneMsg(const std::vector& transforms, - moveit_msgs::PlanningScene& scene_msg) -{ - for (const auto& transform : transforms) - scene_msg.fixed_frame_transforms.push_back(transform); -}