diff --git a/.gitignore b/.gitignore index bee8a64b..0c232bb0 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ __pycache__ +.vscode/* diff --git a/README.md b/README.md index d1b4f159..4d8ddb88 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,14 @@ # ROSboard +## Customize +- Custom image encoding +Choose the encoding image dimension within the `compress_image()` and the crispy image within the `encode_jpeg()` function + +- Custom point cloud subsample size +Choose the number of sub-samples every scan within the `compress_point_cloud2()` function. + +-------- + ROS node that runs a web server on your robot. Run the node, point your web browser at http://your-robot-ip:8888/ and you get nice visualizations. diff --git a/rosboard/__init__.py b/rosboard/__init__.py index 42cf7cd5..24ee297f 100644 --- a/rosboard/__init__.py +++ b/rosboard/__init__.py @@ -1 +1,3 @@ -__version__ = "1.2.1" \ No newline at end of file +__version__ = "1.2.1" + +__img_quality__ = 50 \ No newline at end of file diff --git a/rosboard/compression.py b/rosboard/compression.py index 3f365900..6f92f9eb 100644 --- a/rosboard/compression.py +++ b/rosboard/compression.py @@ -2,7 +2,7 @@ import io import numpy as np from rosboard.cv_bridge import imgmsg_to_cv2 - +import time try: import simplejpeg except ImportError: @@ -28,31 +28,33 @@ def decode_jpeg(input_bytes): return np.asarray(Image.open(io.BytesIO(input_bytes))) def encode_jpeg(img): + # use img_quality to change image quality between 0 and 100 + __img_quality__ = 50 if simplejpeg: if len(img.shape) == 2: img = np.expand_dims(img, axis=2) if not img.flags['C_CONTIGUOUS']: img = img.copy(order='C') - return simplejpeg.encode_jpeg(img, colorspace = "GRAY", quality = 50) + return simplejpeg.encode_jpeg(img, colorspace = "GRAY", quality = img_quality) elif len(img.shape) == 3: if not img.flags['C_CONTIGUOUS']: img = img.copy(order='C') if img.shape[2] == 1: - return simplejpeg.encode_jpeg(img, colorspace = "GRAY", quality = 50) + return simplejpeg.encode_jpeg(img, colorspace = "GRAY", quality = img_quality) elif img.shape[2] == 4: - return simplejpeg.encode_jpeg(img, colorspace = "RGBA", quality = 50) + return simplejpeg.encode_jpeg(img, colorspace = "RGBA", quality = img_quality) elif img.shape[2] == 3: - return simplejpeg.encode_jpeg(img, colorspace = "RGB", quality = 50) + return simplejpeg.encode_jpeg(img, colorspace = "RGB", quality = img_quality) else: return b'' elif cv2: if len(img.shape) == 3 and img.shape[2] == 3: img = img[:,:,::-1] - return cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 50])[1].tobytes() + return cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, img_quality])[1].tobytes() elif PIL: pil_img = Image.fromarray(img) buffered = io.BytesIO() - pil_img.save(buffered, format="JPEG", quality = 50) + pil_img.save(buffered, format="JPEG", quality = img_quality) return buffered.getvalue() _PCL2_DATATYPES_NUMPY_MAP = { @@ -150,9 +152,8 @@ def compress_compressed_image(msg, output): output["_error"] = "Error: %s" % str(e) output["_data_jpeg"] = base64.b64encode(img_jpeg).decode() output["_data_shape"] = list(original_shape) - -def compress_image(msg, output): +def compress_image(msg, output, max_height = 800, max_width = 800): output["data"] = [] output["__comp"] = ["data"] @@ -173,8 +174,9 @@ def compress_image(msg, output): cv2_img = np.stack((cv2_img[:,:,0], cv2_img[:,:,1], np.zeros(cv2_img[:,:,0].shape)), axis = -1) # enforce <800px max dimension, and do a stride-based resize - if cv2_img.shape[0] > 800 or cv2_img.shape[1] > 800: - stride = int(np.ceil(max(cv2_img.shape[0] / 800.0, cv2_img.shape[1] / 800.0))) + # Edit: choose a smaller dimension for quicker encoding and larger dimension vice versa. + if cv2_img.shape[0] > max_height or cv2_img.shape[1] > max_width: + stride = int(np.ceil(max(cv2_img.shape[0] / max_height*1.0, cv2_img.shape[1] / max_width*1.0))) cv2_img = cv2_img[::stride,::stride] # if image format isn't already uint8, make it uint8 for visualization purposes @@ -193,8 +195,13 @@ def compress_image(msg, output): cv2_img = np.clip(cv2_img * 255, 0, 255).astype(np.uint8) try: + start = time.time() img_jpeg = encode_jpeg(cv2_img) + + end = time.time() + output["_warn"] = "encodejpeg elapsed = " + str(end - start) + "sec" output["_data_jpeg"] = base64.b64encode(img_jpeg).decode() + output["_warn"] += "\nlength of encoded jpeg string = " + str(len(base64.b64encode(img_jpeg).decode())) output["_data_shape"] = original_shape except OSError as e: output["_error"] = str(e) @@ -221,7 +228,10 @@ def compress_occupancy_grid(msg, output): except Exception as e: output["_error"] = str(e) try: + start = time.time() img_jpeg = encode_jpeg(cv2_img) + end = time.time() + output["_warn"] = (end - start) output["_data_jpeg"] = base64.b64encode(img_jpeg).decode() except OSError as e: output["_error"] = str(e) @@ -237,7 +247,8 @@ def compress_occupancy_grid(msg, output): 8: np.float64, } -def compress_point_cloud2(msg, output): +def compress_point_cloud2(msg, output, sample_size = 65535): + # sample_size (int): custom sample size, default 65535 # assuming fields are ('x', 'y', 'z', ...), # compression scheme is: # msg['_data_uint16'] = { @@ -249,6 +260,13 @@ def compress_point_cloud2(msg, output): # 65535 represents the max value in the dataset, and bounds: [...] holds information on those bounds so the # client can decode back to a float + ### EDIT: CUSTOM COMPRESSION + # choose point cloud sample size + + if (sample_size < 0): + output["_warn"] = "sample size < 0, set to default 65535" + sample_size = 65535 + output["data"] = [] output["__comp"] = ["data"] @@ -268,9 +286,9 @@ def compress_point_cloud2(msg, output): except AssertionError as e: output["_error"] = "PointCloud2 error: %s" % str(e) - if points.size > 65536: - output["_warn"] = "Point cloud too large, randomly subsampling to 65536 points." - idx = np.random.randint(points.size, size=65536) + if points.size > sample_size: + output["_warn"] = "Point cloud too large, randomly subsampling to {} points.".format(sample_size) + idx = np.random.randint(points.size, size=sample_size) points = points[idx] xpoints = points['x'].astype(np.float32) diff --git a/rosboard/handlers.py b/rosboard/handlers.py index e919453e..14c95ce3 100644 --- a/rosboard/handlers.py +++ b/rosboard/handlers.py @@ -8,7 +8,15 @@ import types import uuid -from . import __version__ +from . import __version__, __img_quality__ + +# class PostHandler(tornado.web.RequestHandler): +# def post(self): +# data = json.loads(self.request.body) +# self.write("Set image quality to: ", data) + +# def set_img_quality(img_quality): +# __img_quality__ = img_quality class NoCacheStaticFileHandler(tornado.web.StaticFileHandler): def set_extra_headers(self, path): @@ -18,6 +26,8 @@ def set_extra_headers(self, path): class ROSBoardSocketHandler(tornado.websocket.WebSocketHandler): sockets = set() joy_msg = None + highcmd_msg = None + img_quality_msg = 50 def initialize(self, node): # store the instance of the ROS node that created this WebSocketHandler so we can access it later @@ -192,6 +202,13 @@ def on_message(self, message): # Joy elif argv[0] == ROSBoardSocketHandler.JOY_MSG: ROSBoardSocketHandler.joy_msg = argv[1] + # HighCmd + elif argv[0] == ROSBoardSocketHandler.HIGHCMD_MSG: + ROSBoardSocketHandler.highcmd_msg = argv[1] + # image quality + elif argv[0] == ROSBoardSocketHandler.IMG_QUALITY: + # ROSBoardSocketHandler.img_quality_msg = argv[1] + __img_quality__ = argv[1] ROSBoardSocketHandler.MSG_PING = "p"; @@ -207,3 +224,5 @@ def on_message(self, message): ROSBoardSocketHandler.PONG_TIME = "t"; ROSBoardSocketHandler.JOY_MSG = "j"; +ROSBoardSocketHandler.HIGHCMD_MSG = "h"; +ROSBoardSocketHandler.IMG_QUALITY = "i"; \ No newline at end of file diff --git a/rosboard/html/css/index.css b/rosboard/html/css/index.css index c1bd474e..810123ba 100644 --- a/rosboard/html/css/index.css +++ b/rosboard/html/css/index.css @@ -111,17 +111,47 @@ input, textarea, *[contenteditable=true] { background:#202020; } +.float-button { + position: fixed; + bottom: 90px; + right: 50px; +} + +/* .grid::after { + content: ''; + display: block; + clear: both; +} */ + +.grid-sizer, +.card { + width: 20%; +} + .card { border-radius:5pt; - overflow:hidden; - margin-left:20pt; + overflow:scroll; + margin-left:5pt; margin-top:20pt; background:#303030; color:#c0c0c0; box-shadow:3pt 3pt 3pt rgba(0,0,0,0.2); - width:calc(100% - 40pt); + /* width:100%; */ + /* width:calc(100% - 10pt); */ } +.card--width-pc { width:calc(60% - 10pt); } +.card--width-odom { width:calc(40% - 10pt); } +.card--width-img { width:calc(20% - 10pt); } +.card--width-state { width:calc(25% - 10pt); } +.card--width-highcmd { width:200px; } + +.card--height-pc { height:500px; } +.card--height-odom { height:200px; } +.card--height-img { height:200px; } +.card--height-state { height: 600px; scroll-behavior: smooth;} +.card--height-highcmd { height: 100px;} + .card-title { font-family:Titillium Web; font-size:12pt; @@ -153,6 +183,7 @@ input, textarea, *[contenteditable=true] { .card-buttons { position:absolute; + /* position:auto; */ display:flex; top:0; right:5pt; @@ -176,6 +207,27 @@ input, textarea, *[contenteditable=true] { cursor:pointer; } +#stand-up-button{ + background-color: white; + color: black; + border: 2px solid #4CAF50; +} + +#stand-up-button:hover{ + background-color: #4CAF50; + color: white; +} + +#sit-down-button{ + background-color: white; + color: black; + border: 2px solid #555555; +} + +#sit-down-button:hover{ + background-color: #555555; + color: white; +} .card-content { -webkit-transition: opacity 0.3s ease; -moz-transition: opacity 0.3s ease; @@ -187,21 +239,21 @@ input, textarea, *[contenteditable=true] { @media screen and (min-width: 900px) { .card { display:inline-block; - width:calc(50% - 40pt); + /* width:calc(50% - 40pt); */ } } @media screen and (min-width: 1200px) { .card { display:inline-block; - width:calc(33% - 40pt); + /* width:calc(33% - 40pt); */ } } @media screen and (min-width: 1800px) { .card { display:inline-block; - width:calc(25% - 40pt); + /* width:calc(25% - 40pt); */ } } @@ -331,7 +383,7 @@ input, textarea, *[contenteditable=true] { /* width */ ::-webkit-scrollbar { - width: 10px; + width: 20px; } /* Track */ diff --git a/rosboard/html/index.html b/rosboard/html/index.html index 47d3f071..92c16c28 100644 --- a/rosboard/html/index.html +++ b/rosboard/html/index.html @@ -9,7 +9,7 @@ - + @@ -46,7 +46,15 @@
+
+
+ +
diff --git a/rosboard/html/js/index.js b/rosboard/html/js/index.js index 87722115..10768733 100644 --- a/rosboard/html/js/index.js +++ b/rosboard/html/js/index.js @@ -15,6 +15,9 @@ importJsOnce("js/viewers/DiagnosticViewer.js"); importJsOnce("js/viewers/TimeSeriesPlotViewer.js"); importJsOnce("js/viewers/PointCloud2Viewer.js"); importJsOnce("js/viewers/JoystickController.js"); +importJsOnce("js/viewers/StatsViewer.js") +importJsOnce("js/viewers/HighStateViewer.js") +importJsOnce("js/viewers/HighCmdViewer.js") // GenericViewer must be last importJsOnce("js/viewers/GenericViewer.js"); @@ -44,7 +47,8 @@ let $grid = null; $(() => { $grid = $('.grid').masonry({ itemSelector: '.card', - gutter: 10, + gutter: 0, + columnWidth: '.grid-sizer', percentPosition: true, }); $grid.masonry("layout"); @@ -57,6 +61,8 @@ setInterval(() => { } }, 5000); + + function updateStoredSubscriptions() { if(window.localStorage) { let storedSubscriptions = {}; @@ -69,13 +75,68 @@ function updateStoredSubscriptions() { } } -function newCard() { - // creates a new card, adds it to the grid, and returns it. - let card = $("
").addClass('card') - .appendTo($('.grid')); +function newCard({topicName, topicType}) { + var card = null; //document.createElement('div'); + // creates a new card, adds it to the grid, and returns it. style='width: 40%' + // let card = $("
").addClass('card') + // .appendTo($('.grid')); + console.log("topicType", topicType) + if(topicType == "sensor_msgs/PointCloud2"){ + // card.className = 'card-pc' + card = $("
")//.appendTo($('.grid'));//.addClass('card-pc') + // .appendTo($('.grid')); + console.log("pc card", card) + } + else if(topicType == "nav_msgs/msg/Odometry"){ + card = $("
")//.appendTo($('.grid'));//.addClass('card-odom') + // .appendTo($('.grid')); card-odom--width + console.log("odom card", card) + } + else if(topicType == "sensor_msgs/Image"){ + card = $("
")//.appendTo($('.grid'));//.addClass('card-img') + // .appendTo($('.grid')); card-img--width + console.log("img card", card) + } + else if(topicType == "unitree_legged_msgs/HighState"){ + card = $("
") + console.log("state card", card) + } + else if(topicType == "unitree_legged_msgs/HighCmd"){ + card = $("
") + console.log("state card", card) + } + else{ + card = $("
")//.appendTo($('.grid'));//.addClass('card-pc') + // .appendTo($('.grid')); card-pc--width + console.log("else card", card) + } + card = card.appendTo($('.grid')) + return card; } +// add prefixed card +let onPrefixedCard = function() { + let preSubscriptions = { + // "/camera/depth/points": { topicType: "sensor_msgs/PointCloud2" }, + // "/odom": { topicType: "nav_msgs/msg/Odometry" }, + // "/camera/rgb/image_raw": { topicType: "sensor_msgs/Image" }, + // "/camera/rgb/image_raw1": { topicType: "sensor_msgs/Image" }, + // "/camera/rgb/image_raw2": { topicType: "sensor_msgs/Image" }, + // "/camera/rgb/image_raw3": { topicType: "sensor_msgs/Image" }, + // "/camera/rgb/image_raw4": { topicType: "sensor_msgs/Image" }, + // "/camera/rgb/image_raw5": { topicType: "sensor_msgs/Image" }, + "/high_state": { topicType: "unitree_legged_msgs/HighState"}, + "/high_cmd": { topicType: "unitree_legged_msgs/HighCmd"}, + }; + // console.log("preSubscriptions", preSubscriptions); + + for(let topic_name in preSubscriptions){ + // console.log("topic_name", topic_name) + initSubscribe({topicName: topic_name, topicType: preSubscriptions[topic_name].topicType}); + } +} + let onOpen = function() { for(let topic_name in subscriptions) { console.log("Re-subscribing to " + topic_name); @@ -199,7 +260,8 @@ function initSubscribe({topicName, topicType}) { } currentTransport.subscribe({topicName: topicName}); if(!subscriptions[topicName].viewer) { - let card = newCard(); + console.log('initSubscribe topicType', topicType) + let card = newCard({topicName, topicType}); let viewer = Viewer.getDefaultViewerForType(topicType); try { subscriptions[topicName].viewer = new viewer(card, topicName, topicType); @@ -222,6 +284,7 @@ function initDefaultTransport() { onMsg: onMsg, onTopics: onTopics, onSystem: onSystem, + onPrefixedCard: onPrefixedCard, }); currentTransport.connect(); } diff --git a/rosboard/html/js/nipplejs.js b/rosboard/html/js/nipplejs.js new file mode 100644 index 00000000..5e40ed7b --- /dev/null +++ b/rosboard/html/js/nipplejs.js @@ -0,0 +1 @@ +!function(t,i){"object"==typeof exports&&"object"==typeof module?module.exports=i():"function"==typeof define&&define.amd?define("nipplejs",[],i):"object"==typeof exports?exports.nipplejs=i():t.nipplejs=i()}(window,function(){return function(t){var i={};function e(o){if(i[o])return i[o].exports;var n=i[o]={i:o,l:!1,exports:{}};return t[o].call(n.exports,n,n.exports,e),n.l=!0,n.exports}return e.m=t,e.c=i,e.d=function(t,i,o){e.o(t,i)||Object.defineProperty(t,i,{enumerable:!0,get:o})},e.r=function(t){"undefined"!=typeof Symbol&&Symbol.toStringTag&&Object.defineProperty(t,Symbol.toStringTag,{value:"Module"}),Object.defineProperty(t,"__esModule",{value:!0})},e.t=function(t,i){if(1&i&&(t=e(t)),8&i)return t;if(4&i&&"object"==typeof t&&t&&t.__esModule)return t;var o=Object.create(null);if(e.r(o),Object.defineProperty(o,"default",{enumerable:!0,value:t}),2&i&&"string"!=typeof t)for(var n in t)e.d(o,n,function(i){return t[i]}.bind(null,n));return o},e.n=function(t){var i=t&&t.__esModule?function(){return t.default}:function(){return t};return e.d(i,"a",i),i},e.o=function(t,i){return Object.prototype.hasOwnProperty.call(t,i)},e.p="",e(e.s=0)}([function(t,i,e){"use strict";e.r(i);var o,n=function(t,i){var e=i.x-t.x,o=i.y-t.y;return Math.sqrt(e*e+o*o)},s=function(t){return t*(Math.PI/180)},r=function(t){return t*(180/Math.PI)},d=function(t,i,e){for(var o,n=i.split(/[ ,]+/g),s=0;s=0&&this._handlers_[t].splice(this._handlers_[t].indexOf(i),1),this},O.prototype.trigger=function(t,i){var e,o=this,n=t.split(/[ ,]+/g);o._handlers_=o._handlers_||{};for(var s=0;ss&&n<3*s&&!t.lockX?i="up":n>-s&&n<=s&&!t.lockY?i="left":n>3*-s&&n<=-s&&!t.lockX?i="down":t.lockY||(i="right"),t.lockY||(e=n>-r&&n0?"up":"down"),t.force>this.options.threshold){var d,a={};for(d in this.direction)this.direction.hasOwnProperty(d)&&(a[d]=this.direction[d]);var p={};for(d in this.direction={x:e,y:o,angle:i},t.direction=this.direction,a)a[d]===this.direction[d]&&(p[d]=!0);if(p.x&&p.y&&p.angle)return t;p.x&&p.y||this.trigger("plain",t),p.x||this.trigger("plain:"+e,t),p.y||this.trigger("plain:"+o,t),p.angle||this.trigger("dir dir:"+i,t)}else this.resetDirection();return t};var T=w;function k(t,i){return this.nipples=[],this.idles=[],this.actives=[],this.ids=[],this.pressureIntervals={},this.manager=t,this.id=k.id,k.id+=1,this.defaults={zone:document.body,multitouch:!1,maxNumberOfNipples:10,mode:"dynamic",position:{top:0,left:0},catchDistance:200,size:100,threshold:.1,color:"white",fadeTime:250,dataOnly:!1,restJoystick:!0,restOpacity:.5,lockX:!1,lockY:!1,shape:"circle",dynamicPage:!1},this.config(i),"static"!==this.options.mode&&"semi"!==this.options.mode||(this.options.multitouch=!1),this.options.multitouch||(this.options.maxNumberOfNipples=1),this.updateBox(),this.prepareNipples(),this.bindings(),this.begin(),this.nipples}k.prototype=new _,k.constructor=k,k.id=0,k.prototype.prepareNipples=function(){var t=this.nipples;t.on=this.on.bind(this),t.off=this.off.bind(this),t.options=this.options,t.destroy=this.destroy.bind(this),t.ids=this.ids,t.id=this.id,t.processOnMove=this.processOnMove.bind(this),t.processOnEnd=this.processOnEnd.bind(this),t.get=function(i){if(void 0===i)return t[0];for(var e=0,o=t.length;e