wpi-arc / datalink_toolkit Goto Github PK

Tools for teleoperation over limited bandwidth datalinks using ROS

CMake 9.52% Python 24.97% C++ 65.51%

datalink_toolkit's Introduction

datalink_toolkit

Tools for using ROS over limited bandwidth datalinks

While this code is freely licensed (2-clause BSD), we do ask that you send us an email so we can see who is using this software.

Repository structure

Unlike earlier Catkinized software we have provided, this repository does not contain a Catkin workspace. As we expect that other teams will be well on their way to migrating to Catkin, the difficulties of managing multiple workspaces do not justify the convenience of distributing these packages in their own workspace. As such, you will need to clone this repository inside the src/ directory of an existing Catkin workspace.

Please note that this software is primarily written for ROS Hydro, since that is what we are using for active development. Since this software was initially developed on ROS Groovy, most parts are (and should remain) backwards-compatible. Due to the use of Catkin, this software is incompatible with ROS Fuerte and earlier.

This repository is structured around 4 core packages:

datalink_launch - Launch files for teleop links using both the nodes in this repository and other packages (namely a custom version of openni2.launch to support divorced TF trees). The launch files in this package provide examples for how to configure the various link components in this repository.
pointcloud_compression - Library of pointcloud compression systems used to compress pointclouds for transmission over low-bandwidth datalinks.
opportunistic_link - This package provides an automatically switched ROS transmit link that allows for a topic to be sent once over the link to multiple subscribers. To reduce data demands, the link stops data flow automatically when no subscribers are connected.
datalink_msgs - Message types for low-bandwidth datalinks, namely to support message aggregation and compression.
datalink_mux_demux - Packages provides nodes for message aggregation/multiplexing and message dis-aggregation/demultiplexing. This allows multiple message topics to be combined together and sent over a single ROS publisher->subscriber link.

Stability and development status

opportunistic_link - Package is stable and used for our DRC team.

datalink_msgs - Package is currently stable with no additional message types planned.

pointcloud_compression - Package is currently stable. Additional compression systems will be added if necessary.

datalink_launch - All launch files should be considered stable. New launch files may be added to provide better examples or provided support for new sensors.

datalink_mux_demux - Package is experimental.

Depencies

Full ROS Hydro installation - on Ubuntu systems: $ sudo apt-get install ros-hydro-desktop-full

Build and usage instructions

First, clone this repository:

$ cd /your/catkin/workspace/src
$ git clone https://github.com/WPI-ARC/datalink_toolkit.git
$ rospack profile

To build all packages in this repository:

(in the surrounding Catkin workspace directory)
$ catkin_make

To build a particular package in the repository:

(in the surrounding Catkin workspace directory)
$ catkin_make --pkg <package name>

To use, you must source the workspace:

(in the surrounding Catkin workspace directory)
$ source devel/setup.bash

For usage information and instructions on running components of these packages together, see the repository Wiki.

datalink_toolkit's People

Contributors

Stargazers

Watchers

Forkers

pal-robotics-forks awesomebytes jacquelinekay calderpg mvieth leroyr

datalink_toolkit's Issues

Exception in pointcloud_link

I've tried all the compression types for the pointcloud link and all of them have an exception after the first message trying to free memory. In addition, the PCL and PC60 seem to have other, unrelated warnings. This is all compiled in my catkin workspace on ROS Indigo with Ubuntu 14.04. Here are a couple examples:

teleop_compression_node.launch

<?xml version="1.0"?>

<launch>
    <node pkg="opportunistic_link" type="request_pointcloud_link_startpoint" name="velodyne_pointcloud_link_startpoint" output="screen" >
        <param name="pointcloud_topic"  type="string"   value="/velodyne_points" />
        <param name="compression_type"  type="string"   value="PC60" />
        <param name="data_service"      type="string"   value="/velodyne_points_data" />
        <param name="link_topic"        type="string"   value="/velodyn_points_compressed" />
    </node>
</launch>

teleop_decompression_node.launch

<?xml version="1.0"?>

<launch>
    <node pkg="opportunistic_link" type="request_pointcloud_link_endpoint" name="velodyne_pointcloud_link_endpoint" output="screen" >
        <param name="relay_topic"          type="string"   value="/velodyne_points_uncompressed" />
        <param name="data_service"         type="string"   value="/velodyne_points_data" />
        <param name="rate_ctrl"            type="string"   value="/velodyne_points_rate" />
        <param name="link_topic"           type="string"   value="/velodyn_points_compressed" />
        <param name="latched"              type="bool"     value="true" />
        <param name="default_rate"         type="double"   value="5" />
        <param name="default_filter_size"  type="double"   value="0.1" />
        <param name="filter_ctrl"          type="string"   value="/velodyne_points_quality" />
        <param name="override_timestamps"  type="bool"     value="false" />

        <!-- <remap from="/tf" to="/tf" /> -->
    </node>
</launch>

ZLIB:

process[velodyne_pointcloud_link_startpoint-1]: started with pid [19478]
[ INFO] [1461618865.737210108]: Starting request pointcloud link startpoint...
[ INFO] [1461618865.756955066]: Relay using ZLIB compression
[ INFO] [1461618865.756989238]: ...startup complete
[ INFO] [1461618874.008745110]: Compression of 22.731546 % took 0.004967 seconds
[ INFO] [1461618874.008783688]: Original size: 440.256000 KB - Compressed size: 100.077000 KB
[ INFO] [1461618874.008890676]: Responded with a single message
[ WARN] [1461618874.019460789]: Single message requested, none available
*** Error in `/home/zac/mBot/ros/devel/lib/opportunistic_link/request_pointcloud_link_startpoint': double free or corruption (out): 0x00007ffcea4a4900 ***

PCL:

process[velodyne_pointcloud_link_startpoint-1]: started with pid [21847]
[ INFO] [1461619036.722727318]: Starting request pointcloud link startpoint...
[ INFO] [1461619036.735261220]: Relay using PCL compression
[ INFO] [1461619036.735347653]: ...startup complete
Failed to find match for field 'rgb'.
[ INFO] [1461619048.874611953]: Compression of 11.657387 % took 0.012872 seconds
[ INFO] [1461619048.874654928]: Original size: 589.120000 KB - Compressed size: 68.676000 KB
[ INFO] [1461619048.874674814]: Responded with a single message
*** Error in `/home/zac/mBot/ros/devel/lib/opportunistic_link/request_pointcloud_link_startpoint': free(): invalid size: 0x00007ffe047225f0 ***

PC60:

process[velodyne_pointcloud_link_startpoint-1]: started with pid [22588]
[ INFO] [1461619086.745956287]: Starting request pointcloud link startpoint...
[ INFO] [1461619086.761933569]: Relay using PC60 compression
[ INFO] [1461619086.761975220]: ...startup complete
PFRAME_COUNTER_: 0 IFRAME_RATE_: 5
[ INFO] [1461619092.588073608]: Start IFRAME encode
[ INFO] [1461619092.590120927]: Compression of 10.495540 % took 0.005677 seconds
[ INFO] [1461619092.590149204]: Original size: 463.616000 KB - Compressed size: 48.659000 KB
[ INFO] [1461619092.590162542]: Responded with a single message
[ WARN] [1461619092.600189151]: Single message requested, none available
*** Error in `/home/zac/mBot/ros/devel/lib/opportunistic_link/request_pointcloud_link_startpoint': free(): invalid size: 0x00007ffeb15181d0 ***

Network bandwidth much higher than it should be

I'm experimenting with both the request pointcloud data link and the request camera data link. On the camera data link I'm seeing rqt_topic report the relay topic /usb_cam0/workstation/image/x264 using about 50K/s while the actual network throughput is over 600K/s. Turning off that subscription, the network idles at about 3K/s. When I subscribe directly to the /usb_cam0/image_raw/x264 the network throughput is around 75K/s, which is what is should be.

In a somewhat opposite case, when I subscribe to the pointcloud relay, rqt_topic reports the bandwidth at around 1.2MB/s while the actual network traffic is only around 350K/s, which seems right based on what the request_pointcloud_link_startpoint reports it's compressing the messages to.

Any ideas on what could describe this?

Screenshot of camera bandwidth:

Screenshot of pointcloud bandwidth:

    <node pkg="opportunistic_link" type="request_camera_link_startpoint" name="$(arg node_name)_link_startpoint" output="$(arg output)" >
        <!-- Request node params -->
        <param name="data_service" type="string" value="$(arg image_name)/data" />

        <!-- Limited node params -->
        <param name="link_topic" type="string" value="$(arg image_name)/data_link" />
        <param name="link_ctrl" type="string" value="$(arg image_name)/link_ctrl" />
        <param name="rate_ctrl" type="string" value="$(arg image_name)/rate_ctrl" />

        <!-- Request and Limited params -->
        <!-- <param name="camera_topic" type="string" value="$(arg image_name)/resized/image" /> -->
        <param name="camera_topic" type="string" value="$(arg image_name)/image_raw" />
        <remap from="/tf" to="$(arg remapped_tf)" />
    </node>

    <node pkg="opportunistic_link" type="request_camera_link_endpoint" name="$(arg node_name)_link_endpoint" output="$(arg output)" >
        <!-- Request node params -->
        <param name="override_timestamps" type="bool" value="$(arg override_timestamps)" />
        <param name="default_rate" type="double" value="$(arg default_rate)" />
        <param name="default_quality" type="int" value="$(arg quality)" />
        <param name="quality_ctrl" type="string" value="$(arg image_name)/quality" />
        <param name="rate_ctrl" type="string" value="$(arg image_name)/rate" />
        <param name="data_service" type="string" value="$(arg image_name)/data" />
        <param name="relay_topic" type="string" value="$(arg image_name)/workstation/image" />

        <!-- Limit node params -->
        <param name="relay_base_topic" type="string" value="$(arg image_name)/workstation/image" />
        <param name="link_image_topic" type="string" value="$(arg image_name)/data_link" />
        <param name="link_info_topic" type="string" value="$(arg image_name)/info_link" />
        <param name="link_ctrl" type="string" value="$(arg image_name)/link_ctrl" />

        <!-- Request and Limited params -->
        <param name="latched" type="bool" value="true" />
        <remap from="/tf" to="$(arg remapped_tf)" />
    </node>

    <node pkg="opportunistic_link" type="request_pointcloud_link_startpoint" name="$(arg node_name)_link_startpoint" output="$(arg output)">
        <param name="pointcloud_topic"   type="string"   value="$(arg pointcloud_name)" />
        <param name="compression_type"   type="string"   value="$(arg compression_type)" />
        <param name="data_service"       type="string"   value="$(arg pointcloud_name)/data" />
        <param name="link_topic"         type="string"   value="$(arg pointcloud_name)/compressed" />
    </node>

    <node pkg="opportunistic_link" type="request_pointcloud_link_endpoint" name="$(arg node_name)_link_endpoint" output="screen" >
        <param name="relay_topic"          type="string"   value="$(arg pointcloud_name)/workstation" />
        <param name="data_service"         type="string"   value="$(arg pointcloud_name)/data" />
        <param name="rate_ctrl"            type="string"   value="$(arg pointcloud_name)/rate" />
        <param name="link_topic"           type="string"   value="$(arg pointcloud_name)/compressed" />
        <param name="filter_ctrl"          type="string"   value="$(arg pointcloud_name)/quality" />
        <param name="latched"              type="bool"     value="true" />
        <param name="default_rate"         type="double"   value="$(arg default_rate)" />
        <param name="default_filter_size"  type="double"   value="$(arg default_filter_size)" />
        <param name="override_timestamps"  type="bool"     value="false" />

        <remap from="/tf" to="$(arg remapped_tf)" />
    </node>

opportunistic startpoints consuming 100% cpu

Hello, I would like to point out that, for example,

https://github.com/WPI-ARC/datalink_toolkit/blob/master/opportunistic_link/src/request_image_link_startpoint.cpp#L29-L35

The looping is done without any rate on it. This causes 100% CPU usage for every datalink.

I've added:

while (ros::ok())
    {
        ros::spinOnce();
        ros::Duration(0.001).sleep(); // 1000Hz
    }

The sleep line, checking at 1000Hz sounds good enough as a default value. This makes the nodes consume 2% CPU.

I've seen you have this so-called "limited" nodes that do something like this but with the ability of changing this kind of rate but there is a lot more of logic. And I haven't seen any example launch file nor documentation (sorry if it's there and I didn't see it!).

Let me know if you think this modification makes sense to make a pull request. I'm gonna use it locally.

request_camera_link_endpoint taking 100% CPU

Even when there are no subscribers the request_camera_link_endpoint is taking 100% CPU.

endpoint launch file:

<?xml version="1.0"?>

<launch>
    <arg name="image_name" default="camera" />
    <arg name="output" default="screen" />
    <arg name="remapped_tf" default="/tf" />
    <arg name="default_rate" default="10.0" />
    <arg name="quality" default="20" />
    <arg name="override_timestamps" default="false" />
    <arg name="node_name" default="$(arg image_name)" />

    <node pkg="opportunistic_link" type="request_camera_link_endpoint" name="$(arg node_name)_link_endpoint" output="$(arg output)" >
        <param name="relay_topic" type="string" value="$(arg image_name)/workstation/image" />
        <param name="latched" type="bool" value="true" />
        <param name="default_rate" type="double" value="$(arg default_rate)" />
        <param name="default_quality" type="int" value="$(arg quality)" />
        <param name="quality_ctrl" type="string" value="$(arg image_name)/quality" />
        <param name="rate_ctrl" type="string" value="$(arg image_name)/rate" />
        <param name="data_service" type="string" value="$(arg image_name)/data" />
        <param name="override_timestamps" type="bool" value="$(arg override_timestamps)" />
        <remap from="/tf" to="$(arg remapped_tf)" />
    </node>
</launch>

robot launch file:

<?xml version="1.0"?>

<launch>
    <arg name="image_name" default="camera" />
    <arg name="output" default="screen" />
    <arg name="remapped_tf" default="/tf" />
    <arg name="node_name" default="$(arg image_name)" />

    <node pkg="opportunistic_link" type="camera_resizer" name="$(arg node_name)_camera_resizer" output="$(arg output)" >
        <param name="camera_base_topic" type="string" value="$(arg image_name)/image_raw" />
        <param name="resized_base_topic" type="string" value="$(arg image_name)/resized/image" />
        <param name="resized_width" type="int" value="320" />
        <param name="resized_height" type="int" value="240" />
        <param name="convert_to_bw" type="bool" value="false" />

        <remap from="/tf" to="$(arg remapped_tf)" />
    </node>

    <node pkg="opportunistic_link" type="request_camera_link_startpoint" name="$(arg node_name)_link_startpoint" output="$(arg output)" >
        <param name="camera_topic" type="string" value="$(arg image_name)/resized/image" />
        <param name="data_service" type="string" value="$(arg image_name)/data" />

        <remap from="/tf" to="$(arg remapped_tf)" />
    </node>
</launch>

robot console:

started roslaunch server http://zac-nuc-2:47175/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.16
 * /usb_cam0_camera_resizer/camera_base_topic: usb_cam0/image_raw
 * /usb_cam0_camera_resizer/convert_to_bw: False
 * /usb_cam0_camera_resizer/resized_base_topic: usb_cam0/resized/...
 * /usb_cam0_camera_resizer/resized_height: 240
 * /usb_cam0_camera_resizer/resized_width: 320
 * /usb_cam0_link_startpoint/camera_topic: usb_cam0/resized/...
 * /usb_cam0_link_startpoint/data_service: usb_cam0/data
 * /usb_cam1_camera_resizer/camera_base_topic: usb_cam1/image_raw
 * /usb_cam1_camera_resizer/convert_to_bw: False
 * /usb_cam1_camera_resizer/resized_base_topic: usb_cam1/resized/...
 * /usb_cam1_camera_resizer/resized_height: 240
 * /usb_cam1_camera_resizer/resized_width: 320
 * /usb_cam1_link_startpoint/camera_topic: usb_cam1/resized/...
 * /usb_cam1_link_startpoint/data_service: usb_cam1/data

NODES
  /
    usb_cam0_camera_resizer (opportunistic_link/camera_resizer)
    usb_cam0_link_startpoint (opportunistic_link/request_camera_link_startpoint)
    usb_cam1_camera_resizer (opportunistic_link/camera_resizer)
    usb_cam1_link_startpoint (opportunistic_link/request_camera_link_startpoint)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[usb_cam0_camera_resizer-1]: started with pid [22697]
process[usb_cam0_link_startpoint-2]: started with pid [22698]
process[usb_cam1_camera_resizer-3]: started with pid [22699]
[ INFO] [1461632408.564816530]: Starting camera resizer...
process[usb_cam1_link_startpoint-4]: started with pid [22700]
[ INFO] [1461632408.600747525]: Starting camera resizer...
[ INFO] [1461632408.601278019]: Starting request camera link startpoint...
[ INFO] [1461632408.632220403]: Starting request camera link startpoint...
[ INFO] [1461632408.849675242]: Subscribed using raw for transport
[ INFO] [1461632408.849732612]: ...startup complete
[ INFO] [1461632408.860271345]: Subscribed using raw for transport
[ INFO] [1461632408.860315974]: ...startup complete
[ WARN] [1461632409.077377644]: Configuration changed qmax: 51
[ INFO] [1461632409.079600070]: Actual image data topic: /usb_cam0/image_raw
[ INFO] [1461632409.079637154]: Actual camera_info topic: /usb_cam0/camera_info
[ INFO] [1461632409.079657501]: Subscribed using raw for transport
[ INFO] [1461632409.079674027]: ...startup complete
[ WARN] [1461632409.118873410]: Configuration changed qmax: 51
[ INFO] [1461632409.120312223]: Actual image data topic: /usb_cam1/image_raw
[ INFO] [1461632409.120388803]: Actual camera_info topic: /usb_cam1/camera_info
[ INFO] [1461632409.120420531]: Subscribed using raw for transport
[ INFO] [1461632409.120450955]: ...startup complete

workstation console:

started roslaunch server http://zac-nuc-2:48929/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.16
 * /usb_cam0_link_endpoint/data_service: usb_cam0/data
 * /usb_cam0_link_endpoint/default_quality: 20
 * /usb_cam0_link_endpoint/default_rate: 10.0
 * /usb_cam0_link_endpoint/latched: True
 * /usb_cam0_link_endpoint/override_timestamps: False
 * /usb_cam0_link_endpoint/quality_ctrl: usb_cam0/quality
 * /usb_cam0_link_endpoint/rate_ctrl: usb_cam0/rate
 * /usb_cam0_link_endpoint/relay_topic: usb_cam0/workstat...
 * /usb_cam1_link_endpoint/data_service: usb_cam1/data
 * /usb_cam1_link_endpoint/default_quality: 20
 * /usb_cam1_link_endpoint/default_rate: 10.0
 * /usb_cam1_link_endpoint/latched: True
 * /usb_cam1_link_endpoint/override_timestamps: False
 * /usb_cam1_link_endpoint/quality_ctrl: usb_cam1/quality
 * /usb_cam1_link_endpoint/rate_ctrl: usb_cam1/rate
 * /usb_cam1_link_endpoint/relay_topic: usb_cam1/workstat...

NODES
  /
    usb_cam0_link_endpoint (opportunistic_link/request_camera_link_endpoint)
    usb_cam1_link_endpoint (opportunistic_link/request_camera_link_endpoint)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[usb_cam0_link_endpoint-1]: started with pid [22991]
process[usb_cam1_link_endpoint-2]: started with pid [22992]
[ INFO] [1461632409.849329801]: Starting request camera link endpoint...
[ INFO] [1461632409.865682662]: Starting request camera link endpoint...
[ WARN] [1461632410.252915139]: Configuration changed qmax: 51
[ INFO] [1461632410.254351043]: ...startup complete
[ WARN] [1461632410.278458415]: Configuration changed qmax: 51
[ INFO] [1461632410.280002630]: ...startup complete

missing pc60_compression.cpp

@calderpg I think you forgot to add the cpp file to your push

Improve documentation

Documentation on the limited_* type nodes is missing or incomplete.

Tutorial 3: wrongly documented parameter name

Hi,

I just wanted to let you guys know there is a mismatch in the parameter name referring to the input topic in the Startpoint for request_image_link_startpoint.cpp and request_camera_link_startpoint.cpp (both using camera_topic as the parameter name), as well as in limited_camera_link_startpoint.cpp (defining camera_base_topic).

See:

The source limited_image_link_startpoint.cpp does match the image_topic parameter documented in the wiki page.

If changing/standardising the parameter names across source codes might result in breaking launch files you guys already use, perhaps adding a footnote in the wiki advising people to double check parameter names in the source code would help a great deal.

I hope you find this helpful.