How to display the robot location on top of the Viewer?

[ngunhaSO]

Can someone point me to an example to display the current robot position on top of the viewer?
I used the example/continuous.html and add the listener to listen to /cmd_vel, but i am not sure how to draw the robot position on top of the viewer.

var ros = new ROSLIB.Ros({
      url : 'ws://<ip_address>:9090'
    });

    // Create the main viewer.
    var viewer = new ROS2D.Viewer({
      divID : 'map',
      width : 800,
      height : 550
    });

// Setup the map client.
    var gridClient = new ROS2D.OccupancyGridClient({
      ros : ros,
      rootObject : viewer.scene,
      // Use this property in case of continuous updates			
      continuous: true
    });
    let startTime ;

    // Scale the canvas to fit to the map
    gridClient.on('change', function() {
      startTime = new Date()
      viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
      viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
    });

  var vel_listener = new ROSLIB.Topic({
			ros: ros,
			name: '/cmd_vel',
			messageType: 'geometry_msgs/Twist'
		})

 vel_listener.subscribe(function(vel){
      let endTime = new Date();
      let dt = (endTime - startTime) / 5000;
      let vx = vel.linear.x;
      let az = vel.angular.z;
     //how to add the robot position marker on the viewer?
});

[ngunhaSO]

Ok, so I tried to use OccupancyGridClientNav, but it doesn't seem to be able to add 2 nav objects to the same viewer scene? I have 2 robots on the same map and I want to display both of them. I tried this but it didn't work

var ros = new ROSLIB.Ros({
      url : 'ws://robot1:9090'
    });
    var ros2 = new ROSLIB.Ros({
      url : 'ws://robot2:9090'
    });
    // Create the main viewer.
    var viewer = new ROS2D.Viewer({
      divID : 'map',
      width : 800,
      height : 550
    });
    var nav = NAV2D.OccupancyGridClientNav({
      ros : ros2,
      topic : '/map',
      rootObject : viewer.scene,
      viewer : viewer,
      serverName : '/move_base'
    });
    var nav2 = NAV2D.OccupancyGridClientNav({
      ros : ros,
      topic : '/map',
      rootObject : viewer.scene,
      viewer : viewer,
      serverName : '/move_base'
    });

[timdori]

Hey. Right now i m trying the same... did you it working?
I also want to show markers... Do you have any idea how to do it?

[corey-mitchell]

@ngunhaSO @timdori were either of you able to get this feature working? I'm stuck with the same issue. I finally got the map to display and fit to the size of the canvas, but I can't figure out how to show the arrow for my bot's position. NAV2D.OccupancyGridClientNav() displays the map and if I double click, it will display the purple arrow telling the bot where to go, but it never shows the yellow arrow showing where the bot is currently located.

[corey-mitchell]

Finally got it using the NAV2D to work. User error as per the norm.

For any future users, make sure you are running the robot_pose_publisher as mentioned in the nav2djs documentation. The robot_pose_publisher needs to be running in order for this widget to work.

[ngunhaSO]

@corey-mitchell I did a workaround to use a for loop and a dynamic function to add multiple robot on the map

[corey-mitchell]

@ngunhaSO Can you provide any snippets by chance? I already know that I will want this feature in the future and it would be nice to have a usable reference.

[CycleMark]

I too would be interest to see how you add a robot to a OccupancyGridClient. I've got it working OccupancyGridClientNav but I'd like more control over it and I don't need to interact with it.

Thanks

Mark

[ngunhaSO]

@corey-mitchell @CycleMark
Here is what i did: I created a default ros and a default OccupancyGridClient instance. Then i pull the list of robots ip address that i have into an array, then subscribe each of the robot. This is not an elegant way but back then, i needed a solution quick and this seemed to work for me.

var ros = new ROSLIB.Ros({
      url : 'ws://10.244.57.30:9090'
    });
    var viewer = new ROS2D.Viewer({
      divID : 'map',
      width : 800,
      height : 550
    });
    let gridClient = new ROS2D.OccupancyGridClient({
        ros : ros,
        rootObject : viewer.scene,
    });
    // Scale the canvas to fit to the map
    gridClient.on('change', function(){
        viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
        viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
    });
    var ip = ['10.244.57.30', '10.244.57.20',  '10.244.57.10' ]
    var robotMarkers = [];
    var topics = [];

var createFunc = function (handlerToCall, discriminator, robotMarker) {
        return discriminator.subscribe(function(pose){
            robotMarker.x = pose.pose.pose.position.x;
            robotMarker.y = -pose.pose.pose.position.y;
            var quaZ = pose.pose.pose.orientation.z;
            var degreeZ = 0;
            if( quaZ >= 0 ) {
                degreeZ = quaZ / 1 * 180
            } else {
                degreeZ = (-quaZ) / 1 * 180 + 180
            };
            robotMarker.rotation = -degreeZ + 35;
        })
    }

for(let i = 0; i < ip.length; i++){
        let ros = new ROSLIB.Ros({
            url : 'ws://' + ip[i] + ':9090'
        });
        // Setup the map client.
        var robotMarker = new ROS2D.NavigationArrow({
            size : 0.25,
            strokeSize : 0.05,
            pulse: true,
            fillColor: createjs.Graphics.getRGB(randomr(), randomg(), randomb(), 0.65)
        });
        robotMarkers.push(robotMarker)
        var poseTopic = new ROSLIB.Topic({
            ros: ros,
            name: '/amcl_pose',
            messageType: 'geometry_msgs/PoseWithCovarianceStamped'
        });
        topics.push(poseTopic);
        createFunc('subscribe', poseTopic, robotMarker);
    }

for(let i = 0; i < robotMarkers.length; i++){
        gridClient.rootObject.addChild(robotMarkers[i]);
    }
```

`

And as the result, i can see all three robots. Also, you can create an array of json object to keep track of the color assigned to each robot in case you need to display legends so the end user know which robot


<img width="791" alt="screenshot 2019-02-19 at 1 34 16 pm" src="https://user-images.githubusercontent.com/15272065/52992336-35201f80-344b-11e9-8906-39f6c697434a.png">

[CycleMark]

Hi @ngunhaSO - This is great, many thanks. Really good jump off point. Pointer sometimes throws a rotation wobble - I shall investigate.

Thanks again

Mark

[ngunhaSO]

@CycleMark I experienced that too but my time was limited back there so i used circle image for displaying. But yeah, i believe with this approach you can control the subscription in 'createFunc()' by checking the type of handler to call

[corey-mitchell]

I'm coming back for future users who may be wanting to make a more custom map with click handlers and such.

I would first like to note, that this approach is not for everyone and may not even be what most people are looking for. But I wanted to share because it was a bit of a pain for me to make so I figured I might be able to help someone else down the road.

I wanted to handle custom click events on my map so that we could develop features such as desired travel lanes and setting up "No-Go" zones on-the-fly. So, I made a custom map that allows me to do just that as well as monitor multiple bot's position in real time.

Here's a sample of said map with a single bot:
`

<!-- RWT CDNs -->
<script type="text/javascript" src="https://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
<script type="text/javascript" src="http://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>

<!-- EaselJS -->
<script type="text/javascript" src="https://code.createjs.com/easeljs-0.8.2.min.js"></script>

<!-- Three.js -->
<script type="text/javascript" src="three.js"></script>

<!-- Canvas Script -->
<script type="text/javascript">
  function init() {
    // Create ROS node and connect to it
    const ros = new ROSLIB.Ros({
      url: 'ws://localhost:9090'
    });

    // Make robot pose subscriber
    const robotPose = new ROSLIB.Topic({
      ros : ros,
      name : '/robot_pose',
      messageType : 'geometry_msgs/Pose'
    });

    // Create an EaselJS stage from the canvas
    const canvas = document.getElementById("myCanvas");
    const stage = new createjs.Stage('myCanvas');

    // Create Map Image
    const map = new Image();
    map.src = 'images/yourMapImage.png';

    // Once map image has loaded...
    map.onload = function(event) {
      // Render Map to canvas after scaling the map and canvas to the appropriate proportions.
      const mapImage = event.target;
      const bitMap = new createjs.Bitmap(mapImage);
      bitMap.scaleX = 500 / canvas.width;
      bitMap.scaleY = 500 / canvas.height;
      canvas.width = 500;
      canvas.height = 500;
      stage.addChild(bitMap);
      stage.update()

      // Create bot image
      const botImg = new Image();
      botImg.src = 'images/yourBotImage.png';

      // Once bot image has loaded...
      botImg.onload = function(event) {
        // Create new Easel bitmap from bot image
        const botImage = event.target;
        const botBitmap = new createjs.Bitmap(botImage);

        // Create a hit box for the new image (This allows Easel to register image click events)
        const hitArea = new createjs.Shape();
        hitArea.graphics.f('#000').dr(0, 0, botBitmap.image.width, botBitmap.image.height);
        botBitmap.hitArea = hitArea;

        // Add 'Click' event listener to bot image
        botBitmap.addEventListener('click', () => {
          console.log('Bot clicked');
        });

        // Set bot img dimensions 
        // Shift image registration point from top left corner to center of image.
        // This helps with alignment and rotates image around center rather than top left corner.
        const botImgSize = 30;
        botBitmap.scaleX = botImgSize / 193;
        botBitmap.scaleY = botImgSize / 280;
        botBitmap.regX = 193 / 2;
        botBitmap.regY = 280 / 2;
        // The numbers 193 and 280 come from the original dimensions of the bot image (in this case the dimensions are 193x280)

        // Target map resolution and bot origin point.
        const resolution = 0.05;
        const originX = Math.abs(-109.2);
        const originY = Math.abs(-35.35);
        // Why the absolute value?
        // Because the origin point of the bot is based upon where the bottom left corner pixel is relative to the robot's reference to (0, 0). (Kinda confusing)

        // Calculate robot's starting point

        // Adjust canvas starting position from (0, 0) being top left corner (html canvas standard) to the bottom left corner (image standard).
        // This will align canvas and map references to the same grid points.
        // Since X = 0 is still the farthest left side, we only need to shift the Y value.
        const yZeroShift = canvas.height;

        // Calculate which pixel (a.k.a. cell) to place the robot image in the canvas.
        // To get the pixel from the robot's position data, we start by dividing the robot's pose by the map's resolution.
        // As found here, http://answers.ros.org/question/205521/robot-coordinates-in-map/
        // Calculation: originX / resolution

        // The pixel calculated will be based upon the original dimensions of the map.
        // To calculate the pixel based upon the current canvas dimensions, divide the robot pose (as previously calculated) by the ratio the map was scaled.
        // Calculation: (originX / xScaleFactor) / resolution
        // You can get the ratio by dividing the original map width/height by the respective new canvas width/height.
        const xScaleFactor = canvas.width / 500;
        const yScaleFactor = canvas.height / 500;

        // Finally, incorporate the (0, 0) shift (this aligns the starting points of the canvas and image)
        const xOriginCell = (originX / xScaleFactor) / resolution;
        const yOriginCell = yZeroShift - ((originY / yScaleFactor) / resolution);

        // Add bot image to canvas
        stage.addChild(botBitmap).set({x: xOriginCell, y: yOriginCell});
        stage.update();

        // Now that the bot is rendered at the appropriate starting point, lets start monitoring pose changes

        // Subscribe to robot pose topic
        robotPose.subscribe(pose => {
          // Remove current bot image (this prevents image trailing)
          stage.removeChild(botBitmap);

          // Target the robot's pose
          const poseX = pose.position.x;
          const poseY = pose.position.y;

          // Use three.js to convert orientation Quaternion value to Euler value.
          const quaternion = new THREE.Quaternion(
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w
          );
          const rotation = new THREE.Euler().setFromQuaternion( quaternion, "XYZ" );

          // Rotate bot image
          // Bit map rotation is measured in degrees, but our Euler value is in radians so be sure to convert BEFORE setting rotation
          botBitmap.rotation = radians_to_degrees(rotation.z);

          // Calculate the new position
          // Using the same equation as above, we can simply add the robot's pose to the original pose position, 
          // before dividing by map-canvas-ratio, to get the new cell.
          const xCell = ((originX + poseX) / xScaleFactor) / resolution;
          const yCell = yZeroShift - (((originY + poseY) / yScaleFactor) / resolution);

          // Add bot image back to canvas in new location and update the stage
          stage.addChild(botBitmap).set({x: xCell, y: yCell});
          stage.update();
        });
      };
    };

    function radians_to_degrees(radians) {
      const pi = Math.PI;
      return radians * (180/pi);
    };

   // Handle Canvas Click Events (for clicks outside of the robot images)
   canvas.addEventListener('click', (e) => onClick(e, xCell, yCell), false);
   function onClick(event, x, y) {
     console.log(getCursorPosition(event));
   };

    // Function for getting cursor position from canvas click
    function getCursorPosition(event) {
       const x;
       const y;
       if(event.pageX != undefined && event.pageY != undefined) {
         x = event.pageX;
         y = event.pageY;
       } else {
         x = event.clientX + document.body.scrollLeft + document.documentElement.scrollLeft
         y = event.clientY + document.body.scrollTop + document.documentElement.scrollTop
       };
       x -= canvas.offsetLeft;
       y -= canvas.offsetTop;
       return [x, y];
     };
  };
</script>

` Using this same logic, you can also render multiple bots so long as you have a way of differentiating between them. I use Firebase's Real-Time DB to have the pose update in real-time and the application monitor the changes to a specific fleet's poses. I use this because it's called asynchronously so you don't have to worry about DOM latency.

Hope this helps someone.
Cheers 🥂

[5730289021-NN]

Hi @ngunhaSO - This is great, many thanks. Really good jump off point. Pointer sometimes throws a rotation wobble - I shall investigate.

Thanks again

Mark

To fix that rotation equation, replace

var quaZ = pose.pose.pose.orientation.z;
            var degreeZ = 0;
            if( quaZ >= 0 ) {
                degreeZ = quaZ / 1 * 180
            } else {
                degreeZ = (-quaZ) / 1 * 180 + 180
            };
            robotMarker.rotation = -degreeZ + 35;

with

robotMarker.rotation = new THREE.Euler().setFromQuaternion(new THREE.Quaternion(
            pose.pose.pose.orientation.x,
            pose.pose.pose.orientation.y,
            pose.pose.pose.orientation.z,
            pose.pose.pose.orientation.w
            )
        ).z * -180 / 3.14159;

don't forget to import THREE which can be imported by
<script type="text/javascript" src="http://cdnjs.cloudflare.com/ajax/libs/three.js/r71/three.min.js"></script>

[Rezenders]

Finally got it using the NAV2D to work. User error as per the norm.

For any future users, make sure you are running the robot_pose_publisher as mentioned in the nav2djs documentation. The robot_pose_publisher needs to be running in order for this widget to work.

Is it possible to use tf instead of this node? robot_pose_publisher is not available in ros melodic

[Rezenders]

Modifying @ngunhaSO solution and using @5730289021-NN rotation fix I was able to show the marker using tf.

    var viewer = new ROS2D.Viewer({
      divID : 'map',
      width : document.getElementById("map").clientWidth - 16,
      height : 300
    });

    // Setup the map client.
    var gridClient = new ROS2D.OccupancyGridClient({
      ros : ros,
      rootObject : viewer.scene,
      // Use this property in case of continuous updates
      continuous: true
    });
    //
    // Scale the canvas to fit to the map
    gridClient.on('change', function() {
      viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
      viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
    });

    var robotMarker = new ROS2D.NavigationArrow({
        size : 0.25,
        strokeSize : 0.05,
        pulse: true,
        fillColor: createjs.Graphics.getRGB(255, 0, 0, 0.65)
    });

    gridClient.rootObject.addChild(robotMarker);

    var tfClient = new ROSLIB.TFClient({
       ros : ros,
       fixedFrame : 'map',
       angularThres : 0.01,
       transThres : 0.01
    });

    function tf_sub_func(tf) {
      console.log(tf);
      robotMarker.x = tf.translation.x;
      robotMarker.y = -tf.translation.y;
      robotMarker.rotation = new THREE.Euler().setFromQuaternion(new THREE.Quaternion(
            tf.rotation.x,
            tf.rotation.y,
            tf.rotation.z,
            tf.rotation.w
            )
        ).z * -180 / 3.14159;
    }

    tfClient.subscribe('base_footprint', tf_sub_func);

However, I would still like to know if there is a more elegant way of doing this.

[denesh-globotix]

This could be an alternative. This uses the following javascript class . Hope that helps! Do remember to run the robot pose publisher package on your robot in order to get the /robot_pose topic

   var viewer = new ROS2D.Viewer({
      divID : 'map',
      width : document.getElementById("map").clientWidth - 16,
      height : 300
    });

    // Setup the map client.
    var gridClient = new ROS2D.OccupancyGridClient({
      ros : ros,
      rootObject : viewer.scene,
      // Use this property in case of continuous updates
      continuous: true
    });
    //
    // Scale the canvas to fit to the map
    gridClient.on('change', function() {
      viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
      viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
    });

    var robotMarker = new ROS2D.NavigationArrow({
        size : 0.25,
        strokeSize : 0.05,
        pulse: true,
        fillColor: createjs.Graphics.getRGB(255, 0, 0, 0.65)
    });

    gridClient.rootObject.addChild(robotMarker);
   
    // Add robot pose and trace
    var robotPosition = new NAVIGATION2D.PoseAndTrace({
        ros : ros,
        rootObject : viewer,
        poseTopic : '/robot_pose', //do make sure the robot_pose_publisher is publishing the robot pose
        });
        
        robotPosition.initScale(); 
    
    ```