Feature/adds attached body regression tests #3149
Conversation
|
The tests are currently segfaulting, with the following output from gtest: It seems to me that the My knowledge of gtest is a little rusty, but I believe inheriting from a fixture should call the parent's |
|
Also, I'm not a URDF expert, I'd appreciate any help simplifying it. After making the #3060 changes, I'm planning to mainly test attached body to attached body transforms, and the ability to look up child objects from parents, etc... so ideally the robot should just be the simplest possible example that a body can be attached to. |
|
Adding an explicit call to the parent's |
|
It seems this is just standard C++ behaviour. I wasn't sure if gtest had some magic under the hood for auto calling parent I've replaced |
|
Codecov ReportAll modified and coverable lines are covered by tests ✅
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## main #3149 +/- ##
==========================================
+ Coverage 45.77% 46.10% +0.34%
==========================================
Files 482 483 +1
Lines 40432 40632 +200
==========================================
+ Hits 18502 18731 +229
+ Misses 21930 21901 -29 ☔ View full report in Codecov by Sentry. |
| * Copyright (c) 2013, Willow Garage, Inc. | ||
| * All rights reserved. |
| urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(URDF_XML); | ||
| auto srdf_model = std::make_shared<srdf::Model>(); | ||
| srdf_model->initString(*urdf_model, SRDF_XML); | ||
| robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model); |
There was a problem hiding this comment.
optional, but you could also consider calling the testing helpers in e.g. moveit_core/utils/src/robot_model_test_utils.cpp
| const moveit::core::LinkModel* link = robot_model_->getLinkModel("link"); | ||
| std::string name = "root_body"; | ||
| Eigen::Isometry3d pose; | ||
| pose = Eigen::Translation3d(1, 2, 3); | ||
| auto box = std::make_shared<shapes::Box>(0.1, 0.2, 0.3); | ||
| std::vector<shapes::ShapeConstPtr> shapes = { box }; | ||
| EigenSTL::vector_Isometry3d shape_poses; | ||
| Eigen::Isometry3d shape_pose; | ||
| shape_pose = Eigen::Translation3d(4, 5, 6); | ||
| shape_poses.push_back(shape_pose); | ||
| std::set<std::string> touch_links = { "link" }; | ||
| trajectory_msgs::msg::JointTrajectory detach_posture; | ||
| detach_posture.joint_names.push_back("joint"); | ||
| trajectory_msgs::msg::JointTrajectoryPoint p; | ||
| p.positions.push_back(0.1); | ||
| detach_posture.points.push_back(p); | ||
| Eigen::Isometry3d subframe_pose; | ||
| subframe_pose = Eigen::Translation3d(7, 8, 9); | ||
| moveit::core::FixedTransformsMap subframes{ { "subframe", subframe_pose } }; | ||
| root_body_ = std::make_shared<moveit::core::AttachedBody>(link, name, pose, shapes, shape_poses, touch_links, | ||
| detach_posture, subframes); |
There was a problem hiding this comment.
this is a really compact block of code, consider splitting it up to make it a bit more readable
| class SingleAttachedBody : public SingleLinkRobot | ||
| { | ||
| public: | ||
| SingleAttachedBody() |
There was a problem hiding this comment.
Do you want this to be a subclass of the above test fixture, or just have it be a single test?
like, I can imagine all this code could be part of the body of TEST_F(SingleAttachedBody, RootBodyHasCorrecAttachedLink), and any repetitive code as you add more tests can be a helper function / method of the original fixture.
This would
- Save you the extra layer of inheritance
- Make the test not just be a one liner
| <inertial> | ||
| <mass value="2.81"/> | ||
| <origin rpy="0 0 0" xyz="0.0 0.0 .0"/> | ||
| <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/> |
There was a problem hiding this comment.
nit: I will forever suggest leading zeros on all decimals. so -.09 -> -0.09
|
|
||
| TEST_F(SingleAttachedBody, RootBodyHasCorrecAttachedLink) | ||
| { | ||
| ASSERT_EQ(root_body_->getName(), "root_body"); |
There was a problem hiding this comment.
Should this test try to actually attach root_body_ to the robot model, and then do some lookups on the robot model to verify that the information propagated correctly?
Looking at the code base, seems like we want to
- Get a robot state from the robot model
- Call
RobotState::attachBody - Call
RobotState::getAttachedBodiesand grab the info - Clear the bodies with
RobotState::clearAttachedBody - Verify again the object is gone, etc.
|
Thanks @sea-bass for the comments. Will take a better look at them after work today. This PR is still very much in the early stages, just wanted to get a simple test passing and go from there. Raised as a regular PR to demonstrate the segfault I was getting in the workflow, but since that's fixed now (and I can run the tests pretty easy locally) I'm happy to move it to a draft. |
Description
I'm planning to implement the proposal made in #3060 (hierarchical collision objects). I figured it'd first be useful to add some tests for the existing functionality, check for regressions whilst making the changes, and as a basis to extend with tests checking the correctness of collision object -> collision object transformation calculations.
Checklist