Last active
September 14, 2021 04:34
-
-
Save captain-yoshi/e4ece5e9383248da98bab8c3a9c1dc74 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| /// Get robot virtual joint parent frame transform | |
| void moveit_benchmark_suite::getVirtualModelTransform(std::vector<geometry_msgs::TransformStamped>& transforms, | |
| const robot_model::RobotModelConstPtr& robot, double timeout) | |
| { | |
| auto tf_buffer = std::make_shared<tf2_ros::Buffer>(); | |
| auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer); | |
| planning_scene::PlanningScene scene(robot); | |
| const std::string& root_link = robot->getRootLinkName(); | |
| std::string virtual_frame; | |
| geometry_msgs::TransformStamped transform; | |
| // 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<std::string> all_frame_names; | |
| tf_buffer->_getFrameStrings(all_frame_names); | |
| for (const std::string& tf_frame_name : all_frame_names) | |
| { | |
| if (robot->hasLinkModel(tf_frame_name)) | |
| continue; | |
| try | |
| { | |
| 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 '" << tf_frame_name << "' to planning frame '" << root_link | |
| << "' (" << ex.what() << ")"); | |
| continue; | |
| } | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment