2 #include <urdf_model/link.h>
6 namespace vm = visualization_msgs;
10 std_msgs::ColorRGBA&
setColor(std_msgs::ColorRGBA& color,
Color color_id,
double alpha) {
108 return start * (1.0 - fraction) + end * fraction;
111 std_msgs::ColorRGBA&
interpolate(std_msgs::ColorRGBA& color,
const std_msgs::ColorRGBA& other,
double fraction) {
123 std_msgs::ColorRGBA&
brighten(std_msgs::ColorRGBA& color,
double fraction) {
124 static std_msgs::ColorRGBA white;
130 std_msgs::ColorRGBA&
darken(std_msgs::ColorRGBA& color,
double fraction) {
131 static std_msgs::ColorRGBA black;
136 std_msgs::ColorRGBA result;
141 geometry_msgs::Pose
composePoses(
const geometry_msgs::Pose& first,
const Eigen::Isometry3d& second) {
142 Eigen::Isometry3d result_eigen;
144 result_eigen = result_eigen * second;
148 geometry_msgs::Pose
composePoses(
const Eigen::Isometry3d& first,
const geometry_msgs::Pose& second) {
149 Eigen::Isometry3d result_eigen;
151 result_eigen = first * result_eigen;
156 m.action = vm::Marker::ADD;
157 m.type = marker_type;
162 if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0)
163 m.pose.orientation.w = 1.0;
167 geometry_msgs::Point p[4];
185 m.scale.x = m.scale.y = m.scale.z = 1.0;
187 m.points.push_back(p[0]);
188 m.points.push_back(p[1]);
189 m.points.push_back(p[2]);
191 m.points.push_back(p[2]);
192 m.points.push_back(p[3]);
193 m.points.push_back(p[0]);
200 for (
auto& p : m.points)
208 for (
auto& p : m.points)
215 m.scale.x = m.scale.y = m.scale.z = 1.0;
217 geometry_msgs::Point p[3];
218 p[0].x = p[0].y = p[0].z = 0.0;
219 p[1].x = p[2].x = 1.0;
221 const double delta_theta = M_PI / 16.0;
224 for (std::size_t i = 0; i < 32; i++) {
225 p[1].y = cos(theta) /
angle;
226 p[1].z = sin(theta) /
angle;
228 p[2].y = cos(theta + delta_theta) /
angle;
229 p[2].z = sin(theta + delta_theta) /
angle;
231 m.points.push_back(p[0]);
232 m.points.push_back(p[1]);
233 m.points.push_back(p[2]);
235 theta += delta_theta;
241 m.scale.x = m.scale.y = m.scale.z = radius;
246 vm::Marker&
makeBox(vm::Marker& m,
double x,
double y,
double z) {
254 vm::Marker&
makeCylinder(vm::Marker& m,
double diameter,
double height) {
255 m.scale.x = m.scale.y = diameter;
261 vm::Marker&
makeMesh(vm::Marker& m,
const std::string& filename,
double sx,
double sy,
double sz) {
266 m.mesh_resource = filename;
267 m.mesh_use_embedded_materials = 1u;
271 vm::Marker&
makeArrow(vm::Marker& m,
const Eigen::Vector3d& start_point,
const Eigen::Vector3d& end_point,
272 double diameter,
double head_length) {
275 m.scale.x = diameter;
276 m.scale.y = 2 * diameter;
277 m.scale.z = head_length;
285 vm::Marker&
makeArrow(vm::Marker& m,
double scale,
bool tip_at_origin) {
286 m.scale.y = m.scale.z = 0.1 * scale;
290 m.pose =
composePoses(m.pose, Eigen::Translation3d(-scale, 0, 0) * Eigen::Isometry3d::Identity());
294 vm::Marker&
makeText(vm::Marker& m,
const std::string& text) {
295 m.scale.x = m.scale.y = m.scale.z = 1.0;
303 case urdf::Geometry::SPHERE: {
304 const urdf::Sphere& sphere =
static_cast<const urdf::Sphere&
>(geom);
308 case urdf::Geometry::BOX: {
309 const urdf::Box& box =
static_cast<const urdf::Box&
>(geom);
310 makeBox(m, box.dim.x, box.dim.y, box.dim.z);
313 case urdf::Geometry::CYLINDER: {
314 const urdf::Cylinder& cylinder =
static_cast<const urdf::Cylinder&
>(geom);
315 makeCylinder(m, 2.0 * cylinder.radius, cylinder.length);
318 case urdf::Geometry::MESH: {
319 const urdf::Mesh& mesh =
static_cast<const urdf::Mesh&
>(geom);
320 makeMesh(m, mesh.filename, mesh.scale.x, mesh.scale.y, mesh.scale.z);
324 ROS_WARN(
"Unsupported geometry type: %d", geom.type);