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;e0&&e.pressureFn(t,i,i.identifier),e.processOnMove(t)};if((i=e.idles.indexOf(a))>=0&&e.idles.splice(i,1),e.actives.push(a),e.ids.push(a.identifier),"semi"!==o.mode)p(a);else{if(!(n(d,a.position)<=o.catchDistance))return a.destroy(),void e.processOnStart(t);p(a)}return a},k.prototype.getOrCreate=function(t,i){var e,o=this.options;return/(semi|static)/.test(o.mode)?(e=this.idles[0])?(this.idles.splice(0,1),e):"semi"===o.mode?this.createNipple(i,t):(console.warn("Coudln't find the needed nipple."),!1):e=this.createNipple(i,t)},k.prototype.processOnMove=function(t){var i=this.options,e=this.manager.getIdentifier(t),o=this.nipples.get(e);if(function(t){return isNaN(t.buttons)?0!==t.pressure:0!==t.buttons}(t)){if(!o)return console.error("Found zombie joystick with ID "+e),void this.manager.removeIdentifier(e);if(i.dynamicPage){var d=c();p=o.el.getBoundingClientRect(),o.position={x:d.x+p.left,y:d.y+p.top}}o.identifier=e;var a=o.options.size/2,p={x:t.pageX,y:t.pageY};i.lockX&&(p.y=o.position.y),i.lockY&&(p.x=o.position.x);var l,u,f,y,m,v,g,b,x=n(p,o.position),O=(l=p,u=o.position,f=u.x-l.x,y=u.y-l.y,r(Math.atan2(y,f))),_=s(O),w=x/a,T={distance:x,position:p};"circle"===o.options.shape?(x=Math.min(x,a),m=o.position,v=x,b={x:0,y:0},g=s(g=O),b.x=m.x-v*Math.cos(g),b.y=m.y-v*Math.sin(g),p=b):(p=function(t,i,e){return{x:Math.min(Math.max(t.x,i.x-e),i.x+e),y:Math.min(Math.max(t.y,i.y-e),i.y+e)}}(p,o.position,a),x=n(p,o.position));var k=p.x-o.position.x,P=p.y-o.position.y;o.frontPosition={x:k,y:P},i.dataOnly||h(o.ui.front,o.frontPosition);var E={identifier:o.identifier,position:p,force:w,pressure:t.force||t.pressure||t.webkitForce||0,distance:x,angle:{radian:_,degree:O},vector:{x:k/a,y:-P/a},raw:T,instance:o,lockX:i.lockX,lockY:i.lockY};(E=o.computeDirection(E)).angle={radian:s(180-O),degree:180-O},o.trigger("move",E),this.trigger("move "+o.id+":move",E)}else this.processOnEnd(t)},k.prototype.processOnEnd=function(t){var i=this,e=i.options,o=i.manager.getIdentifier(t),n=i.nipples.get(o),s=i.manager.removeIdentifier(n.identifier);n&&(e.dataOnly||n.hide(function(){"dynamic"===e.mode&&(n.trigger("removed",n),i.trigger("removed "+n.id+":removed",n),i.manager.trigger("removed "+n.id+":removed",n),n.destroy())}),clearInterval(i.pressureIntervals[n.identifier]),n.resetDirection(),n.trigger("end",n),i.trigger("end "+n.id+":end",n),i.ids.indexOf(n.identifier)>=0&&i.ids.splice(i.ids.indexOf(n.identifier),1),i.actives.indexOf(n)>=0&&i.actives.splice(i.actives.indexOf(n),1),/(semi|static)/.test(e.mode)?i.idles.push(n):i.nipples.indexOf(n)>=0&&i.nipples.splice(i.nipples.indexOf(n),1),i.manager.unbindDocument(),/(semi|static)/.test(e.mode)&&(i.manager.ids[s.id]=s.identifier))},k.prototype.onDestroyed=function(t,i){this.nipples.indexOf(i)>=0&&this.nipples.splice(this.nipples.indexOf(i),1),this.actives.indexOf(i)>=0&&this.actives.splice(this.actives.indexOf(i),1),this.idles.indexOf(i)>=0&&this.idles.splice(this.idles.indexOf(i),1),this.ids.indexOf(i.identifier)>=0&&this.ids.splice(this.ids.indexOf(i.identifier),1),this.manager.removeIdentifier(i.identifier),this.manager.unbindDocument()},k.prototype.destroy=function(){for(var t in this.unbindEvt(this.options.zone,"start"),this.nipples.forEach(function(t){t.destroy()}),this.pressureIntervals)this.pressureIntervals.hasOwnProperty(t)&&clearInterval(this.pressureIntervals[t]);this.trigger("destroyed",this.nipples),this.manager.unbindDocument(),this.off()};var P=k;function E(t){var i,e=this;return e.ids={},e.index=0,e.collections=[],e.config(t),e.prepareCollections(),d(window,"resize",function(t){clearTimeout(i),i=setTimeout(function(){var t,i=c();e.collections.forEach(function(e){e.forEach(function(e){t=e.el.getBoundingClientRect(),e.position={x:i.x+t.left,y:i.y+t.top}})})},100)}),e.collections}E.prototype=new _,E.constructor=E,E.prototype.prepareCollections=function(){var t=this;t.collections.create=t.create.bind(t),t.collections.on=t.on.bind(t),t.collections.off=t.off.bind(t),t.collections.destroy=t.destroy.bind(t),t.collections.get=function(i){var e;return t.collections.every(function(t){return!(e=t.get(i))}),e}},E.prototype.create=function(t){return this.createCollection(t)},E.prototype.createCollection=function(t){var i=new P(this,t);return this.bindCollection(i),this.collections.push(i),i},E.prototype.bindCollection=function(t){var i,e=this,o=function(t,o){i=t.type+" "+o.id+":"+t.type,e.trigger(i,o)};t.on("destroyed",e.onDestroyed.bind(e)),t.on("shown hidden rested dir plain",o),t.on("dir:up dir:right dir:down dir:left",o),t.on("plain:up plain:right plain:down plain:left",o)},E.prototype.bindDocument=function(){this.binded||(this.bindEvt(document,"move").bindEvt(document,"end"),this.binded=!0)},E.prototype.unbindDocument=function(t){Object.keys(this.ids).length&&!0!==t||(this.unbindEvt(document,"move").unbindEvt(document,"end"),this.binded=!1)},E.prototype.getIdentifier=function(t){var i;return t?void 0===(i=void 0===t.identifier?t.pointerId:t.identifier)&&(i=this.latest||0):i=this.index,void 0===this.ids[i]&&(this.ids[i]=this.index,this.index+=1),this.latest=i,this.ids[i]},E.prototype.removeIdentifier=function(t){var i={};for(var e in this.ids)if(this.ids[e]===t){i.id=e,i.identifier=this.ids[e],delete this.ids[e];break}return i},E.prototype.onmove=function(t){return this.onAny("move",t),!1},E.prototype.onend=function(t){return this.onAny("end",t),!1},E.prototype.oncancel=function(t){return this.onAny("end",t),!1},E.prototype.onAny=function(t,i){var e,o=this,n="processOn"+t.charAt(0).toUpperCase()+t.slice(1);i=p(i);return y(i,function(t){e=o.getIdentifier(t),y(o.collections,function(t,i,e){e.ids.indexOf(i)>=0&&(e[n](t),t._found_=!0)}.bind(null,t,e)),t._found_||o.removeIdentifier(e)}),!1},E.prototype.destroy=function(){this.unbindDocument(!0),this.ids={},this.index=0,this.collections.forEach(function(t){t.destroy()}),this.off()},E.prototype.onDestroyed=function(t,i){if(this.collections.indexOf(i)<0)return!1;this.collections.splice(this.collections.indexOf(i),1)};var I=new E;i.default={create:function(t){return I.create(t)},factory:I}}]).default});
\ No newline at end of file
diff --git a/rosboard/html/js/transports/WebSocketV1Transport.js b/rosboard/html/js/transports/WebSocketV1Transport.js
index 58046980..3948ab21 100644
--- a/rosboard/html/js/transports/WebSocketV1Transport.js
+++ b/rosboard/html/js/transports/WebSocketV1Transport.js
@@ -1,14 +1,17 @@
class WebSocketV1Transport {
- constructor({path, onOpen, onClose, onRosMsg, onTopics, onSystem}) {
+ constructor({path, onOpen, onClose, onRosMsg, onTopics, onSystem, onPrefixedCard}) {
this.path = path;
this.onOpen = onOpen ? onOpen.bind(this) : null;
this.onClose = onClose ? onClose.bind(this) : null;
this.onMsg = onMsg ? onMsg.bind(this) : null;
this.onTopics = onTopics ? onTopics.bind(this) : null;
this.onSystem = onSystem ? onSystem.bind(this) : null;
+ this.onPrefixedCard = onPrefixedCard ? onPrefixedCard.bind(this) : null;
this.ws = null;
this.joystickX = 0.0;
this.joystickY = 0.0;
+ this.mode = 0;
+ this.bodyHeight = 0.0;
}
connect() {
@@ -19,9 +22,16 @@ class WebSocketV1Transport {
this.ws = new WebSocket(abspath);
+ // this.ws.onopen = function(){
+ // console.log("connected");
+ // // console.log("that", that.onPrefixedCard(that))
+ // // if(that.onPrefixedCard) that.onPrefixedCard(that);
+ // if(that.onOpen) that.onOpen(that);
+ // }
+
this.ws.onopen = function(){
console.log("connected");
- if(that.onOpen) that.onOpen(that);
+ if(that.onPrefixedCard) that.onPrefixedCard(that);
}
this.ws.onclose = function(){
@@ -51,11 +61,14 @@ class WebSocketV1Transport {
else if(wsMsgType === WebSocketV1Transport.MSG_MSG && that.onMsg) that.onMsg(data[1]);
else if(wsMsgType === WebSocketV1Transport.MSG_TOPICS && that.onTopics) that.onTopics(data[1]);
else if(wsMsgType === WebSocketV1Transport.MSG_SYSTEM && that.onSystem) that.onSystem(data[1]);
+
else console.log("received unknown message: " + wsmsg.data);
- this.send(JSON.stringify([WebSocketV1Transport.JOY_MSG, {
- ["x"]: that.joystickX.toFixed(3),
- ["y"]: that.joystickY.toFixed(3),}]));
+ // this.send(JSON.stringify([WebSocketV1Transport.JOY_MSG, {
+ // ["x"]: that.joystickX.toFixed(3),
+ // ["y"]: that.joystickY.toFixed(3),}]));
+
+
}
}
@@ -75,6 +88,14 @@ class WebSocketV1Transport {
this.joystickX = joystickX;
this.joystickY = joystickY;
}
+ update_highcmd({mode, bodyHeight}) {
+ this.mode = mode;
+ this.bodyHeight = bodyHeight;
+ this.ws.send(JSON.stringify([WebSocketV1Transport.HIGHCMD_MSG, {
+ ["mode"]: this.mode,
+ ["bodyHeight"]: this.bodyHeight
+ }]));
+ }
}
WebSocketV1Transport.MSG_PING = "p";
@@ -89,4 +110,6 @@ class WebSocketV1Transport {
WebSocketV1Transport.PONG_SEQ = "s";
WebSocketV1Transport.PONG_TIME = "t";
- WebSocketV1Transport.JOY_MSG = "j";
\ No newline at end of file
+ WebSocketV1Transport.JOY_MSG = "j";
+ WebSocketV1Transport.HIGHCMD_MSG = "h"
+ WebSocketV1Transport.IMG_QUALITY = "i";
\ No newline at end of file
diff --git a/rosboard/html/js/viewers/HighCmdViewer.js b/rosboard/html/js/viewers/HighCmdViewer.js
new file mode 100644
index 00000000..641e3bc4
--- /dev/null
+++ b/rosboard/html/js/viewers/HighCmdViewer.js
@@ -0,0 +1,53 @@
+"use strict";
+
+class HighCmdController extends Viewer {
+ /**
+ * Gets called when Viewer is first initialized.
+ * @override
+ **/
+ onCreate() {
+ this.viewer = $('')
+ .css({'class': 'card-buttons'})
+ .appendTo(this.card.content);
+
+ this.btnStandUpId = "stand-up-button" ;
+ this.btnSitDownId = "sit-down-button" ;
+ this.btnStandUp = $('')
+ .css({ "class": "card-button"})
+ .text("Stand Up")
+ .appendTo(this.viewer);
+ this.btnSitDown = $('')
+ .css({"class": "card-button"})
+ .text("Sit Down")
+ .appendTo(this.viewer);
+
+ this.btnStandUp.on("click",
+ function(event){
+ let mode = 6 // mode 6 = position stand up
+ let bodyHeight = 0.5
+ currentTransport.update_highcmd({mode, bodyHeight});
+ });
+ this.btnSitDown.on("click",
+ function(event){
+ let mode = 5 // mode 5 = position stand down
+ let bodyHeight = 0.0
+ currentTransport.update_highcmd({mode, bodyHeight});
+ });
+ console.log("HighCmd Viewer created");
+ }
+ onData(msg) {
+ this.card.title.text(msg._topic_name);
+ }
+
+}
+
+HighCmdController.friendlyName = "High Cmd Controller";
+
+HighCmdController.supportedTypes = [
+ "unitree_legged_msgs/msg/HighCmd",
+];
+
+
+HighCmdController.maxUpdateRate = 0.5;
+
+Viewer.registerViewer(HighCmdController);
\ No newline at end of file
diff --git a/rosboard/html/js/viewers/HighStateViewer.js b/rosboard/html/js/viewers/HighStateViewer.js
new file mode 100644
index 00000000..068249a8
--- /dev/null
+++ b/rosboard/html/js/viewers/HighStateViewer.js
@@ -0,0 +1,97 @@
+"use strict";
+
+// GenericViewer just displays message fields and values in a table.
+// It can be used on any ROS type.
+
+class HighStateViewer extends Viewer {
+ /**
+ * Gets called when Viewer is first initialized.
+ * @override
+ **/
+ onCreate() {
+ this.viewerNode = $('')
+ .css({'font-size': '11pt'})
+ .appendTo(this.card.content);
+
+ this.viewerNodeFadeTimeout = null;
+
+ this.expandFields = { };
+ this.fieldNodes = { };
+ this.dataTable = $('')
+ .addClass('mdl-data-table')
+ .addClass('mdl-js-data-table')
+ .css({'width': '100%', 'min-height': '30pt', 'table-layout': 'fixed' })
+ .appendTo(this.viewerNode);
+
+ console.log("High State viewer created")
+ super.onCreate();
+ }
+
+ onData(data) {
+ this.card.title.text(data._topic_name);
+ for(let field in data) {
+ // if(field[0] === "_") continue;
+ // if(field === "header") continue;
+ // if(field === "name") continue;
+ if(field != "imu" && field != "bms" && field != "bodyHeight"){ // only show specific field of /high_state messages
+ continue;
+ }
+ if(!this.fieldNodes[field]) {
+ let tr = $('
')
+ .appendTo(this.dataTable);
+ $(' | ')
+ .addClass('mdl-data-table__cell--non-numeric')
+ .text(field)
+ .css({'width': '10%', 'font-weight': 'bold', 'overflow': 'hidden', 'text-overflow': 'ellipsis'})
+ .appendTo(tr);
+ this.fieldNodes[field] = $(' | ')
+ .addClass('mdl-data-table__cell--non-numeric')
+ .addClass('monospace')
+ .css({'overflow': 'hidden', 'text-overflow': 'ellipsis'})
+ .appendTo(tr);
+ let that = this;
+ this.fieldNodes[field].click(() => {that.expandFields[field] = !that.expandFields[field]; });
+ }
+
+ if(data[field].uuid) {
+ this.fieldNodes[field].text(data[field].uuid.map((byte) => ((byte<16) ? "0": "") + (byte & 0xFF).toString(16)).join(''));
+ this.fieldNodes[field].css({"color": "#808080"});
+ continue;
+ }
+
+ if(typeof(data[field])==="boolean") {
+ if(data[field] === true) {
+ this.fieldNodes[field].text("true");
+ this.fieldNodes[field].css({"color": "#80ff80"});
+ } else {
+ this.fieldNodes[field].text("false");
+ this.fieldNodes[field].css({"color": "#ff8080"});
+ }
+ continue;
+ }
+
+ if(data.__comp && data.__comp.includes(field)) {
+ this.fieldNodes[field][0].innerHTML = "(compressed)";
+ continue;
+ }
+
+ if(this.expandFields[field]) {
+ this.fieldNodes[field][0].innerHTML = (
+ JSON.stringify(data[field], null, ' ')
+ .replace(/\n/g, "
")
+ .replace(/ /g, " ")
+ );
+ } else {
+ this.fieldNodes[field][0].innerHTML = JSON.stringify(data[field], null, ' ');
+ }
+ }
+ }
+}
+
+HighStateViewer.friendlyName = "High State message";
+
+HighStateViewer.supportedTypes = [
+ "unitree_legged_msgs/msg/HighState",
+];
+
+Viewer.registerViewer(HighStateViewer);
\ No newline at end of file
diff --git a/rosboard/html/js/viewers/ImageViewer.js b/rosboard/html/js/viewers/ImageViewer.js
index d5b8dfde..6a7a7cd0 100644
--- a/rosboard/html/js/viewers/ImageViewer.js
+++ b/rosboard/html/js/viewers/ImageViewer.js
@@ -55,12 +55,13 @@ class ImageViewer extends Viewer {
onData(msg) {
this.card.title.text(msg._topic_name);
-
+ console.time("image Viewer")
if(msg.__comp) {
this.decodeAndRenderCompressed(msg);
} else {
this.decodeAndRenderUncompressed(msg);
}
+ console.timeEnd("image Viewer")
}
decodeAndRenderCompressed(msg) {
diff --git a/rosboard/html/js/viewers/JoystickController.js b/rosboard/html/js/viewers/JoystickController.js
index a1ac5fb3..b074b9de 100644
--- a/rosboard/html/js/viewers/JoystickController.js
+++ b/rosboard/html/js/viewers/JoystickController.js
@@ -52,6 +52,7 @@ JoystickController.friendlyName = "JoystickController";
JoystickController.supportedTypes = [
"geometry_msgs/msg/Twist",
+ "geometry_msgs/Twist"
];
JoystickController.maxUpdateRate = 10.0;
diff --git a/rosboard/html/js/viewers/StatsViewer.js b/rosboard/html/js/viewers/StatsViewer.js
new file mode 100644
index 00000000..c650dcc5
--- /dev/null
+++ b/rosboard/html/js/viewers/StatsViewer.js
@@ -0,0 +1,109 @@
+"use strict";
+
+// Viewer for /rosout and other logs that can be expressed in
+// rcl_interfaces/msgs/Log format.
+
+class StatsViewer extends Viewer {
+ /**
+ * Gets called when Viewer is first initialized.
+ * @override
+ **/
+ onCreate() {
+ this.card.title.text("StatsViewer");
+
+ // wrapper and wrapper2 are css BS that are necessary to
+ // have something that is 100% width but fixed aspect ratio
+ this.wrapper = $('')
+ .css({
+ "position": "relative",
+ "width": "100%",
+ })
+ .appendTo(this.card.content);
+
+ this.wrapper2 = $('')
+ .css({
+ "width": "100%",
+ "padding-bottom": "80%",
+ "background": "#101010",
+ "position": "relative",
+ "overflow": "hidden",
+ })
+ .appendTo(this.wrapper);
+
+ // actual log container, put it inside wrapper2
+ this.logContainer = $('')
+ .addClass("monospace")
+ .css({
+ "position": "absolute",
+ "width": "100%",
+ "height": "100%",
+ "font-size": "7pt",
+ "line-height": "1.4em",
+ "overflow-y": "hidden",
+ "overflow-x": "hidden",
+ })
+ .appendTo(this.wrapper2);
+
+ // add the first log
+ $('')
+ .text("Logs will appear here.")
+ .appendTo(this.logContainer);
+
+ super.onCreate();
+
+ let that = this;
+ this.logScrollTimeout = setTimeout(() => {
+ that.logContainer[0].scrollTop = that.logContainer[0].scrollHeight;
+ }, 1000);
+ }
+
+ onData(msg) {
+ while(this.logContainer.children().length > 30) {
+ this.logContainer.children()[0].remove();
+ }
+
+ this.card.title.text(msg._topic_name);
+
+ let color = "#c0c0c0"; // default color
+ let level_text = "";
+
+ // set colors based on log level, if defined
+ // 10-20-30-40-50 is ROS2, 1-2-4-8-16 is ROS1
+ if(msg.level === 10 || msg.level === 1) { level_text = "DEBUG"; color = "#00a000"; }
+ if(msg.level === 20 || msg.level === 2) { level_text = "INFO"; color = "#a0a0a0"; }
+ if(msg.level === 30 || msg.level === 4) { level_text = "WARN"; color = "#c0c000"; }
+ if(msg.level === 40 || msg.level === 8) { level_text = "ERROR"; color = "#ff4040"; }
+ if(msg.level === 50 || msg.level === 16) { level_text = "FATAL"; color = "#ff0000"; }
+
+ let text = "";
+ if(level_text !== "") text += "[" + level_text + "] "
+ if(msg.name) text += "[" + msg.name + "] ";
+ text += msg.msg;
+
+ text = text
+ .replace(/&/g, "&")
+ .replace(//g, ">")
+ .replace(/"/g, """)
+ .replace(/'/g, "'")
+ .replace(/\n/g, "
\n")
+ .replace(/(\[[0-9\.]*\])/g, '$1');
+
+ $('')
+ .html(text)
+ .css({ "color": color })
+ .appendTo(this.logContainer);
+
+ this.logContainer[0].scrollTop = this.logContainer[0].scrollHeight;
+ }
+}
+
+StatsViewer.friendlyName = "Log view";
+
+StatsViewer.supportedTypes = [
+ "rcl_interfaces/msg/Log",
+ "rosgraph_msgs/msg/Log",
+ "rosboard_msgs/msg/SystemStats",
+];
+
+Viewer.registerViewer(StatsViewer);
\ No newline at end of file
diff --git a/rosboard/html/js/viewers/meta/Viewer.js b/rosboard/html/js/viewers/meta/Viewer.js
index b0c4db51..8864e37d 100644
--- a/rosboard/html/js/viewers/meta/Viewer.js
+++ b/rosboard/html/js/viewers/meta/Viewer.js
@@ -28,7 +28,7 @@ class Viewer {
card.title = $('').addClass('card-title').text("Waiting for data ...").appendTo(card);
// card content div
- card.content = $('').addClass('card-content').text('').appendTo(card);
+ card.content = $('').addClass('card-content').text('').appendTo(card);
// card pause button
let menuId = 'menu-' + Math.floor(Math.random() * 1e6);
@@ -56,11 +56,14 @@ class Viewer {
for(let i in viewers) {
let item = $('').appendTo(this.card.menu);
let that = this;
- item.click(() => { Viewer.onSwitchViewer(that, viewers[i]); });
+ item.click(() => { Viewer.onSwitchViewer(that, viewers[i]); console.log(viewers[i]); });
}
componentHandler.upgradeAllRegistered();
+ // card pin it button
+ // card.pinitButton
+
// card pause button
card.pauseButton = $('')
.addClass('mdl-button')
diff --git a/rosboard/rosboard.py b/rosboard/rosboard.py
index 4f44aa0d..04a6544c 100755
--- a/rosboard/rosboard.py
+++ b/rosboard/rosboard.py
@@ -18,13 +18,14 @@
from geometry_msgs.msg import Twist
from rosgraph_msgs.msg import Log
+from unitree_legged_msgs.msg import HighCmd
from rosboard.serialization import ros2dict
from rosboard.subscribers.dmesg_subscriber import DMesgSubscriber
from rosboard.subscribers.processes_subscriber import ProcessesSubscriber
from rosboard.subscribers.system_stats_subscriber import SystemStatsSubscriber
from rosboard.subscribers.dummy_subscriber import DummySubscriber
-from rosboard.handlers import ROSBoardSocketHandler, NoCacheStaticFileHandler
+from rosboard.handlers import ROSBoardSocketHandler, NoCacheStaticFileHandler #, PostHandler
class ROSBoardNode(object):
instance = None
@@ -58,7 +59,9 @@ def __init__(self, node_name = "rosboard_node"):
# ros2 docs don't explain why but we need this magic.
self.sub_rosout = rospy.Subscriber("/rosout", Log, lambda x:x)
- self.twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=100)
+ self.twist_pub = rospy.Publisher('/dog/raw_cmd_vel', Twist, queue_size=1)
+
+ self.highcmd_pub = rospy.Publisher('/high_cmd', HighCmd, queue_size=1)
tornado_settings = {
'debug': True,
@@ -97,6 +100,9 @@ def __init__(self, node_name = "rosboard_node"):
# loop to send client joy message to ros topic
threading.Thread(target = self.joy_loop, daemon = True).start()
+ # loop to send command message to ros topic /high_cmd
+ threading.Thread(target = self.highcmd_loop, daemon = True).start()
+
self.lock = threading.Lock()
rospy.loginfo("ROSboard listening on :%d" % self.port)
@@ -135,13 +141,29 @@ def joy_loop(self):
twist = Twist()
while True:
time.sleep(0.1)
- if not isinstance(ROSBoardSocketHandler.joy_msg, dict):
+ if not (ROSBoardSocketHandler.joy_msg): # return False if joymsg is empty
continue
if 'x' in ROSBoardSocketHandler.joy_msg and 'y' in ROSBoardSocketHandler.joy_msg:
twist.linear.x = -float(ROSBoardSocketHandler.joy_msg['y']) * 3.0
twist.angular.z = -float(ROSBoardSocketHandler.joy_msg['x']) * 2.0
self.twist_pub.publish(twist)
+ def highcmd_loop(self):
+ """
+ Sending HighCmd message from client
+ """
+ highcmd = HighCmd()
+ while True:
+ time.sleep(0.1)
+ if not (ROSBoardSocketHandler.highcmd_msg): # return False if highcmd_msg is empty
+ continue
+ else:
+ print("[INFO] WebSocketTransport -> highcmd_msg = ",ROSBoardSocketHandler.highcmd_msg)
+ highcmd.mode = ROSBoardSocketHandler.highcmd_msg["mode"]
+ highcmd.bodyHeight = ROSBoardSocketHandler.highcmd_msg["bodyHeight"]
+ ROSBoardSocketHandler.highcmd_msg = {} # clear highcmd_msg
+ self.highcmd_pub.publish(highcmd)
+
def pingpong_loop(self):
"""
Loop to send pings to all active sockets every 5 seconds.
@@ -236,7 +258,7 @@ def sync_subs(self):
{
"_topic_name": topic_name, # special non-ros topics start with _
"_topic_type": topic_type,
- "_error": "Could not load message type '%s'. Are the .msg files for it source-bashed?" % topic_type,
+ "_error": "Could not load message '{}' of type '{}'. Are the .msg files for it source-bashed?".format(topic_name,topic_type),
},
]
)
diff --git a/rosboard/serialization.py b/rosboard/serialization.py
index 80294e66..fc250ab5 100644
--- a/rosboard/serialization.py
+++ b/rosboard/serialization.py
@@ -38,7 +38,7 @@ def ros2dict(msg):
if (msg.__module__ == "sensor_msgs.msg._Image" or \
msg.__module__ == "sensor_msgs.msg._image") \
and field == "data":
- rosboard.compression.compress_image(msg, output)
+ rosboard.compression.compress_image(msg, output, max_height=800, max_width=800)
continue
# OccupancyGrid: render and compress to jpeg
@@ -63,7 +63,7 @@ def ros2dict(msg):
if (msg.__module__ == "sensor_msgs.msg._PointCloud2" or \
msg.__module__ == "sensor_msgs.msg._point_cloud2") \
and field == "data":
- rosboard.compression.compress_point_cloud2(msg, output)
+ rosboard.compression.compress_point_cloud2(msg, output, sample_size=1000)
continue
value = getattr(msg, field)
diff --git a/setup.py b/setup.py
index a2ca5d77..12c88a5c 100644
--- a/setup.py
+++ b/setup.py
@@ -29,8 +29,8 @@
]
},
#zip_safe=True,
- maintainer='dheera',
- maintainer_email='dheera.r.e.m.o.v.e.t.h.i.s@dheera.net',
+ maintainer=['jiaqiang', 'minh'],
+ maintainer_email=['jizhan@utu.fi', 'mhnguy@utu.fi'],
description='ROS node that turns your robot into a web server to visualize ROS topics',
license='BSD',
tests_require=['pytest'],