55 const std::string
LOGNAME =
"visual_tools";
65 : nh_(nh), marker_topic_(
std::move(marker_topic)), base_frame_(
std::move(base_frame))
218 mesh_marker_.type = visualization_msgs::Marker::MESH_RESOURCE;
253 text_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
282 if (wait_for_subscriber)
290 bool blocking =
true;
296 bool blocking =
false;
314 ROS_ERROR_STREAM_NAMED(LOGNAME,
"loadMarkerPub() has not been called yet, unable to wait for subscriber.");
317 if (blocking && num_existing_subscribers == 0)
323 while (num_existing_subscribers == 0)
328 << wait_time <<
" sec. It is possible initially published visual messages " 373 std_msgs::ColorRGBA result;
490 "be used with getColor(). Defaulting to " 509 case 0:
return BLACK;
break;
510 case 1:
return BROWN;
break;
511 case 2:
return BLUE;
break;
512 case 3:
return CYAN;
break;
513 case 4:
return GREY;
break;
515 case 6:
return GREEN;
break;
518 case 9:
return ORANGE;
break;
519 case 10:
return PURPLE;
break;
520 case 11:
return RED;
break;
521 case 12:
return PINK;
break;
522 case 13:
return WHITE;
break;
523 case 14:
return YELLOW;
break;
527 case 18:
return RAND;
break;
528 case 19:
return CLEAR;
break;
529 case 20:
return DEFAULT;
break;
543 case 4:
return XSMALL;
break;
544 case 5:
return SMALL;
break;
545 case 6:
return MEDIUM;
break;
546 case 7:
return LARGE;
break;
547 case 8:
return XLARGE;
break;
551 default:
throw std::runtime_error(
"Unknown size");
562 case XXXXSMALL:
return "XXXXSMALL";
break;
563 case XXXSMALL:
return "XXXSMALL";
break;
564 case XXSMALL:
return "XXSMALL";
break;
565 case XSMALL:
return "XSMALL";
break;
566 case SMALL:
return "SMALL";
break;
567 case MEDIUM:
return "MEDIUM";
break;
568 case LARGE:
return "LARGE";
break;
569 case XLARGE:
return "XLARGE";
break;
570 case XXLARGE:
return "XXLARGE";
break;
571 case XXXLARGE:
return "XXXLARGE";
break;
572 case XXXXLARGE:
return "XXXXLARGE";
break;
573 default:
throw std::runtime_error(
"Unknown size");
581 std_msgs::ColorRGBA result;
583 const std::size_t MAX_ATTEMPTS = 20;
584 std::size_t attempts = 0;
589 result.r =
fRand(0.0, 1.0);
590 result.g =
fRand(0.0, 1.0);
591 result.b =
fRand(0.0, 1.0);
595 if (attempts > MAX_ATTEMPTS)
597 ROS_WARN_STREAM_NAMED(LOGNAME,
"Unable to find appropriate random color after " << MAX_ATTEMPTS <<
" attempts");
600 }
while (result.r + result.g + result.b < 1.5);
610 return start + (((end - start) / range) * value);
618 ROS_WARN_STREAM_NAMED(LOGNAME,
"Intensity value for color scale is below range [0,1], value: " << value);
623 ROS_WARN_STREAM_NAMED(LOGNAME,
"Intensity value for color scale is above range [0,1], value: " << value);
627 std_msgs::ColorRGBA
start;
628 std_msgs::ColorRGBA end;
635 else if (value == 1.0)
639 else if (value <= 0.5)
648 value = fmod(value, 0.5);
651 std_msgs::ColorRGBA result;
652 result.r =
slerp(start.r, end.r, 0.5, value);
653 result.g =
slerp(start.g, end.g, 0.5, value);
654 result.b =
slerp(start.b, end.b, 0.5, value);
662 geometry_msgs::Vector3 result;
714 Eigen::Vector3d center;
715 center[0] = (a[0] + b[0]) / 2.0;
716 center[1] = (a[1] + b[1]) / 2.0;
717 center[2] = (a[2] + b[2]) / 2.0;
723 bool verbose =
false;
732 Eigen::Isometry3d result_pose = Eigen::Isometry3d::Identity();
735 Eigen::Quaterniond q;
736 Eigen::Vector3d axis_vector = b - a;
740 std::cout << std::endl;
741 std::cout <<
"axis_vector " << std::endl;
745 axis_vector.normalize();
749 std::cout << std::endl;
750 std::cout <<
"axis_vector after normalize " << std::endl;
754 Eigen::Vector3d up_vector(0.0, 0.0, 1.0);
755 Eigen::Vector3d right_axis_vector = axis_vector.cross(up_vector);
759 std::cout <<
"right_axis_vector " << std::endl;
763 if (right_axis_vector[0] == 0.0 && right_axis_vector[1] == 0.0 && right_axis_vector[2] == 0.0)
767 std::cout <<
"right axis vector is zero " << std::endl;
769 result_pose = Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY());
770 result_pose.translation() = a;
774 right_axis_vector.normalized();
778 std::cout <<
"right_axis_vector normalized " << std::endl;
782 double theta = axis_vector.dot(up_vector);
783 double angle_rotation = -1.0 *
acos(theta);
789 tf::Vector3 tf_right_axis_vector;
803 std::cout <<
"rotation matrix: " << std::endl;
804 std::cout << q.toRotationMatrix() << std::endl;
817 std::cout <<
"rotation matrix after normalize: " << std::endl;
818 std::cout << q.toRotationMatrix() << std::endl;
822 result_pose = q * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY());
823 result_pose.translation() = a;
855 if (
markers_.markers.size() >= queueSize || queueSize == 0)
866 ROS_WARN_STREAM_NAMED(LOGNAME,
"Batch publishing triggered but it was not enabled (unnecessary function call)");
898 if (markers.markers.empty())
906 for (
auto& marker : markers.markers)
908 marker.color.r = 1.0 - marker.color.r;
909 marker.color.g = 1.0 - marker.color.g;
910 marker.color.b = 1.0 - marker.color.b;
911 for (
auto& color : marker.colors)
913 color.r = 1.0 - color.r;
914 color.g = 1.0 - color.g;
915 color.b = 1.0 - color.b;
938 geometry_msgs::Point p[3];
939 static const double delta_theta = M_PI / 16.0;
943 for (std::size_t i = 0; i < 32; i++)
950 p[1].y = scale *
cos(theta) / angle;
951 p[1].z = scale *
sin(theta) / angle;
954 p[2].y = scale *
cos(theta + delta_theta) / angle;
955 p[2].z = scale *
sin(theta + delta_theta) / angle;
961 theta += delta_theta;
984 geometry_msgs::Point p[4];
985 p[0].x = 1.0 * scale;
986 p[0].y = 1.0 * scale;
989 p[1].x = -1.0 * scale;
990 p[1].y = 1.0 * scale;
993 p[2].x = -1.0 * scale;
994 p[2].y = -1.0 * scale;
997 p[3].x = 1.0 * scale;
998 p[3].y = -1.0 * scale;
1030 geometry_msgs::Point p[4];
1031 p[0].x = 1.0 * scale;
1033 p[0].z = 1.0 * scale;
1035 p[1].x = -1.0 * scale;
1037 p[1].z = 1.0 * scale;
1039 p[2].x = -1.0 * scale;
1041 p[2].z = -1.0 * scale;
1043 p[3].x = 1.0 * scale;
1045 p[3].z = -1.0 * scale;
1076 geometry_msgs::Point p[4];
1078 p[0].y = 1.0 * scale;
1079 p[0].z = 1.0 * scale;
1082 p[1].y = -1.0 * scale;
1083 p[1].z = 1.0 * scale;
1086 p[2].y = -1.0 * scale;
1087 p[2].z = -1.0 * scale;
1090 p[3].y = 1.0 * scale;
1091 p[3].z = -1.0 * scale;
1118 geometry_msgs::Pose pose_msg;
1126 geometry_msgs::Pose pose_msg;
1132 const std::string& ns, std::size_t
id)
1134 geometry_msgs::Pose pose_msg;
1135 pose_msg.position = point;
1148 geometry_msgs::Vector3 scale_msg;
1156 const std::string& ns, std::size_t
id)
1162 const geometry_msgs::Vector3 scale,
const std::string& ns, std::size_t
id)
1168 const geometry_msgs::Vector3 scale,
const std::string& ns, std::size_t
id)
1170 geometry_msgs::Pose pose_msg;
1176 const geometry_msgs::Vector3 scale,
const std::string& ns, std::size_t
id)
1201 const geometry_msgs::Vector3 scale,
const std::string& ns, std::size_t
id)
1244 Eigen::Isometry3d arrow_pose = pose * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
1250 Eigen::Isometry3d arrow_pose =
convertPose(pose) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
1256 Eigen::Isometry3d arrow_pose =
convertPose(pose.pose) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
1257 geometry_msgs::PoseStamped new_pose = pose;
1265 Eigen::Isometry3d arrow_pose = pose * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1271 Eigen::Isometry3d arrow_pose =
convertPose(pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1277 Eigen::Isometry3d arrow_pose =
convertPose(pose.pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1278 geometry_msgs::PoseStamped new_pose = pose;
1286 Eigen::Isometry3d arrow_pose =
convertPose(pose.pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
1287 geometry_msgs::PoseStamped new_pose = pose;
1289 return publishArrow(new_pose, color, scale, length,
id);
1369 scales scale, std::size_t
id)
1413 geometry_msgs::Pose pose_shifted = pose;
1414 pose_shifted.position.x -= 0.05;
1415 pose_shifted.position.y -= 0.05;
1416 pose_shifted.position.z -= 0.05;
1417 publishText(pose_shifted, label, color, static_cast<scales>(static_cast<int>(scale) + 1),
false);
1424 return publishAxis(pose, radius * 10.0, radius, ns);
1430 return publishAxis(pose, radius * 10.0, radius, ns);
1448 const std::string& ns)
1451 Eigen::Isometry3d x_pose =
1452 Eigen::Translation3d(length / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY());
1453 x_pose = pose * x_pose;
1457 Eigen::Isometry3d y_pose =
1458 Eigen::Translation3d(0, length / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX());
1459 y_pose = pose * y_pose;
1463 Eigen::Isometry3d z_pose = Eigen::Translation3d(0, 0, length / 2.0) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
1464 z_pose = pose * z_pose;
1473 for (
const auto& i : path)
1483 const std::string& ns)
1486 for (
const auto& i : path)
1495 scales scale,
const std::string& ns)
1502 double radius,
const std::string& ns)
1508 const std_msgs::ColorRGBA& color,
double radius,
const std::string& ns)
1511 double height = (point1 - point2).lpNorm<2>();
1517 Eigen::Isometry3d pose;
1521 Eigen::Isometry3d rotation;
1522 rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
1523 pose = pose * rotation;
1530 const std::string& ns)
1536 const std::string& ns)
1542 double radius,
const std::string& ns)
1565 const std::string& ns, std::size_t
id)
1571 double scale,
const std::string& ns, std::size_t
id)
1612 typedef std::pair<std::size_t, std::size_t> node_ids;
1613 std::set<node_ids> added_edges;
1614 std::pair<std::set<node_ids>::iterator,
bool> return_value;
1615 Eigen::Vector3d a, b;
1616 for (std::size_t i = 0; i < graph.nodes.size(); ++i)
1618 for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j)
1621 return_value = added_edges.insert(node_ids(i, j));
1622 if (!return_value.second)
1630 b =
convertPoint(graph.nodes[graph.edges[i].node_ids[j]]);
1646 colors color,
const std::string& ns, std::size_t
id)
1664 geometry_msgs::Pose pose;
1665 pose.position.x = (point1.x - point2.x) / 2.0 + point2.x;
1666 pose.position.y = (point1.y - point2.y) / 2.0 + point2.y;
1667 pose.position.z = (point1.z - point2.z) / 2.0 + point2.z;
1754 geometry_msgs::Vector3 scale;
1762 const std_msgs::ColorRGBA& color,
scales scale)
1768 const std_msgs::ColorRGBA& color,
double radius)
1770 geometry_msgs::Vector3 scale;
1784 const std_msgs::ColorRGBA& color,
scales scale)
1790 const std_msgs::ColorRGBA& color,
const geometry_msgs::Vector3& scale)
1810 BOOST_ASSERT_MSG(aPoints.size() == bPoints.size() && bPoints.size() == colors.size(),
"Mismatching vector sizes: " 1811 "aPoints, bPoints, and colors");
1813 std::vector<geometry_msgs::Point> aPoints_msg;
1814 std::vector<geometry_msgs::Point> bPoints_msg;
1815 std::vector<std_msgs::ColorRGBA> colors_msg;
1817 for (std::size_t i = 0; i < aPoints.size(); ++i)
1823 colors_msg.push_back(
getColor(colors[i]));
1825 BOOST_ASSERT_MSG(aPoints_msg.size() == bPoints_msg.size() && bPoints_msg.size() == colors_msg.size(),
"Mismatched " 1832 const std::vector<geometry_msgs::Point>& bPoints,
1833 const std::vector<std_msgs::ColorRGBA>&
colors,
const geometry_msgs::Vector3& scale)
1848 for (std::size_t i = 0; i < aPoints.size(); ++i)
1858 BOOST_ASSERT_MSG(
line_list_marker_.colors.size() == aPoints.size() * 2,
"Colors arrays mismatch in size");
1865 const std::string& ns)
1867 if (path.size() < 2)
1879 std_msgs::ColorRGBA this_color =
getColor(color);
1885 for (std::size_t i = 1; i < path.size(); ++i)
1899 const std::string& ns)
1901 std::vector<geometry_msgs::Point> point_path(path.size());
1902 for (std::size_t i = 0; i < path.size(); ++i)
1904 point_path[i] = path[i].position;
1911 const std::string& ns)
1917 const std::string& ns)
1923 const std::string& ns)
1929 const std::string& ns)
1931 if (path.size() < 2)
1938 for (std::size_t i = 1; i < path.size(); ++i)
1947 const std::string& ns)
1949 if (path.size() < 2)
1956 for (std::size_t i = 1; i < path.size(); ++i)
1965 const std::string& ns)
1967 if (path.size() < 2)
1974 for (std::size_t i = 1; i < path.size(); ++i)
1976 publishCylinder(path[i - 1].translation(), path[i].translation(), color, radius, ns);
1983 double radius,
const std::string& ns)
1985 if (path.size() < 2)
1991 if (path.size() != colors.size())
1993 ROS_ERROR_STREAM_NAMED(LOGNAME,
"Skipping path because " << path.size() <<
" different from " << colors.size()
1999 for (std::size_t i = 1; i < path.size(); ++i)
2008 double radius,
const std::string& ns)
2010 if (path.size() < 2)
2016 if (path.size() != colors.size())
2018 ROS_ERROR_STREAM_NAMED(LOGNAME,
"Skipping path because " << path.size() <<
" different from " << colors.size()
2024 for (std::size_t i = 1; i < path.size(); ++i)
2033 const std::string& ns)
2035 std::vector<geometry_msgs::Point> points;
2036 geometry_msgs::Point temp;
2037 geometry_msgs::Point first;
2040 for (std::size_t i = 0; i < polygon.points.size(); ++i)
2042 temp.x = polygon.points[i].x;
2043 temp.y = polygon.points[i].y;
2044 temp.z = polygon.points[i].z;
2049 points.push_back(temp);
2051 points.push_back(first);
2057 colors color,
const std::string& ns, std::size_t
id)
2059 Eigen::Vector3d min_point, max_point;
2060 min_point << -depth / 2, -width / 2, -height / 2;
2061 max_point << depth / 2, width / 2, height / 2;
2066 const Eigen::Vector3d& max_point,
colors color,
const std::string& ns,
2070 Eigen::Vector3d p1(min_point[0], min_point[1], min_point[2]);
2071 Eigen::Vector3d p2(min_point[0], min_point[1], max_point[2]);
2072 Eigen::Vector3d p3(max_point[0], min_point[1], max_point[2]);
2073 Eigen::Vector3d p4(max_point[0], min_point[1], min_point[2]);
2074 Eigen::Vector3d p5(min_point[0], max_point[1], min_point[2]);
2075 Eigen::Vector3d p6(min_point[0], max_point[1], max_point[2]);
2076 Eigen::Vector3d p7(max_point[0], max_point[1], max_point[2]);
2077 Eigen::Vector3d p8(max_point[0], max_point[1], min_point[2]);
2101 std_msgs::ColorRGBA this_color =
getColor(color);
2173 scales scale, std::size_t
id)
2185 Eigen::Vector3d p1(-width / 2.0, -height / 2.0, 0.0);
2186 Eigen::Vector3d p2(-width / 2.0, height / 2.0, 0.0);
2187 Eigen::Vector3d p3(width / 2.0, height / 2.0, 0.0);
2188 Eigen::Vector3d p4(width / 2.0, -height / 2.0, 0.0);
2199 std_msgs::ColorRGBA this_color =
getColor(color);
2231 const Eigen::Vector3d& p2_in,
const Eigen::Vector3d& p3_in,
2232 const Eigen::Vector3d& p4_in,
colors color,
scales scale)
2252 std_msgs::ColorRGBA this_color =
getColor(color);
2284 const std::string& ns)
2286 std::vector<geometry_msgs::Point> points_msg;
2288 for (
const auto& point : points)
2297 const std::string& ns)
2299 std::vector<geometry_msgs::Point> points_msg;
2301 for (
const auto& point : points)
2310 const std::string& ns)
2312 geometry_msgs::Vector3 scale_vector;
2320 const std::string& ns)
2326 const geometry_msgs::Vector3& scale,
const std::string& ns)
2334 std_msgs::ColorRGBA this_color =
getColor(color);
2342 for (std::size_t i = 0; i < points.size(); ++i)
2352 scales scale,
const std::string& ns)
2354 BOOST_ASSERT_MSG(points.size() == colors.size(),
"Mismatching vector sizes: points and colors");
2356 std::vector<geometry_msgs::Point> points_msg;
2357 std::vector<std_msgs::ColorRGBA> colors_msg;
2359 for (std::size_t i = 0; i < points.size(); ++i)
2364 colors_msg.push_back(
getColor(colors[i]));
2371 const std::vector<std_msgs::ColorRGBA>&
colors,
2372 const geometry_msgs::Vector3& scale,
const std::string& ns)
2395 const geometry_msgs::Vector3 scale,
bool static_id)
2407 const geometry_msgs::Vector3 scale,
bool static_id)
2441 geometry_msgs::Pose pose_msg;
2453 Eigen::Isometry3d shared_pose_eigen_;
2455 return shared_pose_eigen_;
2465 Eigen::Isometry3d shared_pose_eigen_;
2466 shared_pose_eigen_ = Eigen::Isometry3d::Identity();
2467 shared_pose_eigen_.translation().x() = point.x;
2468 shared_pose_eigen_.translation().y() = point.y;
2469 shared_pose_eigen_.translation().z() = point.z;
2470 return shared_pose_eigen_;
2475 geometry_msgs::Pose pose_msg;
2476 pose_msg.orientation.x = 0.0;
2477 pose_msg.orientation.y = 0.0;
2478 pose_msg.orientation.z = 0.0;
2479 pose_msg.orientation.w = 1.0;
2480 pose_msg.position = point;
2486 Eigen::Isometry3d shared_pose_eigen_;
2487 shared_pose_eigen_ = Eigen::Isometry3d::Identity();
2488 shared_pose_eigen_.translation() = point;
2489 return shared_pose_eigen_;
2494 geometry_msgs::Pose pose_msg;
2496 return pose_msg.position;
2501 Eigen::Vector3d point_eigen;
2502 point_eigen[0] = point.x;
2503 point_eigen[1] = point.y;
2504 point_eigen[2] = point.z;
2510 Eigen::Vector3d point_eigen;
2511 point_eigen[0] = point.x;
2512 point_eigen[1] = point.y;
2513 point_eigen[2] = point.z;
2519 geometry_msgs::Point32 point32_msg;
2520 point32_msg.x = point[0];
2521 point32_msg.y = point[1];
2522 point32_msg.z = point[2];
2528 geometry_msgs::Point point_msg;
2529 point_msg.x = point.x;
2530 point_msg.y = point.y;
2531 point_msg.z = point.z;
2537 geometry_msgs::Point point_msg;
2538 point_msg.x = point.x();
2539 point_msg.y = point.y();
2540 point_msg.z = point.z();
2547 Eigen::Isometry3d result;
2552 result = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) *
2553 Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
2557 result = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) *
2558 Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX());
2562 result = Eigen::Translation3d(tx, ty, tz) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) *
2563 Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
2576 if (transform6.size() != 6)
2582 return convertFromXYZRPY(transform6[0], transform6[1], transform6[2], transform6[3], transform6[4], transform6[5],
2589 convertToXYZRPY(pose, xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4], xyzrpy[5]);
2593 double& pitch,
double& yaw)
2600 Eigen::Vector3d vec = pose.rotation().eulerAngles(0, 1, 2);
2608 Eigen::Isometry3d pose_eigen;
2665 Eigen::Vector3d axis;
2666 axis[0] =
sin(elevation) *
cos(azimuth);
2667 axis[1] =
sin(elevation) *
sin(azimuth);
2668 axis[2] =
cos(elevation);
2670 Eigen::Quaterniond quaternion(Eigen::AngleAxis<double>(static_cast<double>(angle), axis));
2671 pose = Eigen::Translation3d(pose.translation().x(), pose.translation().y(), pose.translation().z()) * quaternion;
2677 pose.position.x = 0;
2678 pose.position.y = 0;
2679 pose.position.z = 0;
2682 pose.orientation.x = 0;
2683 pose.orientation.y = 0;
2684 pose.orientation.z = 0;
2685 pose.orientation.w = 1;
2690 geometry_msgs::Pose pose;
2692 pose.position.x = 0;
2693 pose.position.y = 0;
2694 pose.position.z = 0;
2697 pose.orientation.x = 0;
2698 pose.orientation.y = 0;
2699 pose.orientation.z = 0;
2700 pose.orientation.w = 1;
2706 static const std::size_t NUM_VARS = 16;
2708 for (std::size_t i = 0; i < NUM_VARS; ++i)
2710 if (fabs(pose1.data()[i] - pose2.data()[i]) > threshold)
2721 double d =
static_cast<double>(rand()) / RAND_MAX;
2722 return min + d * (max - min);
2727 float d =
static_cast<float>(rand()) / RAND_MAX;
2728 return min + d * (max - min);
2733 int n = max - min + 1;
2734 int remainder = RAND_MAX % n;
2739 }
while (x >= RAND_MAX - remainder);
2745 std::cout <<
"T.xyz = [" << translation.x() <<
", " << translation.y() <<
", " << translation.z() <<
"]" << std::endl;
2750 Eigen::Quaterniond q(transform.rotation());
2751 std::cout <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", " 2752 << transform.translation().z() <<
"], Q.xyzw = [" << q.x() <<
", " << q.y() <<
", " << q.z() <<
", " 2753 << q.w() <<
"]" << std::endl;
2759 Eigen::Vector3d vec = transform.rotation().eulerAngles(0, 1, 2);
2760 std::cout <<
"transform: [" << transform.translation().x() <<
", " << transform.translation().y() <<
", " 2761 << transform.translation().z() <<
", " << vec[0] <<
", " << vec[1] <<
", " << vec[2] <<
"]\n";
2766 std::cout <<
"T.xyz = [" << transform.translation().x() <<
", " << transform.translation().y() <<
", " 2767 << transform.translation().z() <<
"], R = \n" 2768 << transform.rotation() <<
"\n";
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
#define ROS_INFO_STREAM_NAMED(name, args)
void vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void publish(const boost::shared_ptr< M > &message) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
std::string getTopic() const
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
#define ROS_ERROR_STREAM(args)
uint32_t getNumSubscribers() const
ROSCPP_DECL void spinOnce()
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
#define ROS_WARN_STREAM_NAMED(name, args)