3 #include <Eigen/Geometry>
4 #include <geometry_msgs/PoseStamped.h>
5 #include <std_msgs/ColorRGBA.h>
6 #include <visualization_msgs/Marker.h>
32 std_msgs::ColorRGBA
getColor(
Color color,
double alpha = 1.0);
33 std_msgs::ColorRGBA&
setColor(std_msgs::ColorRGBA& color,
Color color_id,
double alpha = 1.0);
35 std_msgs::ColorRGBA&
interpolate(std_msgs::ColorRGBA& color,
const std_msgs::ColorRGBA& other,
double fraction);
36 std_msgs::ColorRGBA&
brighten(std_msgs::ColorRGBA& color,
double fraction);
37 std_msgs::ColorRGBA&
darken(std_msgs::ColorRGBA& color,
double fraction);
39 geometry_msgs::Pose
composePoses(
const geometry_msgs::Pose& first,
const Eigen::Isometry3d& second);
40 geometry_msgs::Pose
composePoses(
const Eigen::Isometry3d& first,
const geometry_msgs::Pose& second);
46 visualization_msgs::Marker&
makeXYPlane(visualization_msgs::Marker& m);
48 visualization_msgs::Marker&
makeXZPlane(visualization_msgs::Marker& m);
49 visualization_msgs::Marker&
makeYZPlane(visualization_msgs::Marker& m);
52 visualization_msgs::Marker&
makeCone(visualization_msgs::Marker& m,
double angle);
54 visualization_msgs::Marker&
makeSphere(visualization_msgs::Marker& m,
double radius = 1.0);
57 visualization_msgs::Marker&
makeCylinder(visualization_msgs::Marker& m,
double diameter,
double height);
60 visualization_msgs::Marker&
makeBox(visualization_msgs::Marker& m,
double x,
double y,
double z);
63 visualization_msgs::Marker&
makeMesh(visualization_msgs::Marker& m,
const std::string& filename,
double sx = 1.0,
64 double sy = 1.0,
double sz = 1.0);
65 inline visualization_msgs::Marker&
makeMesh(visualization_msgs::Marker& m,
const std::string& filename,
double scale) {
66 return makeMesh(m, filename, scale, scale, scale);
70 visualization_msgs::Marker&
makeArrow(visualization_msgs::Marker& m,
const Eigen::Vector3d& start_point,
71 const Eigen::Vector3d& end_point,
double diameter,
double head_length = 0.0);
74 visualization_msgs::Marker&
makeArrow(visualization_msgs::Marker& m,
double scale = 1.0,
bool tip_at_origin =
false);
77 visualization_msgs::Marker&
makeText(visualization_msgs::Marker& m,
const std::string& text);
80 visualization_msgs::Marker&
makeFromGeometry(visualization_msgs::Marker& m,
const urdf::Geometry& geom);
83 void appendFrame(T& container,
const geometry_msgs::PoseStamped& pose,
double scale = 1.0,
84 const std::string& ns =
"frame",
double diameter_fraction = 0.1) {
85 visualization_msgs::Marker m;
88 m.header = pose.header;
91 m.pose =
composePoses(pose.pose, Eigen::Translation3d(scale / 2.0, 0, 0) *
92 Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));
94 container.push_back(m);
97 m.pose =
composePoses(pose.pose, Eigen::Translation3d(0, scale / 2.0, 0) *
98 Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()));
100 container.push_back(m);
103 m.pose =
composePoses(pose.pose, Eigen::Translation3d(0, 0, scale / 2.0) * Eigen::Isometry3d::Identity());
105 container.push_back(m);