54 class RvizVisualToolsDemo
74 ROS_INFO(
"Sleeping 5 seconds before running demo");
84 Eigen::Isometry3d pose_copy = pose;
85 pose_copy.translation().x() -= 0.2;
92 Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
93 Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
95 pose1.translation().x() = x_location;
97 double space_between_rows = 0.2;
104 for (
double i = 0; i <= 1.0; i += 0.02)
113 pose1.translation().x() +=
step;
119 pose1.translation().x() = 0;
120 y += space_between_rows;
121 pose1.translation().y() = y;
123 for (
double i = 0; i <= 1.0; i +=
step)
131 pose1.translation().x() +=
step;
132 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitX()) *
133 Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitY()) *
134 Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
140 pose1 = Eigen::Isometry3d::Identity();
141 y += space_between_rows;
142 pose1.translation().y() = y;
144 for (
double i = 0; i <= 1.0; i +=
step)
152 pose1.translation().x() +=
step;
153 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
159 double cuboid_max_size = 0.075;
160 double cuboid_min_size = 0.01;
161 pose1 = Eigen::Isometry3d::Identity();
162 pose2 = Eigen::Isometry3d::Identity();
163 y += space_between_rows;
164 pose1.translation().y() = y;
165 pose2.translation().y() = y;
167 for (
double i = 0; i <= 1.0; i +=
step)
170 pose2.translation().x() += i * cuboid_max_size + cuboid_min_size;
171 pose2.translation().y() += i * cuboid_max_size + cuboid_min_size;
172 pose2.translation().z() += i * cuboid_max_size + cuboid_min_size;
180 pose1.translation().x() +=
step;
186 double line_max_size = 0.075;
187 double line_min_size = 0.01;
188 pose1 = Eigen::Isometry3d::Identity();
189 pose2 = Eigen::Isometry3d::Identity();
190 y += space_between_rows;
191 pose1.translation().y() = y;
192 pose2.translation().y() = y;
194 for (
double i = 0; i <= 1.0; i +=
step)
197 pose2.translation().x() += i * line_max_size + line_min_size;
198 pose2.translation().y() += i * line_max_size + line_min_size;
199 pose2.translation().z() += i * line_max_size + line_min_size;
207 pose1.translation().x() +=
step;
213 pose1 = Eigen::Isometry3d::Identity();
214 y += space_between_rows;
215 pose1.translation().y() = y;
217 for (
double i = 0; i <= 1.0; i +=
step)
225 pose1.translation().x() +=
step;
226 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
232 pose1 = Eigen::Isometry3d::Identity();
233 y += space_between_rows;
234 pose1.translation().y() = y;
236 double angle_step = 0.1;
239 for (
double i = 0; i <= 1.0; i +=
step)
247 pose1.translation().x() +=
step;
248 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
255 pose1 = Eigen::Isometry3d::Identity();
256 y += space_between_rows;
257 pose1.translation().y() = y;
260 Eigen::Vector3d min_point, max_point;
261 min_point << -0.05, -0.05, -0.05;
262 max_point << 0.05, 0.05, 0.05;
263 for (
double i = 0; i <= 1.0; i +=
step)
271 pose1.translation().x() +=
step;
272 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
278 pose1 = Eigen::Isometry3d::Identity();
279 y += space_between_rows;
280 pose1.translation().y() = y;
282 double depth = 0.05, width = 0.05, height = 0.05;
283 for (
double i = 0; i <= 1.0; i +=
step)
291 pose1.translation().x() +=
step;
292 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
298 pose1 = Eigen::Isometry3d::Identity();
299 y += space_between_rows;
300 pose1.translation().y() = y;
302 double max_plane_size = 0.075;
303 double min_plane_size = 0.01;
304 for (
double i = 0; i <= 1.0; i +=
step)
314 pose1.translation().x() +=
step;
320 pose1 = Eigen::Isometry3d::Identity();
321 y += space_between_rows;
322 pose1.translation().y() = y;
324 graph_msgs::GeometryGraph graph;
325 for (
double i = 0; i <= 1.0; i +=
step)
327 graph.nodes.push_back(
visual_tools_->convertPose(pose1).position);
328 graph_msgs::Edges edges;
331 edges.node_ids.push_back(0);
333 graph.edges.push_back(edges);
340 pose1.translation().x() +=
step;
354 pose1.translation().x() = 0;
355 y += space_between_rows;
356 pose1.translation().y() = y;
357 pose1.translation().z() = 0;
359 for (
double i = 0; i <= 1.0; i +=
step)
367 pose1.translation().x() +=
step;
368 pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitX()) *
369 Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitY()) *
370 Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
376 pose1 = Eigen::Isometry3d::Identity();
377 pose2 = Eigen::Isometry3d::Identity();
378 y += space_between_rows;
379 pose1.translation().y() = y;
383 std::vector<rviz_visual_tools::colors>
colors;
385 for (
double i = 0; i < 1.0; i +=
step)
387 pose1.translation().y() = y;
388 if (++index % 2 == 0)
390 pose1.translation().y() +=
step / 2.0;
395 pose1.translation().y() -=
step / 2.0;
398 path.emplace_back(pose1.translation());
399 pose1.translation().x() +=
step;
411 double x_width = 0.15;
412 double y_width = 0.05;
415 double a, b, c = 0,
d;
416 y += space_between_rows;
417 double x_plane = 0, y_plane = y;
419 pose1 = Eigen::Isometry3d::Identity();
420 pose1.translation().x() = x_plane;
421 pose1.translation().y() = y_plane;
424 for (std::size_t i = 0; i < 10; ++i)
430 d = -(x_plane * x_plane + y_plane * y_plane);
445 Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
446 Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
449 std::vector<colors>
colors;
460 pose1.translation().x() = x_location - 0.1;
464 pose1.translation().x() = x_location;
473 pose1.translation().y() +=
step;
477 points1.emplace_back(pose1.translation());
478 pose1.translation().x() +=
step;
479 points1.emplace_back(pose1.translation());
481 pose1.translation().x() = x_location;
482 pose1.translation().y() +=
step;
486 points1.emplace_back(pose1.translation());
487 pose1.translation().x() +=
step;
488 points1.emplace_back(pose1.translation());
490 pose1.translation().x() = x_location;
491 pose1.translation().y() +=
step;
495 pose1.translation().y() +=
step;
499 pose1.translation().y() +=
step;
503 pose1.translation().y() +=
step;
507 pose1.translation().y() +=
step;
511 pose2.translation().x() +=
step / 2.0;
513 pose1.translation().y() +=
step;
519 pose2.translation().x() +=
step / 2.0;
520 points1.emplace_back(pose1.translation());
521 points2.emplace_back(pose2.translation());
522 pose1.translation().x() +=
step / 2.0;
525 pose2.translation().x() +=
step / 2.0;
531 pose1.translation().x() = x_location;
532 pose1.translation().y() +=
step;
541 pose1.translation().y() +=
step;
545 pose1.translation().y() +=
step;
551 pose2.translation().x() +=
step / 2.0;
552 visual_tools_->publishCylinder(pose1.translation(), pose2.translation(),
BLUE, scale);
553 pose1.translation().y() +=
step;
561 pose1.translation().y() +=
step;
576 Eigen::Isometry3d pose1 = Eigen::Isometry3d::Identity();
577 Eigen::Isometry3d pose2 = Eigen::Isometry3d::Identity();
580 pose1.translation().x() = x_location - 0.1;
583 pose1.translation().x() = x_location;
584 pose2.translation().x() = x_location;
599 scale =
static_cast<scales>(
static_cast<int>(scale) + 1);
600 pose1.translation().y() +=
visual_tools_->getScale(scale).x + 0.1;
603 pose2.translation().y() = pose1.translation().y();
604 pose2.translation().x() = x_location +
visual_tools_->getScale(scale).x * 1.3;
617 int main(
int argc,
char** argv)
619 ros::init(argc, argv,
"visual_tools_demo");
628 double x_location = 0;