Tutorial: Advanced Visualization Features - Drawing to the Debug Layer

Table of Contents

• Getting Started
  • Create your own package
  • Generate template files of your new component and a test application
• Start Implementing
• Show the DebugLayer in ArmarXGui
• Advanced Drawing Capabilities
• Showing Objects from Memory

In this tutorial, advanced visualization features are explained. It is shown how the DebugDrawer (and an extended version, the EntityDrawer) is used and how the layered visualization can be dispalyed in several ArmarXGui plugins.

Getting Started

For this tutorial, you need following prerequisites:

• A copy of ArmarX including RobotAPI (containing the DebugDrawer component), ArmarXGui (to visualize the drawings), and optionally MemoryX (containing the EntityDrawer that can be used to display objects from the memory).

Create your own package

If you already have a package in which you would like to add this tutorial, go to the next step Generate template files of your new component and a test application.

In order to create a new ArmarX package called "RobotComponentTutorials", execute the following code in the directory where you want to make your package,

${ArmarX_DIR}/ArmarXCore/build/bin/armarx-package init RobotComponentTutorials

or you can use "-d" option to specify the directory.

${ArmarX_DIR}/ArmarXCore/build/bin/armarx-package init RobotComponentTutorials -d ${ArmarX_DIR}/

Generate template files of your new component and a test application

Execute the following commands in the toplevel directory of your package. All required files for the component and the test application will be generated.

cd RobotComponentTutorials
${ArmarX_DIR}/ArmarXCore/build/bin/armarx-package add component DebugDrawerTutorial
${ArmarX_DIR}/ArmarXCore/build/bin/armarx-package add application DebugDrawerTutorial

Have a look into the directory to check if everything worked as intended. You should find the following files:

source/RobotComponentTutorials/components/CMakeLists.txt
source/RobotComponentTutorials/components/DebugDrawerTutorial
source/RobotComponentTutorials/components/DebugDrawerTutorial/DebugDrawerTutorial.h
source/RobotComponentTutorials/components/DebugDrawerTutorial/DebugDrawerTutorial.cpp
source/RobotComponentTutorials/components/DebugDrawerTutorial/CMakeLists.txt

Start Implementing

• First, we load the component to the QtCraetor IDE (as described in Tutorial: Counting with Statecharts -> Edit the source code). You should have a project similar to the following setup:

• Add the following line into the CMakeLists.txt at the top-level directory of your project.

  depends_on_armarx_package(RobotAPI)
  depends_on_armarx_package(MemoryX "OPTIONAL")

• Edit the CMakeLists.txt file in your DebugDrawerTutorial component directory.
  1. Add the following dependencies

     find_package(Eigen3 QUIET)
     find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
      
     armarx_build_if(Eigen3_FOUND "Eigen3 not available")
     armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
      
     include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
     include_directories(${Simox_INCLUDE_DIRS})

  2. Add the library

     set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore RobotAPICore ${Simox_LIBRARIES})

• Edit DebugDrawerTutorial.h
  1. Add includes

     #include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
     #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>

  2. Add DebugDrawer Proxy

     protected:
         armarx::DebugDrawerInterfacePrx debugDrawerPrx;

• Edit DebugDrawerTutorial.cpp
  1. Offering the debugDrawer topic

     void DebugDrawerTutorial::onInitComponent()
     {
         // writing to the DebugDrawer topic. In ArmarX this topic's default name is DebugDrawerUpdates
         offeringTopic("DebugDrawerUpdates");
     }

  2. Get the proxy and draw a coordinate system

     void DebugDrawerTutorial::onConnectComponent()
     {
         // retrieve the proxy to the debug drawer
         debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
         
         // setup a 4x4 homogeneous matrix
         Eigen::Matrix4f p;
         p.setIdentity();
         
         // set x,y,z coordinates (mm)
         p(0,3) = 0.0f;
         p(1,3) = 500.0f;
         p(2,3) = 1000.0f;
         
         // draw the pose
         PosePtr p_global(new Pose(p));
         debugDrawerPrx->setPoseDebugLayerVisu("MyPose", p_global);
     }

• Setup an application to test the component

  1. Update the application's /source/ddtest/applications/DebugDrawerTest/CMakeLists.txt

     find_package(Eigen3 QUIET)
     armarx_build_if(Eigen3_FOUND "Eigen3 not available")
     include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
     set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore DebugDrawerTutorial)

  At this point, the package should compile without any errors. To make sure it does so, first run CMake and then build the package.

Show the DebugLayer in ArmarXGui

The debug layer is implemented in several gui plugins (currently the DebugDrawerViewer, WorkingMemoryGui, RobotViewerGui) and in the Viewer of the DynamicSimulator. In the following example, we show how to start the kinemtic simulation of Armar3 in order to show the debug layer of the RobotViewer gui plugin.

1. First, we start a robot simulation (in a separate terminal shell), e.g. the RobotAPI scenario KinemticSimulationArmar3

   cd ${ArmarX_DIR}/RobotAPI/scenarios/KinematicSimulationArmar3
   ./startSceanrio.sh

2. Now, we can start the ArmarXGui and load the RobotViewer

   armarx gui

   Select AddWidget->RobotControl->RobotViewerGUI and configure as shown below.
   
   The gui plugin with some options and the Armar3 robot should be visualized as follows
   
3. The demo application is started in order to paint on the debug layer:

   ${ArmarX_DIR}/RobotComponentTutorials/build/bin/DebugDrawerTutorialAppRun 

   Then, the coordinate system should show up in the ArmarXGui 3D visualization as follows:
   
4. Now you can safely stop the example by pressing <ctrl>-c in the shell where you started the DebugDrawerTutorialAppRun. The visualization remains until it is updated or the gui plugin is started again.

Advanced Drawing Capabilities

Now we want to dispaly some more graphics. Therefore, we add periodic task to continously update the visualization.

• Edit DebugDrawerTutorial.h
  1. Add includes

     #include <ArmarXCore/core/services/tasks/PeriodicTask.h>

  2. Add run method and periodic task object

     protected:
         void run();
         armarx::DebugDrawerInterfacePrx debugDrawerPrx;
         PeriodicTask<DebugDrawerTutorial>::pointer_type task;

• Edit DebugDrawerTutorial.cpp

  1. Setup and start the periodic task

     void DebugDrawerTutorial::onConnectComponent()
     {
         // retrieve the proxy to the debug drawer and clear it 
         debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
         debugDrawerPrx->clearDebugLayer();
      
         try
         {
             task = new PeriodicTask<DebugDrawerTutorial>(this, &DebugDrawerTutorial::run, 10, false, "DebugDrawerTutorialTask");
             task->start();
         }
         catch (...)
         {
             ARMARX_WARNING << "Failed to start DebugDrawerTutorial task";
         }
     }

     1. Implement periodic run method

        void DebugDrawerTutorial::run()
        {
            // setup a 4x4 homogeneous matrix
            Eigen::Matrix4f p;
            p.setIdentity();
         
            // set rotating coordinates (mm)
            p(0,3) = sin(clock() / 10000.0)*200;
            p(1,3) = 500.0f;
            p(2,3) = 1000.0f + cos(clock() / 10000.0)*200;
            PosePtr p_global(new Pose(p));
         
            // size of the box in mm
            Eigen::Vector3f v(100,100,100);
            Vector3Ptr s(new Vector3(v));
         
            // draw/update red box
            debugDrawerPrx->setBoxDebugLayerVisu("MyBox", p_global, s, DrawColor {1, 0, 0, 1});
        }

     2. Stop the run() method when the component gets disconnected:

        void DebugDrawerTutorial::onDisconnectComponent()
        {
            task->stop();
        }

  When starting the application again you should see a rotating box

  
  1. Add some more visualizations The following list is an exerpt from the DebugDrawer's slice definition file (DebugDrawerInterface.ice) and shows several visualization features you can play around with.
     • Pose (Coordinate systems)

       setPoseDebugLayerVisu(string poseName, PoseBase globalPose);
       setScaledPoseDebugLayerVisu(string poseName, PoseBase globalPose, float scale);

     • Line

       void setLineDebugLayerVisu(string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);

     • Box

       void setBoxDebugLayerVisu(string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);

     • Text

       void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);

     • Sphere

       void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);

     • PointCloud

       void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);

     • Polygon

       void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);

     • Arrow

       void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);

     • Cylinder

       void setCylinderDebugLayerVisu(string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);

• Custom Layers

    Until now, all visualizations have been drawn to the "debug" layer, which is a default layer of the DebugDrawer. You can add custom layers by specifying the layer string.
  

  E.g. the call

  debugDrawerPrx->setBoxDebugLayerVisu("MyBox", p_global, s, DrawColor {1, 0, 0, 1});

  becomes

  debugDrawerPrx->setBoxVisu("myLayer","MyBox", p_global, s, DrawColor {1, 0, 0, 1});

  You can clear your custom layer with

  clearLayer(string layerName);

• Add robot visualizations

  A robot can be visualized in a network transparent way by the DebugDrawer. Edit the DebugDrawerTutorial.cpp as follows:

  1. Setup the robot in onConnectComponent()

     void DebugDrawerTutorial::onConnectComponent()
     {
         ...
         // draw robot collision model (ArmarIII.xml in project RobotAPI)
         debugDrawerPrx->setRobotVisu("MyRobotLayer", "MyRobot", "RobotAPI/robots/Armar3/ArmarIII.xml", "RobotAPI", armarx::CollisionModel);
      
         // set position of robot
         Eigen::Matrix4f p = Eigen::Matrix4f::Identity();
         p(0,3) = 1000;
         debugDrawerPrx->updateRobotPose("MyRobotLayer", "MyRobot", new Pose(p));
      
         // set color to green
         debugDrawerPrx->updateRobotColor("MyRobotLayer", "MyRobot", DrawColor {0, 1, 0, 1});
     }

  2. Update the configuration (joint angles) in the run() method

     void DebugDrawerTutorial::run()
     {
         ...
         // update robot joint config
         std::map<std::string, float> c;
         c["Shoulder 1 R"] = -sin(clock() / 10000.0) / 2;
         c["Shoulder 1 L"] = sin(clock() / 10000.0) / 2;
         debugDrawerPrx->updateRobotConfig("MyRobotLayer", "MyRobot", c);
     }

  The result is a moving robot in the "MyRobotLayer" debug layer:

  

Showing Objects from Memory

tbd...