Source: org/terraswarm/accessor/accessors/web/robotics/LocationRosPublisher.js

  1. // Copyright (c) 2015-2016 The Regents of the University of California.
  2. // All rights reserved.
  3. //
  4. // Permission is hereby granted, without written agreement and without
  5. // license or royalty fees, to use, copy, modify, and distribute this
  6. // software and its documentation for any purpose, provided that the above
  7. // copyright notice and the following two paragraphs appear in all copies
  8. // of this software.
  9. //
  10. // IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY
  11. // FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
  12. // ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF
  13. // THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
  14. // SUCH DAMAGE.
  15. //
  16. // THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
  17. // INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  18. // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
  19. // PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
  20. // CALIFORNIA HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,
  21. // ENHANCEMENTS, OR MODIFICATIONS.
  22. //
  24. /** Publish location X,Y,Z coordinates as a PointCloud to a ROS topic.
  25.  *
  26.  *  Incoming packets should have at least these keys:
  27.  *     {
  28.  *       X: <x coordinate>,
  29.  *       Y: <y coordinate>,
  30.  *       id: <unique id for this location>
  31.  *     }
  32.  *
  33.  *  All locations are published as a PointCloud so that the ROS visualization
  34.  *  only needs to subscribe to one topic.
  35.  *
  36.  *  @accessor robotics/LocationRosPublisher
  37.  *  @parameter {string} topic The ROS topic to publish to.
  38.  *  @parameter {string} frame_id The frame_id of the header (only needed if a header is required).
  39.  *  @author Brad Campbell
  40.  *  @version $$Id$$
  41.  */
  43. // Stop extra messages from jslint and jshint.  Note that there should
  44. // be no space between the / and the * and global. See
  45. // https://chess.eecs.berkeley.edu/ptexternal/wiki/Main/JSHint */
  46. /*globals exports, extend, get, getParameter, parameter  */
  47. /*jshint globalstrict: true*/
  48. 'use strict';
  50. var ROS_TYPE = 'sensor_msgs/PointCloud';
  52. // location_id -> {position: {X, Y, Z}, color: <some color as a float>}
  53. var locations = {};
  55. /** Sets up by accessor by inheriting inputs from setup() in
  56.  * WebSocketClient. Adds additional parameters regarding the ROS topic
  57.  * to publish to.
  58.  */
  59. exports.setup = function () {
  60.     this.extend('net/WebSocketClient');
  61.     this.parameter('topic', {
  62.         type: "string"
  63.     });
  64.     this.parameter('frame_id', {
  65.         type: "string",
  66.         value: ""
  67.     });
  68. };
  70. /**  Inherits initialize from WebSocketClient.
  71.  *   Advertise the topic we are publishing to.
  72.  */
  73. exports.initialize = function () {
  74.     this.exports.ssuper.initialize.call(this);
  75. };
  77. /** Override onOpen from WebSocketClient */
  78. exports.onOpen = function () {
  79.     this.exports.ssuper.onOpen.call(this);
  81.     // Advertise what we have when the websocket opens.
  82.     var advertise = {
  83.         "op": "advertise",
  84.         "topic": this.getParameter('topic'),
  85.         "type": ROS_TYPE
  86.     };
  87.     this.exports.sendToWebSocket.call(this, advertise);
  88. };
  90. function random_color() {
  91.     var letters = '0123456789ABCDEF'.split('');
  92.     var color = '0x';
  93.     for (var i = 0; i < 6; i += 1) {
  94.         color += letters[Math.floor(Math.random() * 16)];
  95.     }
  96.     return parseInt(color, 16);
  97. }
  99. /** Override inputHandler on 'toSend' from WebSocketClient. */
  100. exports.toSendInputHandler = function () {
  101.     var msg = this.get('toSend');
  103.     // Update the current location map with this incoming packet
  104.     var id = msg.id;
  105.     var x = msg.X || 0;
  106.     var y = msg.Y || 0;
  107.     var z = msg.Z || 0;
  109.     // Check if this ID already has a color
  110.     var color = 0.0;
  111.     if (id in locations) {
  112.         color = locations[id].color;
  113.     } else {
  114.         color = random_color();
  115.     }
  117.     // Actually update the record
  118.     locations[id] = {
  119.         position: {
  120.             x: x,
  121.             y: y,
  122.             z: z
  123.         },
  124.         color: color
  125.     };
  127.     // Create arrays we can publish
  128.     var location_points = [];
  129.     var colors = [];
  130.     Object.keys(locations).forEach(function (key) {
  131.         location_points.push(locations[key].position);
  132.         colors.push(locations[key].color);
  133.     });
  135.     var out = {
  136.         header: {
  137.             frame_id: this.getParameter('frame_id')
  138.         },
  139.         points: location_points,
  140.         channels: [{
  141.             name: 'rgb',
  142.             values: colors
  143.         }]
  144.     };
  146.     var data = {
  147.         "op": "publish",
  148.         "topic": this.getParameter('topic'),
  149.         "msg": out
  150.     };
  152.     exports.sendToWebSocket(data);
  153. };
  155. /** Unadvertise the topic and inherit wrapup from WebSocketClient. */
  156. exports.wrapup = function () {
  157.     if (this.exports.isOpen()) {
  158.         var unadvertise = {
  159.             "op": "unadvertise",
  160.             "topic": this.getParameter('topic')
  161.         };
  162.         this.exports.sendToWebSocket.call(this, unadvertise);
  163.     }
  164.     this.exports.ssuper.wrapup.call(this);
  165. };