Add a key to toggle link names, fix #165

src/app.rs

@@ -114,6 +114,7 @@ r:    set random angles
 z:    reset joint positions and origin
 c:    toggle visual/collision
 f:    toggle show link frames
+n:    toggle show link names
 m:    toggle show menu
 "#;
 
@@ -137,6 +138,7 @@ pub struct UrdfViewerApp {
     handle: Arc<RobotStateHandle>,
     is_collision: bool,
     show_frames: bool,
+    show_link_names: bool,
     ik_constraints: k::Constraints,
     point_size: f32,
     package_path: HashMap<String, String>,
@@ -228,6 +230,7 @@ impl UrdfViewerApp {
             handle: Arc::new(handle),
             is_collision,
             show_frames: false,
+            show_link_names: false,
             ik_constraints: k::Constraints::default(),
             point_size: 10.0,
             package_path,
@@ -306,6 +309,26 @@ impl UrdfViewerApp {
         }
     }
 
+    fn draw_link_names(&mut self, window: &mut Window) {
+        for n in self.robot.iter() {
+            let name = node_to_frame_name(n);
+            let link_name = if let Some(n) = name.strip_suffix("_joint_frame") {
+                n
+            } else {
+                name.as_str()
+            };
+            let tr = n.world_transform().unwrap();
+            let t = tr.translation;
+            self.viewer.draw_text_from_3d(
+                window,
+                link_name,
+                40.0,
+                &na::Point3::new(t.x, t.y, t.z),
+                &na::Point3::new(1f32, 1.0, 1.0),
+            );
+        }
+    }
+
     fn update_robot(&mut self) {
         // this is hack to handle invalid mimic joints, like hsr
         let joint_positions = self.robot.joint_positions();
@@ -473,6 +496,9 @@ impl UrdfViewerApp {
                 self.show_frames = !self.show_frames;
                 self.update_frame_markers();
             }
+            Key::N => {
+                self.show_link_names = !self.show_link_names;
+            }
             #[cfg(not(target_family = "wasm"))]
             Key::L => {
                 // reload
@@ -825,6 +851,9 @@ impl window::State for AppState {
                 &na::Point3::new(0.9f32, 0.5, 1.0),
             );
         }
+        if self.app.show_link_names {
+            self.app.draw_link_names(window);
+        }
 
         for mut event in window.events().iter() {
             match event.value {

src/viewer.rs

@@ -1,6 +1,7 @@
 use crate::urdf::*;
 use k::nalgebra as na;
 use kiss3d::camera::ArcBall;
+use kiss3d::camera::Camera;
 use kiss3d::ncollide3d::simba::scalar::SubsetOf;
 use kiss3d::scene::SceneNode;
 use kiss3d::window::Window;
@@ -224,6 +225,37 @@ impl Viewer {
     ) {
         window.draw_text(text, pos, size, &self.font, color);
     }
+
+    pub fn draw_text_from_3d(
+        &mut self,
+        window: &mut Window,
+        text: &str,
+        size: f32,
+        pos: &na::Point3<f32>,
+        color: &na::Point3<f32>,
+    ) {
+        let height = window.height() as f32;
+        let width = window.width() as f32;
+        let text_position_in_2d: na::Matrix<
+            f32,
+            na::Const<2>,
+            na::Const<1>,
+            na::ArrayStorage<f32, 2, 1>,
+        > = self.arc_ball.project(pos, &na::Vector2::new(width, height));
+        self.draw_text(
+            window,
+            text,
+            size,
+            // The x2 factor should be removed for kiss3d >= 0.36
+            // See: https://github.com/sebcrozet/kiss3d/issues/98
+            &na::Point2::new(
+                text_position_in_2d.x * 2.0,
+                (height - text_position_in_2d.y) * 2.0,
+            ),
+            color,
+        );
+    }
+
     pub fn set_temporal_color(&mut self, link_name: &str, r: f32, g: f32, b: f32) {
         if let Some(obj) = self.scenes.get_mut(link_name) {
             obj.set_color(r, g, b);