40 #include <urdf_parser/urdf_parser.h>
43 #include <gtest/gtest.h>
44 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
46 #if __has_include(<tf2/LinearMath/Vector3.hpp>)
47 #include <tf2/LinearMath/Vector3.hpp>
49 #include <tf2/LinearMath/Vector3.h>
54 #ifndef VISUALIZE_PR2_RVIZ
55 #define VISUALIZE_PR2_RVIZ 0
58 #if VISUALIZE_PR2_RVIZ
59 #include <rclcpp/rclcpp.hpp>
60 #include <visualization_msgs/msg/marker.hpp>
61 #include <geometric_shapes/shape_operations.h>
97 EXPECT_NEAR(extents_base_footprint[0], 0.001, 1e-4);
98 EXPECT_NEAR(extents_base_footprint[1], 0.001, 1e-4);
99 EXPECT_NEAR(extents_base_footprint[2], 0.001, 1e-4);
103 EXPECT_NEAR(offset_base_footprint[0], 0.0, 1e-4);
104 EXPECT_NEAR(offset_base_footprint[1], 0.0, 1e-4);
105 EXPECT_NEAR(offset_base_footprint[2], 0.071, 1e-4);
109 EXPECT_NEAR(extents_base_link[0], 0.668242, 1e-4);
110 EXPECT_NEAR(extents_base_link[1], 0.668242, 1e-4);
111 EXPECT_NEAR(extents_base_link[2], 0.656175, 1e-4);
114 EXPECT_NEAR(offset_base_link[0], 0.0, 1e-4);
115 EXPECT_NEAR(offset_base_link[1], 0.0, 1e-4);
116 EXPECT_NEAR(offset_base_link[2], 0.656175 / 2, 1e-4);
118 std::vector<double> pr2_aabb;
120 ASSERT_EQ(pr2_aabb.size(), 6u);
122 EXPECT_NEAR(pr2_aabb[0], -0.3376, 1e-4);
123 EXPECT_NEAR(pr2_aabb[1], 0.6499, 1e-4);
124 EXPECT_NEAR(pr2_aabb[2], -0.6682 / 2, 1e-4);
125 EXPECT_NEAR(pr2_aabb[3], 0.6682 / 2, 1e-4);
126 EXPECT_NEAR(pr2_aabb[4], 0.0044, 1e-4);
127 EXPECT_NEAR(pr2_aabb[5], 1.6328, 1e-4);
138 EXPECT_NEAR(aabb.center()[0], 0.5394, 1e-4);
139 EXPECT_NEAR(aabb.center()[1], 0.1880, 1e-4);
140 EXPECT_NEAR(aabb.center()[2], 1.1665, 1e-4);
141 EXPECT_NEAR(aabb.sizes()[0], 0.2209, 1e-4);
142 EXPECT_NEAR(aabb.sizes()[1], 0.1201, 1e-4);
143 EXPECT_NEAR(aabb.sizes()[2], 0.2901, 1e-4);
145 #if VISUALIZE_PR2_RVIZ
146 std::cout <<
"Overall bounding box of PR2:" <<
'\n';
147 std::string dims[] = {
"x",
"y",
"z" };
148 for (std::size_t i = 0; i < 3; ++i)
150 double dim = pr2_aabb[2 * i + 1] - pr2_aabb[2 * i];
151 double center = dim / 2;
152 std::cout << dims[i] <<
": size=" << dim <<
", offset=" << (pr2_aabb[2 * i + 1] - center) <<
'\n';
158 rclcpp::init(argc, argv);
159 auto node = rclcpp::Node::make_shared(
"visualize_pr2");
161 node->create_publisher<visualization_msgs::msg::Marker>(
"/visualization_aabb", rmw_qos_profile_default);
162 auto pub_obb = node->create_publisher<visualization_msgs::msg::Marker>(
"/visualization_obb", rmw_qos_profile_default);
163 rclcpp::Rate loop_rate(10);
169 auto msg = std::make_shared<visualization_msgs::msg::Marker>();
171 msg->header.frame_id = pr2_state.
getRobotModel()->getRootLinkName();
172 msg->type = visualization_msgs::msg::Marker::CUBE;
174 msg->lifetime.sec = 3000;
178 msg->pose.position.x = (pr2_aabb[0] + pr2_aabb[1]) / 2;
179 msg->pose.position.y = (pr2_aabb[2] + pr2_aabb[3]) / 2;
180 msg->pose.position.z = (pr2_aabb[4] + pr2_aabb[5]) / 2;
181 msg->pose.orientation.x = msg->pose.orientation.y = msg->pose.orientation.z = 0;
182 msg->pose.orientation.w = 1;
183 msg->scale.x = pr2_aabb[1] - pr2_aabb[0];
184 msg->scale.y = pr2_aabb[3] - pr2_aabb[2];
185 msg->scale.z = pr2_aabb[5] - pr2_aabb[4];
186 pub_aabb->publish(msg);
189 std::vector<const moveit::core::LinkModel*> links = pr2_state.
getRobotModel()->getLinkModelsWithCollisionGeometry();
190 for (std::size_t i = 0; i < links.size(); ++i)
194 transform.translate(links[i]->getCenteredBoundingBoxOffset());
199 msg->ns = links[i]->getName();
200 msg->pose.position.x = transform.translation()[0];
201 msg->pose.position.y = transform.translation()[1];
202 msg->pose.position.z = transform.translation()[2];
203 msg->pose.orientation.x = msg->pose.orientation.y = msg->pose.orientation.z = 0;
204 msg->pose.orientation.w = 1;
207 msg->scale.x = aabb.sizes()[0];
208 msg->scale.y = aabb.sizes()[1];
209 msg->scale.z = aabb.sizes()[2];
210 pub_aabb->publish(msg);
214 msg->pose.position.x = transform.translation()[0];
215 msg->pose.position.y = transform.translation()[1];
216 msg->pose.position.z = transform.translation()[2];
217 msg->scale.x = extents[0];
218 msg->scale.y = extents[1];
219 msg->scale.z = extents[2];
222 Eigen::Quaterniond q(transform.linear());
223 msg->pose.orientation.x = q.x();
224 msg->pose.orientation.y = q.y();
225 msg->pose.orientation.z = q.z();
226 msg->pose.orientation.w = q.w();
227 pub_obb->publish(msg);
231 std::vector<const moveit::core::AttachedBody*> attached_bodies;
233 for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_bodies.begin();
234 it != attached_bodies.end(); ++it)
236 const EigenSTL::vector_Isometry3d& transforms = (*it)->getGlobalCollisionBodyTransforms();
237 const std::vector<shapes::ShapeConstPtr>&
shapes = (*it)->getShapes();
238 for (std::size_t i = 0; i < transforms.size(); ++i)
245 msg->ns = (*it)->getName() + std::to_string(i);
246 msg->pose.position.x = transforms[i].translation()[0];
247 msg->pose.position.y = transforms[i].translation()[1];
248 msg->pose.position.z = transforms[i].translation()[2];
249 msg->pose.orientation.x = msg->pose.orientation.y = msg->pose.orientation.z = 0;
250 msg->pose.orientation.w = 1;
253 msg->scale.x = aabb.sizes()[0];
254 msg->scale.y = aabb.sizes()[1];
255 msg->scale.z = aabb.sizes()[2];
256 pub_aabb->publish(msg);
260 msg->pose.position.x = transforms[i].translation()[0];
261 msg->pose.position.y = transforms[i].translation()[1];
262 msg->pose.position.z = transforms[i].translation()[2];
263 msg->scale.x = extents[0];
264 msg->scale.y = extents[1];
265 msg->scale.z = extents[2];
268 Eigen::Quaterniond q(transforms[i].linear());
269 msg->pose.orientation.x = q.x();
270 msg->pose.orientation.y = q.y();
271 msg->pose.orientation.z = q.z();
272 msg->pose.orientation.w = q.w();
273 pub_obb->publish(msg);
283 geometry_msgs::msg::Pose origin;
284 origin.position.x = 0;
285 origin.position.y = 0;
286 origin.position.z = 0.051;
287 origin.orientation.w = 1.0;
288 builder.
addChain(
"base_footprint->base_link",
"fixed", { origin });
290 origin.position.x = 0;
291 origin.position.y = 0;
292 origin.position.z = 0;
293 builder.
addCollisionMesh(
"base_link",
"package://moveit_resources_pr2_description/urdf/meshes/base_v0/base_L.stl",
296 origin.position.x = 0;
297 origin.position.y = 0;
298 origin.position.z = 0.071;
299 builder.
addCollisionBox(
"base_footprint", { 0.001, 0.001, 0.001 }, origin);
301 builder.
addVirtualJoint(
"odom_combined",
"base_footprint",
"planar",
"world_joint");
302 builder.
addGroup({}, {
"world_joint" },
"base");
304 ASSERT_TRUE(builder.
isValid());
306 std::vector<double> simple_aabb;
309 ASSERT_EQ(simple_aabb.size(), 6u);
310 EXPECT_NEAR(simple_aabb[0], -0.6682 / 2, 1e-4);
311 EXPECT_NEAR(simple_aabb[1], 0.6682 / 2, 1e-4);
312 EXPECT_NEAR(simple_aabb[2], -0.6682 / 2, 1e-4);
313 EXPECT_NEAR(simple_aabb[3], 0.6682 / 2, 1e-4);
314 EXPECT_NEAR(simple_aabb[4], 0.0510, 1e-4);
315 EXPECT_NEAR(simple_aabb[5], 0.7071, 1e-4);
322 geometry_msgs::msg::Pose origin;
323 origin.position.x = 0;
324 origin.position.y = 0;
325 origin.position.z = 1.0;
327 q.setRPY(0, 0, 1.5708);
328 origin.orientation = tf2::toMsg(q);
329 builder.
addChain(
"base_footprint->base_link",
"fixed", { origin });
330 origin.position.x = 5.0;
331 origin.position.y = 0;
332 origin.position.z = 1.0;
334 origin.position.x = 4.0;
335 origin.position.y = 0;
336 origin.position.z = 1.0;
338 origin.position.x = -5.0;
339 origin.position.y = 0;
340 origin.position.z = -1.0;
341 q.setRPY(0, 1.5708, 0);
342 origin.orientation = tf2::toMsg(q);
344 builder.
addVirtualJoint(
"odom_combined",
"base_footprint",
"planar",
"world_joint");
345 builder.
addGroup({}, {
"world_joint" },
"base");
347 ASSERT_TRUE(builder.
isValid());
364 std::vector<double> complex_aabb;
367 ASSERT_EQ(complex_aabb.size(), 6u);
368 EXPECT_NEAR(complex_aabb[0], -5.05, 1e-4);
369 EXPECT_NEAR(complex_aabb[1], 0.5, 1e-4);
370 EXPECT_NEAR(complex_aabb[2], -0.5, 1e-4);
371 EXPECT_NEAR(complex_aabb[3], 5.05, 1e-4);
372 EXPECT_NEAR(complex_aabb[4], -1.05, 1e-4);
373 EXPECT_NEAR(complex_aabb[5], 2.05, 1e-4);
376 int main(
int argc,
char** argv)
378 testing::InitGoogleTest(&argc, argv);
379 return RUN_ALL_TESTS();
moveit::core::RobotState loadModel(const moveit::core::RobotModelPtr &model)
moveit::core::RobotState loadModel(const std::string &robot_name)
Represents an axis-aligned bounding box.
void extendWithTransformedBox(const Eigen::Isometry3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
A link from the robot. Contains the constant transform applied to the link and its geometry.
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes tha...
const Eigen::Vector3d & getCenteredBoundingBoxOffset() const
Get the offset of the center of the bounding box of this link when the link is positioned at origin.
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
void addChain(const std::string §ion, const std::string &type, const std::vector< geometry_msgs::msg::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::msg::Pose origin)
Adds a collision box to a specific link.
bool isValid()
Returns true if the building process so far has been valid.
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
void addCollisionMesh(const std::string &link_name, const std::string &filename, geometry_msgs::msg::Pose origin)
Adds a collision mesh to a specific link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx,...
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Vec3fX< details::Vec3Data< double > > Vector3d
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
int main(int argc, char **argv)
TEST_F(TestAABB, TestPR2)