caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Wrote the base of rover_control. Currently drives rover using FrSky. Needs tuning.

Browse Source
This commit is contained in:
Corwin Perren
2018-02-26 03:04:21 -08:00
parent 5837622d3c
commit f37337a976
90 changed files with 34328 additions and 49773 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
software/firmware/iris/iris.ino
View File

@@ -46,7 +46,7 @@ uint8_t message_count = 0;
uint8_t failSafe;
uint16_t lostFrames = 0;
uint16_t telem_24v_scalar = 37989;
uint16_t telem_24v_scalar = 37500;
////////// Class Instantiations //////////
SBUS x8r(SBUS_HARDWARE_PORT);
@@ -57,7 +57,7 @@ void setup() {
x8r.begin();
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
slave.begin(2000000);
slave.begin(115200);
Serial.begin(9600);
}
@@ -104,8 +104,7 @@ void poll_sbus(){
}
void poll_sensors(){
uint16_t v24 = telem_24v_scalar * (analogRead(HARDWARE::TELEM_24V) / 8192.0);
Serial.println(v24);
modbus_data[MODBUS_REGISTERS::VOLTAGE_24] = telem_24v_scalar * (analogRead(HARDWARE::TELEM_24V) / 8192.0);
}

18
software/rover/drive_test/CMakeLists.txt
View File

@@ -1,18 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(drive_test)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
stage_ros
geometry_msgs
roscpp
rospy
)
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
)

BIN
software/rover/drive_test/HW2.pdf
View File

Binary file not shown.

0
software/rover/drive_test/ReadMe
View File

7
software/rover/drive_test/launch/drive_test.launch
View File

@@ -1,7 +0,0 @@
<launch>
<node pkg="stage_ros" type="stageros" name="simulator" args="$(find drive_test)/worlds/manyDots.world"/>
<node pkg="drive_test" type="drive_test.py" name="hw2" output="screen">
<param name="goalX" value="20" />
<param name="goalY" value="-20" />
</node>
</launch>

58
software/rover/drive_test/package.xml
View File

@@ -1,58 +0,0 @@
<?xml version="1.0"?>
<package>
<name>drive_test</name>
<version>0.0.0</version>
<description>ROB456 HW2</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="rob456@todo.todo">rob456</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/pioneer_delivery</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>stage_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>stage_ros</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

90
software/rover/drive_test/src/drive_test.py
View File

@@ -1,90 +0,0 @@
#!/usr/bin/env python
import rospy
import math
import tf
from tf.transformations import euler_from_quaternion
import message_filters
from pprint import pprint
import time
# The laser scan message
from sensor_msgs.msg import Joy
# The odometry message
from nav_msgs.msg import Odometry
# the velocity command message
from geometry_msgs.msg import Twist
# global pi - this may come in handy
pi = math.pi
# method to control the robot
def callback_spacenav(twist_message):
scalar_forward = 100.0
scalar_turn = 2.0
# make a new twist message
command = Twist()
# Fill in the fields. Field values are unspecified
# until they are actually assigned. The Twist message
# holds linear and angular velocities.
command.linear.x = twist_message.linear.x * scalar_forward
command.linear.y = twist_message.linear.y * scalar_forward
command.linear.z = twist_message.linear.z * scalar_forward
command.angular.x = twist_message.angular.x * scalar_forward
command.angular.y = twist_message.angular.y * scalar_forward
command.angular.z = twist_message.angular.z * scalar_turn
# print command
# Send the commands
pub.publish(command)
# method to control the robot
def callback_joy(joy_message):
scalar_forward = 100.0
scalar_turn = 2.0
# make a new twist message
command = Twist()
# Fill in the fields. Field values are unspecified
# until they are actually assigned. The Twist message
# holds linear and angular velocities.
command.linear.x = joy_message.axes[1] * 100
#command.linear.y = twist_message.linear.y * scalar_forward
#command.linear.z = twist_message.linear.z * scalar_forward
#command.angular.x = twist_message.angular.x * scalar_forward
#command.angular.y = twist_message.angular.y * scalar_forward
command.angular.z = joy_message.axes[0] * 1.0
# command.angular.z = joy_message.axes[2] * 1.0
print joy_message
# Send the commands
pub.publish(command)
# main function call
if __name__ == "__main__":
# Initialize the node
rospy.init_node('drive_test', log_level=rospy.DEBUG)
# subscribe to laser scan message
# sub = rospy.Subscriber('/spacenav/twist', Twist, callback_spacenav)
# subscribe to laser scan message
sub = rospy.Subscriber('/joy', Joy, callback_joy)
# synchronize laser scan and odometry data
# ts = message_filters.TimeSynchronizer([sub, sub2], 10)
# ts.registerCallback(callback)
# publish twist message
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Turn control over to ROS
rospy.spin()

48122
software/rover/drive_test/worlds/manyDots.pgm
View File

File diff suppressed because it is too large Load Diff

71
software/rover/drive_test/worlds/manyDots.world
View File

@@ -1,71 +0,0 @@
define block model
(
size [0.5 0.5 0.5]
gui_nose 0
)
define topurg ranger
(
sensor(
range [ 0.0 30.0 ]
fov 270.25
samples 1081
)
# generic model properties
color "black"
size [ 0.05 0.05 0.1 ]
)
define erratic position
(
size [0.35 0.35 0.25]
origin [-0.05 0 0 0]
gui_nose 0
drive "diff"
topurg(pose [ 0.050 0.000 0 0.000 ])
odom_error [0.00 0.00 0.00 0.00 0.00 0.00]
)
define floorplan model
(
# sombre, sensible, artistic
color "gray30"
# most maps will need a bounding box
boundary 1
gui_nose 0
gui_grid 0
gui_outline 0
gripper_return 0
fiducial_return 0
laser_return 1
)
# set the resolution of the underlying raytrace model in meters
resolution 0.005
interval_sim 100 # simulation timestep in milliseconds
window
(
size [ 745.000 448.000 ]
rotate [ 0.000 0.000 ]
scale 5
)
# load an environment bitmap
floorplan
(
name "manyDots"
bitmap "manyDots.pgm"
size [54.0 58.7 0.5]
pose [ 0 0.0 0 90.000 ]
)
# throw in a robot
erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])

197
software/rover/launcher/CMakeLists.txt
View File

@@ -1,197 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(launcher)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES launcher
# CATKIN_DEPENDS rospy
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/launcher.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/launcher_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_launcher.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

BIN
software/rover/launcher/launch/.test_imu.py.kate-swp
View File

Binary file not shown.

15
software/rover/launcher/launch/launch_everything.launch
View File

@@ -1,15 +0,0 @@
<launch>
<node name="rover_gps" pkg="ublox_gps" type="ublox_gps" output="screen" clear_params="false" respawn="true" respawn_delay="10">
<rosparam command="load" file="$(find ublox_gps)/config/nmea.yaml" />
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<remap from="/imu/data" to="/your/imu/topic" />
<remap from="/gps/fix" to="/rover_gps/fix" />
<remap from="/odometry/filtered" to="/your/robot_localization/output/topic" />
</node>
</launch>

17
software/rover/launcher/random_testing/test_imu.py
View File

@@ -1,17 +0,0 @@
import rospy
from geometry_msgs.msg import Quaternion, Vector3
from std_msgs.msg import Float64
if __name__ == "__main__":
rospy.init_node("imu_test")
orientation = Quaternion()
orientation_covarience = Float64()
angular_velocity = Vector3()
angular_velocity_covarience = Float64()
linear_acceleration = Vector3()
linear_acceleration_covarience = Float64()

201
software/rover/nimbro_topic_transport/CMakeLists.txt Normal file
View File

@@ -0,0 +1,201 @@
cmake_minimum_required(VERSION 2.8)
project(nimbro_topic_transport)
find_package(catkin REQUIRED COMPONENTS
roscpp
topic_tools
rostest
message_generation
)
add_message_files(FILES
CompressedMsg.msg
ReceiverStats.msg
SenderStats.msg
TopicBandwidth.msg
)
generate_messages(DEPENDENCIES
std_msgs
)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
# Some optional components of the NimbRo software framework
find_package(plot_msgs)
if(plot_msgs_FOUND)
include_directories(${plot_msgs_INCLUDE_DIRS})
add_definitions(-DWITH_PLOTTING=1)
endif()
find_package(config_server)
if(config_server_FOUND)
include_directories(${config_server_INCLUDE_DIRS})
add_definitions(-DWITH_CONFIG_SERVER=1)
endif()
find_package(catch_ros)
# If OpenFEC is available, we can enable FEC for the UDP sender
set(DEFAULT_OPENFEC_PATH /opt/openfec_v1.4.2)
set(OPENFEC_PATH "" CACHE PATH "Path to OpenFEC (optional)")
if(NOT OPENFEC_PATH AND IS_DIRECTORY ${DEFAULT_OPENFEC_PATH})
set(OPENFEC_PATH ${DEFAULT_OPENFEC_PATH})
endif()
if(OPENFEC_PATH)
include_directories(${OPENFEC_PATH}/src/lib_common)
set(OPENFEC_LIBRARY ${OPENFEC_PATH}/bin/Release/libopenfec.so)
add_definitions(-DWITH_OPENFEC=1)
message(STATUS "Found and using OpenFEC at: ${OPENFEC_PATH}")
else()
set(OPENFEC_LIBRARY "")
endif()
find_library(BZ2_LIBRARY bz2 REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
add_executable(udp_sender
src/udp/topic_sender.cpp
src/udp/udp_sender.cpp
src/topic_info.cpp
)
add_dependencies(udp_sender
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(udp_sender
${catkin_LIBRARIES}
${BZ2_LIBRARY}
${OPENFEC_LIBRARY}
${config_server_LIBRARIES}
)
add_executable(udp_receiver
src/udp/udp_receiver.cpp
src/udp/topic_receiver.cpp
src/topic_info.cpp
)
add_dependencies(udp_receiver
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(udp_receiver
${catkin_LIBRARIES}
${BZ2_LIBRARY}
${OPENFEC_LIBRARY}
)
add_executable(tcp_sender
src/tcp/tcp_sender.cpp
src/topic_info.cpp
)
add_dependencies(tcp_sender
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(tcp_sender
${catkin_LIBRARIES}
${BZ2_LIBRARY}
${config_server_LIBRARIES}
)
add_executable(tcp_receiver
src/tcp/tcp_receiver.cpp
src/topic_info.cpp
)
add_dependencies(tcp_receiver
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(tcp_receiver
${catkin_LIBRARIES}
${BZ2_LIBRARY}
)
# Tools
add_executable(action_proxy
src/action_proxy.cpp
src/topic_info.cpp
)
target_link_libraries(action_proxy
${catkin_LIBRARIES}
)
# GUI
find_package(Qt4 REQUIRED)
include(${QT_USE_FILE})
qt4_wrap_cpp(MOC_SRCS
src/gui/topic_gui.h
src/gui/dot_widget.h
)
qt4_wrap_cpp(BAND_MOC_SRCS
src/gui/bandwidth_gui.h
src/gui/contrib/qcustomplot/qcustomplot.h
)
add_library(topic_gui
${MOC_SRCS}
src/gui/topic_gui.cpp
src/gui/dot_widget.cpp
)
add_library(bandwidth_gui
${BAND_MOC_SRCS}
src/gui/bandwidth_gui.cpp
src/gui/contrib/qcustomplot/qcustomplot.cpp
)
add_dependencies(topic_gui
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(topic_gui
${QT_LIBRARIES}
)
add_dependencies(bandwidth_gui
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(bandwidth_gui
${QT_LIBRARIES}
yaml-cpp
)
# Tests
if(catch_ros_FOUND)
include_directories(${catch_ros_INCLUDE_DIRS})
catch_add_rostest_node(test_comm
test/test_comm.cpp
)
target_link_libraries(test_comm
${catkin_LIBRARIES}
)
add_rostest(test/topic_transport.test ARGS protocol:=udp)
add_rostest(test/topic_transport.test ARGS protocol:=tcp)
if(OPENFEC_PATH)
add_rostest(test/topic_transport.test ARGS protocol:=udp fec:=true)
endif()
catch_add_rostest_node(test_bidirectional
test/test_bidirectional.cpp
)
target_link_libraries(test_bidirectional
${catkin_LIBRARIES}
)
add_rostest(test/bidirectional.test ARGS allow_bidirectional:=true)
add_rostest(test/bidirectional.test ARGS allow_bidirectional:=false)
endif()
#install
install(TARGETS udp_receiver tcp_receiver udp_sender tcp_sender
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

71
software/rover/nimbro_topic_transport/README.md Normal file
View File

@@ -0,0 +1,71 @@
nimbro_topic_transport
======================
This package includes nodes for transmitting ROS topic messages over a network
connection. For an overview over the available parameters, see
`doc/configuration.md`.
For a quick getting-started guide see `doc/getting_started.md`.
The remainder of this document introduces the features of the topic transport
and explains the choices you have in detail.
The fundamental choice you have is whether you want to use the TCP or the UDP
protocol.
UDP
---
The UDP protocol is the right choice for most ROS topics. It is especially
suited for lossy and/or delayed connections, where TCP has problems. Note
however, that with our UDP protocol there is no guarantee that a message
arrives.
Typical message types transmitted using the UDP protocol are camera images,
joystick commands, TF snapshots (see tf_throttle). In short, everything where
retransmitting missed packets does not make sense, because you have newer data
available already.
For example launch files, see the launch directory.
TCP
---
The TCP protocol is more useful for topics where you need a transmission
guarantee. For example, it makes sense to transmit 3D laser scans via this
method, because re-transmission of dropped packets is still faster than
waiting for the next scan.
For example launch files, see the launch directory.
Visualization & diagnosis
-------------------------
The nimbro_topic_transport package provides GUI plugins for the `rqt` GUI:
- "nimbro_network/Topic Transport" shows a dot graph of all network connections
- "nimbro_network/Topic Bandwidth" shows detailed bandwidth usage for a
single connection.
Note that it may be necessary to transmit the topics `/network/sender_stats` and
`/network/receiver_stats` over the network to get the full amount of
information.
Relay mode
----------
The UDP transport supports a special "relay" mode that was used in the DARPA
Robotics Challenge. Basically, it saturates a given target bitrate, repeating
messages if necessary. In the DRC, the high bandwidth link was switched on for
very short times (1 second). By saturating the available bandwidth, we made
sure that we got the maximum amount of data across in that window.
Forward error correction (FEC)
------------------------------
The UDP transport can do forward error correction if [OpenFEC][1] is available.
A short guide on how to install and configure the system is available
in `doc/FEC.md`.
[1]: http://openfec.org/

15
software/rover/nimbro_topic_transport/bandwidth_plugin.xml Normal file
View File

@@ -0,0 +1,15 @@
<library path="lib/libbandwidth_gui">
<class name="BandwidthGui" type="nimbro_topic_transport::BandwidthGui" base_class_type="rqt_gui_cpp::Plugin">
<description>
Display bandwidth information for transferred topics
</description>
<qtgui>
<group>
<label>nimbro_network</label>
</group>
<label>Topic Bandwidth</label>
<statustip>Diagnostic information</statustip>
</qtgui>
</class>
</library>

18
software/rover/nimbro_topic_transport/doc/FEC.md Normal file
View File

@@ -0,0 +1,18 @@
Forward Error Correction
========================
This is a short guide on how to get FEC running. If you want to use FEC,
you have to download [OpenFEC][OpenFEC] and compile it. Assuming you already
have compiled `nimbro_topic_transport` at least once, you can then inform
it about your OpenFEC installation:
```
cd my_catkin_workspace/build/nimbro_topic_transport
cmake -DOPENFEC_PATH=/path/to/openfec .
make
```
Afterwards, you can use the `fec` parameter on sender & receiver side as
described in `configuration.md`.
[OpenFEC]: http://openfec.org/

74
software/rover/nimbro_topic_transport/doc/configuration.md Normal file
View File

@@ -0,0 +1,74 @@
Configuration
=============
`nimbro_topic_transport` is configured through ROS parameters.
Receiver parameters
-------------------
`tcp_receiver` and `udp_receiver` accept the following parameters:
Required:
- `port` (int): UDP/TCP port to bind to (required)
Optional:
- `drop_repeated_msgs` (bool): If a message with the same sequence number
arrives twice, drop it. Needed in conjunction with the relay mode.
(UDP only, default true)
- `fec` (bool): Enable Forward Error correction (UDP only, default false)
- `keep_compressed` (bool): Do not uncompress compressed topics, instead
publish them as the type `nimbro_topic_transport/CompressedMsg`
(default false)
- `label` (string): Display a label in the visualization GUIs
- `topic_prefix` (string): prepend topic_prefix before advertised topic names
- `warn_drop_incomplete` (bool): Display a warning every time an incomplete
packet is dropped (UDP only, default true)
Sender parameters
-----------------
`tcp_sender` and `udp_sender` accept the following parameters:
Required:
- `destination_addr` (string): Hostname or IP address of the destination
machine (required)
- `destination_port` (int): Port number to connect to (required)
- `source_port` (int): Source port to bind to. If not specified, the port is
chosen by the OS (TODO: true for udp_sender!)
- `topics` (list): List of topics to be transmitted (see below)
Optional:
- `fec` (float): If non-zero, this is the proportion of repair packets sent for
Forward Error Correction (0.5 -> Send 50% more data). This needs support for
FEC compiled in, see README.md (default 0.0)
- `label` (string): Display a label in the visualization GUIs
- `relay_mode` (bool): Enable relay mode, see README.md
(UDP only, default false)
- `relay_target_bitrate` (float): Target bitrate for relay mode (UDP only)
- `relay_control_rate` (float): Check if new packets can be sent in relay mode
at this rate (UDP only)
- `ignored_publishers` (list of string): Names of nodes whose messages should be
ignored if received by this sender. This should be used on both senders when
messages are to be sent to a topic both ways (always specify the name of the
receiver belonging to the other sender). See `launch/bidirectional_topics.launch`
for an example setup (TCP only)
Topic configuration
-------------------
Configuration of topics to be transmitted is done on the parameter server of
the sender's side. See the example launch files for the usual setup.
Here is a list of parameters that are available per topic. The only mandatory
parameter is `name`.
- `name`: Name of the topic to be sent over.
- `rate`: Rate limit on messages / sec (floating point). Messages over the
rate limit are silently dropped on the sender side. The default is 0.0
(no rate limit).
Note: Limiting only works well for lower rates (<20 Hz).
(UDP only)
- `resend`: If the sender does not get a message 1.0/`rate` after the last one,
it will re-send the last received one. (UDP only)
- `compress`: If true, compress the data on the wire with bz2.

26
software/rover/nimbro_topic_transport/doc/getting_started.md Normal file
View File

@@ -0,0 +1,26 @@
Getting started
===============
We assume that we have two machines, machineA and machineB. Both will be running
roscores, and we will transport some topics from machineA to machineB.
On machineA:
```bash
roslaunch nimbro_topic_transport udp_sender.launch target:=machineB
rostopic pub -r 1.0 /my_first_topic std_msgs/String "Hello World"
```
Instead of using the host name `machineB`, you can also use an IP address.
On machineB:
```bash
roslaunch nimbro_topic_transport udp_receiver.launch
rostopic echo /my_first_topic
```
You should see the `Hello World` messages arriving.
For customization, you should copy the launch files into your own package.

30
software/rover/nimbro_topic_transport/launch/bidirectional_machine1.launch Normal file
View File

@@ -0,0 +1,30 @@
<launch>
<arg name="target" default="localhost" />
<arg name="allow_bidirectional" default="true" />
<!-- The TCP sender node -->
<node name="tcp_sender_machine1" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<!-- The destination host name or IP address -->
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17001" />
<!-- Load the list of topics from a YAML file -->
<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics_machine1.yaml" />
<!-- If bidirectional traffic over a topic is expected, fill this parameter.
See details in bidirectional_topics.launch -->
<rosparam param="ignored_publishers" if="$(arg allow_bidirectional)">["/tcp_receiver_machine1"]</rosparam>
<rosparam param="ignored_publishers" unless="$(arg allow_bidirectional)">[]</rosparam>
</node>
<node name="tcp_receiver_machine1" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
<param name="port" value="17002" />
<!-- Remap topics so that sender & receiver do not clash if run on the
same machine. This is not necessary in a typical setup :-)
-->
<remap from="/recv/my_first_topic" to="/my_first_topic" />
</node>
</launch>

29
software/rover/nimbro_topic_transport/launch/bidirectional_machine2.launch Normal file
View File

@@ -0,0 +1,29 @@
<launch>
<arg name="target" default="localhost" />
<arg name="allow_bidirectional" default="true" />
<!-- The TCP sender node -->
<node name="tcp_sender_machine2" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<!-- The destination host name or IP address -->
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17002" />
<!-- Load the list of topics from a YAML file -->
<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics_machine2.yaml" />
<!-- If bidirectional traffic over a topic is expected, fill this parameter.
See details in bidirectional_topics.launch -->
<rosparam param="ignored_publishers" if="$(arg allow_bidirectional)">["/tcp_receiver_machine2"]</rosparam>
<rosparam param="ignored_publishers" unless="$(arg allow_bidirectional)">[]</rosparam>
</node>
<node name="tcp_receiver_machine2" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
<param name="port" value="17001" />
<!-- Remap topics so that sender & receiver do not clash if run on the
same machine. This is not necessary in a typical setup :-)
-->
<remap from="/my_first_topic" to="/recv/my_first_topic" />
</node>
</launch>

22
software/rover/nimbro_topic_transport/launch/bidirectional_topics.launch Normal file
View File

@@ -0,0 +1,22 @@
<launch>
<arg name="allow_bidirectional" default="true" />
<!--
If you set up transports for a topic in both directions, don't forget to
provide the `ignored_publishers` parameter to both senders. Otherwise,
nimbro_network would enter an infinite republishing loop. You can test it
by playing with the `allow_bidirectional` argument and publishing to one
of the topics.
The `ignored_publishers` sender parameter is a list of node-names of
all receivers receiving any of the topics this publisher publishes.
-->
<include file="$(find nimbro_topic_transport)/launch/bidirectional_machine1.launch">
<arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
</include>
<include file="$(find nimbro_topic_transport)/launch/bidirectional_machine2.launch">
<arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
</include>
</launch>

11
software/rover/nimbro_topic_transport/launch/tcp_receiver.launch Normal file
View File

@@ -0,0 +1,11 @@
<launch>
<node name="tcp_receiver" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
<param name="port" value="17001" />
<!-- Remap topics so that sender & receiver do not clash if run on the
same machine. This is not necessary in a typical setup :-)
-->
<remap from="/my_first_topic" to="/recv/my_first_topic" />
<remap from="/my_second_topic" to="/recv/my_second_topic" />
</node>
</launch>

14
software/rover/nimbro_topic_transport/launch/tcp_sender.launch Normal file
View File

@@ -0,0 +1,14 @@
<launch>
<arg name="target" default="localhost" />
<!-- The UDP sender node -->
<node name="tcp_sender" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
<!-- The destination host name or IP address -->
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17001" />
<!-- Load the list of topics from a YAML file -->
<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
</node>
</launch>

5
software/rover/nimbro_topic_transport/launch/topics.yaml Normal file
View File

@@ -0,0 +1,5 @@
topics:
- name: "/drive/motoroneandtwo"
compress: true # enable bz2 compression
rate: 20.0 # rate limit at 1Hz
- name: "/my_second_topic"

2
software/rover/nimbro_topic_transport/launch/topics_machine1.yaml Normal file
View File

@@ -0,0 +1,2 @@
topics:
- name: "/my_first_topic"

2
software/rover/nimbro_topic_transport/launch/topics_machine2.yaml Normal file
View File

@@ -0,0 +1,2 @@
topics:
- name: "/recv/my_first_topic"

19
software/rover/nimbro_topic_transport/launch/udp_receiver.launch Normal file
View File

@@ -0,0 +1,19 @@
<launch>
<!--
This launch file runs a udp_receiver node, which receives topics
over the network on port 17001 and publishes them on the local roscore.
See udp_sender.launch for the sender part.
-->
<node name="udp_receiver" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<!-- The port to receive packets on -->
<param name="port" value="17001" />
<!-- Remap topics so that sender & receiver do not clash if run on the
same machine. This is not necessary in a typical setup :-)
-->
<remap from="/my_first_topic" to="/recv/my_first_topic" />
<remap from="/my_second_topic" to="/recv/my_second_topic" />
</node>
</launch>

26
software/rover/nimbro_topic_transport/launch/udp_sender.launch Normal file
View File

@@ -0,0 +1,26 @@
<launch>
<!--
This launch file runs a udp_sender node, which sends topics from the local
roscore over the network on port 17001.
By default, this launch file sends topics to your local machine for
testing purposes. If you want to send to another machine, use
roslaunch nimbro_topic_transport udp_sender.launch target:=other_host
where other_host can be a host name or IP address.
See udp_receiver.launch for the receiving part.
-->
<arg name="target" default="localhost" />
<!-- The UDP sender node -->
<node name="udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<!-- The destination host name or IP address -->
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17001" />
<!-- Load the list of topics from a YAML file -->
<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
</node>
</launch>

8
software/rover/nimbro_topic_transport/msg/CompressedMsg.msg Normal file
View File

@@ -0,0 +1,8 @@
# Topic type (e.g. 'std_msgs/String')
string type
# Topic md5 sum
uint32[4] md5
uint8[] data

31
software/rover/nimbro_topic_transport/msg/ReceiverStats.msg Normal file
View File

@@ -0,0 +1,31 @@
Header header
# Node name
string node
# Protocol ("TCP" or "UDP")
string protocol
# Connection label
string label
# Local hostname
string host
# Remote hostname (if resolvable, otherwise IP)
string remote
# Port
uint16 local_port
# Port
uint16 remote_port
# is Forward Error Correction enabled?
bool fec
# Bandwidth in bit/s
float32 bandwidth
# Drop rate (estimated, percentage of missing packets in a fixed time interval)
float32 drop_rate

31
software/rover/nimbro_topic_transport/msg/SenderStats.msg Normal file
View File

@@ -0,0 +1,31 @@
Header header
# Node name
string node
# Protocol ("TCP" or "UDP")
string protocol
# Connection label
string label
# Local hostname
string host
# Destination address
string destination
# Destination port
uint16 destination_port
# Source port
uint16 source_port
# Is Forward Error Correction enabled?
bool fec
# Bandwidth in bit/s
float32 bandwidth
# Topic specific bandwidth
TopicBandwidth[] topics

4
software/rover/nimbro_topic_transport/msg/TopicBandwidth.msg Normal file
View File

@@ -0,0 +1,4 @@
# Topic name
string name
# Bandwidth
float32 bandwidth

26
software/rover/nimbro_topic_transport/my_launch/udp_sender.launch Normal file
View File

@@ -0,0 +1,26 @@
<launch>
<!--
This launch file runs a udp_sender node, which sends topics from the local
roscore over the network on port 17001.
By default, this launch file sends topics to your local machine for
testing purposes. If you want to send to another machine, use
roslaunch nimbro_topic_transport udp_sender.launch target:=other_host
where other_host can be a host name or IP address.
See udp_receiver.launch for the receiving part.
-->
<arg name="target" default="192.168.1.10" />
<!-- The UDP sender node -->
<node name="udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
<!-- The destination host name or IP address -->
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17001" />
<!-- Load the list of topics from a YAML file -->
<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
</node>
</launch>

32
software/rover/nimbro_topic_transport/package.xml Normal file
View File

@@ -0,0 +1,32 @@
<package format="2">
<name>nimbro_topic_transport</name>
<version>1.0.0</version>
<description>Contains nodes for TCP/UDP communication between ROS masters</description>
<maintainer email="max.schwarz@uni-bonn.de">Max Schwarz</maintainer>
<license>GPLv2</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>topic_tools</depend>
<build_depend>message_generation</build_depend>
<!-- This is not a hard dependency, the package compiles without plot_msgs. -->
<depend>plot_msgs</depend>
<!-- This is not a hard dependency, the package compiles without config_server. -->
<depend>config_server</depend>
<!-- This is not a hard dependency, the package compiles without catch_ros -->
<depend>catch_ros</depend>
<depend>rqt_gui</depend>
<export>
<rqt_gui plugin="${prefix}/rqt_plugin.xml" />
<rqt_gui plugin="${prefix}/bandwidth_plugin.xml" />
</export>
</package>

15
software/rover/nimbro_topic_transport/rqt_plugin.xml Normal file
View File

@@ -0,0 +1,15 @@
<library path="lib/libtopic_gui">
<class name="topic_gui" type="nimbro_topic_transport::TopicGUI" base_class_type="rqt_gui_cpp::Plugin">
<description>
Display diagnostic information about the topic transport
</description>
<qtgui>
<group>
<label>nimbro_network</label>
</group>
<label>Topic Transport</label>
<statustip>Diagnostic information</statustip>
</qtgui>
</class>
</library>

138
software/rover/nimbro_topic_transport/src/action_proxy.cpp Normal file
View File

@@ -0,0 +1,138 @@
// Proxy node for action topics
// actionlib expects that all action topics are handled by a single node
// -- so republish / resubscribe them from here.
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include <actionlib_msgs/GoalID.h>
#include <actionlib_msgs/GoalStatusArray.h>
#include <topic_tools/shape_shifter.h>
#include "topic_info.h"
ros::Subscriber m_sub_goal;
ros::Publisher m_pub_goal;
void handleGoal(const topic_tools::ShapeShifter::ConstPtr& goal)
{
m_pub_goal.publish(goal);
}
ros::Subscriber m_sub_cancel;
ros::Publisher m_pub_cancel;
void handleCancel(const actionlib_msgs::GoalID& id)
{
m_pub_cancel.publish(id);
}
ros::Subscriber m_sub_result;
ros::Publisher m_pub_result;
void handleResult(const topic_tools::ShapeShifter::ConstPtr& result)
{
m_pub_result.publish(result);
}
ros::Subscriber m_sub_status;
ros::Publisher m_pub_status;
void handleStatus(const actionlib_msgs::GoalStatusArray& status)
{
m_pub_status.publish(status);
}
ros::Subscriber m_sub_feedback;
ros::Publisher m_pub_feedback;
void handleFeedback(const topic_tools::ShapeShifter::ConstPtr& fb)
{
m_pub_feedback.publish(fb);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "action_proxy");
ros::NodeHandle nh("~");
std::string type;
if(!nh.getParam("type", type))
{
ROS_FATAL("need type parameter");
return 1;
}
std::string input;
if(!nh.getParam("input", input))
{
ROS_FATAL("need input parameter");
return 1;
}
std::string output;
if(!nh.getParam("output", output))
{
ROS_FATAL("need output parameter");
return 1;
}
{
ros::AdvertiseOptions ops;
ops.topic = input + "/goal";
ops.datatype = type + "ActionGoal";
ops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(ops.datatype);
m_pub_goal = nh.advertise(ops);
ros::SubscribeOptions subops;
subops.init<topic_tools::ShapeShifter>(output + "/goal", 10, boost::bind(&handleGoal, _1));
subops.datatype = type + "ActionGoal";
subops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(subops.datatype);
m_sub_goal = nh.subscribe(subops);
}
{
m_pub_cancel = nh.advertise<actionlib_msgs::GoalID>(input + "/cancel", 1);
m_sub_cancel = nh.subscribe(output + "/cancel", 10, &handleCancel);
}
{
ros::AdvertiseOptions ops;
ops.topic = output + "/result";
ops.datatype = type + "ActionResult";
ops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(ops.datatype);
m_pub_result = nh.advertise(ops);
ros::SubscribeOptions subops;
subops.init<topic_tools::ShapeShifter>(input + "/result", 10, boost::bind(&handleResult, _1));
subops.datatype = type + "ActionResult";
subops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(subops.datatype);
m_sub_result = nh.subscribe(subops);
}
{
ros::AdvertiseOptions ops;
ops.topic = output + "/feedback";
ops.datatype = type + "ActionFeedback";
ops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(ops.datatype);
m_pub_feedback = nh.advertise(ops);
ros::SubscribeOptions subops;
subops.init<topic_tools::ShapeShifter>(input + "/feedback", 10, boost::bind(&handleFeedback, _1));
subops.datatype = type + "ActionFeedback";
subops.md5sum = nimbro_topic_transport::topic_info::getMd5Sum(subops.datatype);
m_sub_feedback = nh.subscribe(subops);
}
{
m_pub_status = nh.advertise<actionlib_msgs::GoalStatusArray>(output + "/status", 10);
m_sub_status = nh.subscribe(input + "/status", 10, &handleStatus);
}
ros::spin();
return 0;
}

353
software/rover/nimbro_topic_transport/src/gui/bandwidth_gui.cpp Normal file
View File

@@ -0,0 +1,353 @@
// Bandwidth display for transferred topics
// Author: Sebastian Schüller
#include "bandwidth_gui.h"
#include "contrib/qcustomplot/qcustomplot.h"
#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>
#include <yaml-cpp/yaml.h>
#include <algorithm>
#include <QLayout>
#include <QComboBox>
#include <QPushButton>
#include <QTextEdit>
#include <QMessageBox>
Q_DECLARE_METATYPE(nimbro_topic_transport::SenderStatsConstPtr)
static const int WINDOW_SIZE = 64;
static const int BRUSH_VALUE = 180;
static const int BRUSH_SATURATION = 150;
static const std::string DEFAULT_GROUPS =
"cameras: [\"/camera_center/h264\", \"/camera_left/h264\", \"/camera_right/h264\"]\n"
"hand_cameras: [\"/camera_left_hand/h264\", \"/camera_right_hand/h264\"]\n"
"network_stats: [\"/network/sender_stats\", \"/network/receiver_stats\"]\n";
inline double bpsToKbps(double bps)
{
return bps/1000.0;
}
inline double bpsToMbps(double bps)
{
return bpsToKbps(bps)/1000.0;
}
namespace nimbro_topic_transport
{
BandwidthGui::BandwidthGui()
: m_maxBandwidth(0)
, m_hue(175)
{}
BandwidthGui::~BandwidthGui()
{}
void BandwidthGui::initPlugin(qt_gui_cpp::PluginContext& ctx)
{
m_plot = new QCustomPlot();
m_plot->legend->setVisible(true);
m_plot->legend->setIconBorderPen(Qt::NoPen);
m_plot->yAxis->setLabel("Kb/s");
QWidget* wrapper = new QWidget();
m_connectionBox = new QComboBox(wrapper);
QPushButton* groupBtn = new QPushButton("Group Settings", wrapper);
QGridLayout* gl = new QGridLayout(wrapper);
gl->addWidget(m_plot, 0, 0, 1, 2);
gl->addWidget(m_connectionBox, 1, 0);
gl->addWidget(groupBtn, 1, 1);
wrapper->setLayout(gl);
wrapper->setWindowTitle("Bandwidth");
ctx.addWidget(wrapper);
connect(
m_plot, SIGNAL(legendClick(QCPLegend *, QCPAbstractLegendItem *, QMouseEvent *)),
this, SLOT(handleClickedLegend(QCPLegend*, QCPAbstractLegendItem*, QMouseEvent*))
);
connect(
m_connectionBox, SIGNAL(currentIndexChanged(int)),
this, SLOT(clearPlot())
);
connect(
groupBtn, SIGNAL(pressed()),
this, SLOT(updateGroupInfo())
);
qRegisterMetaType<nimbro_topic_transport::SenderStatsConstPtr>();
connect(
this, SIGNAL(senderStatsReceived(nimbro_topic_transport::SenderStatsConstPtr)),
this, SLOT(handleSenderStats(nimbro_topic_transport::SenderStatsConstPtr)),
Qt::QueuedConnection
);
m_sub_senderStats = getPrivateNodeHandle().subscribe(
"/network/sender_stats", 1, &BandwidthGui::senderStatsReceived, this
);
m_plot->xAxis->setTickLabelType(QCPAxis::ltDateTime);
m_plot->xAxis->setDateTimeFormat("hh:mm:ss");
m_plot->xAxis->setAutoTickStep(true);
m_plot->yAxis->setRangeLower(0);
QCPLayoutGrid* subLayout = new QCPLayoutGrid();
QCPLayoutElement* dummy = new QCPLayoutElement();
m_plot->plotLayout()->addElement(0, 1, subLayout);
subLayout->addElement(0, 0, m_plot->legend);
subLayout->addElement(1, 0, dummy);
subLayout->setRowStretchFactor(0, 0.01);
m_plot->plotLayout()->setColumnStretchFactor(1, 0.01);
connect(
m_plot->xAxis, SIGNAL(rangeChanged(QCPRange)),
m_plot->xAxis2, SLOT(setRange(QCPRange))
);
connect(
m_plot->yAxis, SIGNAL(rangeChanged(QCPRange)),
m_plot->yAxis2, SLOT(setRange(QCPRange))
);
connect(
&m_plotTimer, SIGNAL(timeout()),
this, SLOT(updatePlot())
);
m_plotTimer.start(0);
}
void BandwidthGui::shutdownPlugin()
{
m_sub_senderStats.shutdown();
}
void BandwidthGui::clearPlot()
{
m_maxBandwidth = 0;
m_bandwidths.clear();
m_plot->clearGraphs();
}
void BandwidthGui::handleSenderStats(const nimbro_topic_transport::SenderStatsConstPtr& msg)
{
// Update connection info
std::stringstream ss;
ss << "[" << msg->protocol << "] ";
ss << msg->host << ":" << msg->source_port << " -> ";
ss << msg->destination << ":" << msg->destination_port;
QString connection = QString::fromStdString(ss.str());
if(m_connectionBox->findText(connection) == -1)
m_connectionBox->addItem(connection);
if(connection != m_connectionBox->currentText())
return;
for(auto& gv : m_bandwidths)
gv.second.value = 0.0;
for(const auto& topic : msg->topics)
{
//Look up if topic is in group
std::string name = topic.name;
for(const auto& group : m_groupMap)
{
const auto& members = group.second;
if(std::find(members.begin(), members.end(), name) != members.end())
{
name = group.first;
break;
}
}
if(m_bandwidths.find(name) == m_bandwidths.end())
{
// Add and configure new graph to plot
auto graph = m_plot->addGraph();
graph->setName(QString::fromStdString(name));
graph->setPen(QPen(QColor::fromHsv(0,110,150)));
graph->setBrush(QBrush(QColor::fromHsv(m_hue, BRUSH_SATURATION, BRUSH_VALUE)));
m_hue = (m_hue + 137) % 360;
m_bandwidths[name].graph = graph;
m_bandwidths[name].last_timestamp = std::numeric_limits<double>::max();
}
m_bandwidths[name].value += bpsToKbps(topic.bandwidth);
m_bandwidths[name].timestamp = msg->header.stamp.toSec();
}
}
void BandwidthGui::updateGroupInfo()
{
QString temp;
GroupDialog dial; //ToDo find correct parent
dial.setText(m_grpYamlString);
int result = dial.exec();
if (result != QDialog::Accepted)
return;
temp = dial.text();
if (m_grpYamlString == temp)
return;
GroupMap groupMap;
if(!groupFromYaml(temp.toStdString(), &groupMap))
{
QMessageBox::warning(m_plot->parentWidget(), "Warning!", "Malformed YAML");
return;
}
m_grpYamlString = temp;
clearPlot();
m_groupMap = groupMap;
return;
}
bool BandwidthGui::groupFromYaml(const std::string& yaml, GroupMap* groupMap)
{
YAML::Node grpInfo = YAML::Load(yaml);
if (grpInfo.IsNull())
{
return true;
}
if (grpInfo.Type() != YAML::NodeType::Map)
{
return false;
}
for (const auto& pair : grpInfo)
{
if(pair.first.Type() != YAML::NodeType::Scalar)
{
return false;
}
std::string key = pair.first.as<std::string>();
for (const auto& element : pair.second)
{
if(element.Type() != YAML::NodeType::Scalar)
{
return false;
}
(*groupMap)[key].push_back(element.as<std::string>());
}
}
return true;
}
void BandwidthGui::updatePlot()
{
double plotValue = 0;
QCPGraph* prevGraph = nullptr;
for(auto& bw : m_bandwidths)
{
auto& gv = bw.second;
plotValue += gv.value;
if(prevGraph)
gv.graph->setChannelFillGraph(prevGraph);
prevGraph = gv.graph;
gv.graph->addData(gv.timestamp, plotValue);
m_maxBandwidth = std::max(m_maxBandwidth, plotValue);
m_plot->yAxis->setRangeUpper(m_maxBandwidth + (m_maxBandwidth/50.0));
gv.graph->removeDataBefore(5*WINDOW_SIZE);
gv.last_timestamp = gv.timestamp;
}
m_plot->xAxis->setRange(ros::Time::now().toSec() + 0.25, WINDOW_SIZE, Qt::AlignRight);
m_plot->replot();
}
void BandwidthGui::handleClickedLegend(QCPLegend *legend, QCPAbstractLegendItem *item, QMouseEvent *event)
{
QCPPlottableLegendItem* graphItem = dynamic_cast<QCPPlottableLegendItem*>(item);
if(!graphItem)
return;
auto color = graphItem->plottable()->brush().color();
auto hue = color.hue() + 180 % 360;
auto sat = color.saturation();
auto val = color.value();
if(sat == BRUSH_SATURATION)
{
sat = 255;
val = 255;
}
else
{
sat = BRUSH_SATURATION;
val = BRUSH_VALUE;
}
color.setHsv(hue, sat, val);
graphItem->plottable()->setBrush(QBrush(color));
}
void BandwidthGui::saveSettings(qt_gui_cpp::Settings& pluginSettings, qt_gui_cpp::Settings& instanceSettings) const
{
instanceSettings.setValue("connection", m_connectionBox->currentText());
instanceSettings.setValue("groups", m_grpYamlString);
}
void BandwidthGui::restoreSettings(const qt_gui_cpp::Settings& pluginSettings, const qt_gui_cpp::Settings& instanceSettings)
{
if(instanceSettings.contains("connection"))
m_connectionBox->addItem(instanceSettings.value("connection").toString());
if(instanceSettings.contains("groups"))
{
m_grpYamlString = instanceSettings.value("groups").toString();
GroupMap groupMap;
if(m_grpYamlString.isEmpty())
m_grpYamlString = QString::fromStdString(DEFAULT_GROUPS);
if(!groupFromYaml(m_grpYamlString.toStdString(), &groupMap))
{
m_grpYamlString = "";
return;
}
m_groupMap = groupMap;
}
}
////// Group Dialog
GroupDialog::GroupDialog()
: m_tEdit(new QTextEdit(this))
{
QPushButton* okBtn = new QPushButton("Accept", this);
QPushButton* cancelBtn = new QPushButton("Cancel", this);
QGridLayout* gl = new QGridLayout(this);
gl->addWidget(m_tEdit, 0, 0, 1, 2);
gl->addWidget(cancelBtn, 1, 0);
gl->addWidget(okBtn, 1, 1);
this->setLayout(gl);
connect(
okBtn, SIGNAL(pressed()),
this, SLOT(accept())
);
connect(
cancelBtn, SIGNAL(pressed()),
this, SLOT(reject())
);
}
void GroupDialog::setText(QString text)
{
m_tEdit->setPlainText(text);
}
QString GroupDialog::text()
{
return m_tEdit->toPlainText();
}
}
PLUGINLIB_EXPORT_CLASS(nimbro_topic_transport::BandwidthGui, rqt_gui_cpp::Plugin)

93
software/rover/nimbro_topic_transport/src/gui/bandwidth_gui.h Normal file
View File

@@ -0,0 +1,93 @@
// Bandwidth display for transferred topics
// Author: Sebastian Schüller
#ifndef BANDWIDTH_GUI
#define BANDWIDTH_GUI
#include <rqt_gui_cpp/plugin.h>
#include <ros/subscriber.h>
#include <nimbro_topic_transport/SenderStats.h>
#include <map>
#include <QTimer>
#include <QDialog>
class QCustomPlot;
class QCPGraph;
class QCPLegend;
class QCPAbstractLegendItem;
class QMouseEvent;
class QComboBox;
class QTextEdit;
typedef std::map<std::string, std::vector<std::string> > GroupMap;
namespace nimbro_topic_transport
{
class BandwidthGui : public rqt_gui_cpp::Plugin
{
Q_OBJECT
public:
BandwidthGui();
virtual ~BandwidthGui();
virtual void initPlugin(qt_gui_cpp::PluginContext & ) override;
virtual void shutdownPlugin() override;
virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const override;
virtual void restoreSettings(const qt_gui_cpp::Settings& pluginSettings, const qt_gui_cpp::Settings& instanceSettings) override;
Q_SIGNALS:
// Bounce SenderStats msg from ros thread to GUI thread
void senderStatsReceived(const nimbro_topic_transport::SenderStatsConstPtr& msg);
private Q_SLOTS:
void handleSenderStats(const nimbro_topic_transport::SenderStatsConstPtr& msg);
void updateGroupInfo();
void updatePlot();
void clearPlot();
void handleClickedLegend(QCPLegend *legend, QCPAbstractLegendItem *item, QMouseEvent *event);
private:
struct GraphValue
{
float value; // bandwidth value
double timestamp; // in seconds since epoch
double last_timestamp; // in seconds, timestamp of last point
QCPGraph* graph;
};
bool groupFromYaml(const std::string& yaml, GroupMap* map);
QCustomPlot* m_plot;
QComboBox* m_connectionBox;
QTimer m_plotTimer;
ros::Subscriber m_sub_senderStats;
std::map<std::string, GraphValue> m_bandwidths;
double m_maxBandwidth;
int m_hue;
std::vector<std::string> m_connections;
QString m_grpYamlString;
GroupMap m_groupMap;
};
// Text Dialog to change Group Settings
class GroupDialog : public QDialog
{
Q_OBJECT
public:
GroupDialog();
void setText(QString text);
QString text();
private:
QTextEdit* m_tEdit;
};
}
#endif

674
software/rover/nimbro_topic_transport/src/gui/contrib/qcustomplot/GPL.txt Normal file
View File

@@ -0,0 +1,674 @@
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
<program> Copyright (C) <year> <name of author>
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
<http://www.gnu.org/licenses/>.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
<http://www.gnu.org/philosophy/why-not-lgpl.html>.

416
software/rover/nimbro_topic_transport/src/gui/contrib/qcustomplot/changelog.txt Normal file
View File

@@ -0,0 +1,416 @@
#### Version 1.3.1 released on 25.04.15 ####
Bugfixes:
- Fixed bug that prevented automatic axis rescaling when some graphs/curves had only NaN data points
- Improved QCPItemBracket selection boundaries, especially bsCurly and bsCalligraphic
- Fixed bug of axis rect and colorscale background shifted downward by one logical pixel (visible in scaled png and pdf export)
- Replot upon mouse release is now only performed if a selection change has actually happened (improves responsivity on particularly complex plots)
- Fixed bug that allowed scatter-only graphs to be selected by clicking the non-existent line between scatters
- Fixed crash when trying to select a scatter-only QCPGraph whose only points in the visible key range are at identical key coordinates and vertically off-screen, with adaptive sampling enabled
- Fixed pdf export of QCPColorMap with enabled interpolation (didn't appear interpolated in pdf)
- Reduced QCPColorMap jitter of internal cell boundaries for small sized maps when viewed with high zoom, by applying oversampling factors dependant on map size
- Fixed bug of QCPColorMap::fill() not causing the buffered internal image map to be updated, and thus the change didn't become visible immediately
- Axis labels with size set in pixels (setPixelSize) instead of points now correctly calculate the exponent's font size if beautifully typeset powers are enabled
- Fixed QCPColorMap appearing at the wrong position for logarithmic axes and color map spanning larger ranges
Other:
- Pdf export used to embed entire QCPColorMaps, potentially leading to large files. Now only the visible portion of the map is embedded in the pdf
- Many documentation fixes and extensions, style modernization
- Reduced documentation file size (and thus full package size) by automatically reducing image palettes during package build
- Fixed MSVC warning message (at warning level 4) due to temporary QLists in some foreach statements
#### Version 1.3.0 released on 27.12.14 ####
Added features:
- New plottable class QCPFinancial allows display of candlestick/ohlc data
- New class QCPBarsGroup allows horizontal grouping of multiple QCPBars plottables
- Added QCPBars feature allowing non-zero base values (see property QCPBars::setBaseValue)
- Added QCPBars width type, for more flexible bar widths (see property QCPBars::setWidthType)
- New QCPCurve optimization algorithm, fixes bug which caused line flicker at deep zoom into curve segment
- Item positions can now have different position types and anchors for their x and y coordinates (QCPItemPosition::setTypeX/Y, setParentAnchorX/Y)
- QCPGraph and QCPCurve can now display gaps in their lines, when inserting quiet NaNs as values (std::numeric_limits<double>::quiet_NaN())
- QCPAxis now supports placing the tick labels inside the axis rect, for particularly space saving plots (QCPAxis::setTickLabelSide)
Added features after beta:
- Made code compatible with QT_NO_CAST_FROM_ASCII, QT_NO_CAST_TO_ASCII
- Added compatibility with QT_NO_KEYWORDS after sending code files through a simple reg-ex script
- Added possibility to inject own QCPAxis(-subclasses) via second, optional QCPAxisRect::addAxis parameter
- Added parameter to QCPItemPixmap::setScaled to specify transformation mode
Bugfixes:
- Fixed bug in QCPCurve rendering of very zoomed-in curves (via new optimization algorithm)
- Fixed conflict with MSVC-specific keyword "interface" in text-document-integration example
- Fixed QCPScatterStyle bug ignoring the specified pen in the custom scatter shape constructor
- Fixed bug (possible crash) during QCustomPlot teardown, when a QCPLegend that has no parent layout (i.e. was removed from layout manually) gets deleted
Bugfixes after beta:
- Fixed bug of QCPColorMap/QCPColorGradient colors being off by one color sampling step (only noticeable in special cases)
- Fixed bug of QCPGraph adaptive sampling on vertical key axis, causing staggered look
- Fixed low (float) precision in QCPCurve optimization algorithm, by not using QVector2D anymore
Other:
- Qt 5.3 and Qt 5.4 compatibility
#### Version 1.2.1 released on 07.04.14 ####
Bugfixes:
- Fixed regression which garbled date-time tick labels on axes, if setTickLabelType is ltDateTime and setNumberFormat contains the "b" option
#### Version 1.2.0 released on 14.03.14 ####
Added features:
- Adaptive Sampling for QCPGraph greatly improves performance for high data densities (see QCPGraph::setAdaptiveSampling)
- QCPColorMap plottable with QCPColorScale layout element allows plotting of 2D color maps
- QCustomPlot::savePdf now has additional optional parameters pdfCreator and pdfTitle to set according PDF metadata fields
- QCustomPlot::replot now allows specifying whether the widget update is immediate (repaint) or queued (update)
- QCPRange operators +, -, *, / with double operand for range shifting and scaling, and ==, != for range comparison
- Layers now have a visibility property (QCPLayer::setVisible)
- static functions QCPAxis::opposite and QCPAxis::orientation now offer more convenience when handling axis types
- added notification signals for selectability change (selectableChanged) on all objects that have a selected/selectable property
- added notification signal for QCPAxis scaleType property
- added notification signal QCPLayerable::layerChanged
Bugfixes:
- Fixed assert halt, when QCPAxis auto tick labels not disabled but nevertheless a custom non-number tick label ending in "e" given
- Fixed painting glitches when QCustomPlot resized inside a QMdiArea or under certain conditions inside a QLayout
- If changing QCPAxis::scaleType and thus causing range sanitizing and a range modification, rangeChanged wouldn't be emitted
- Fixed documentation bug that caused indentation to be lost in code examples
Bugfixes after beta:
- Fixed bug that caused crash if clicked-on legend item is removed in mousePressEvent.
- On some systems, font size defaults to -1, which used to cause a debug output in QCPAxisPainterPrivate::TickLabelDataQCP. Now it's checked before setting values based on the default font size.
- When using multiple axes on one side, setting one to invisible didn't properly compress the freed space.
- Fixed bug that allowed selection of plottables when clicking in the bottom or top margin of a QCPAxisRect (outside the inner rect)
Other:
- In method QCPAbstractPlottable::getKeyRange/getValueRange, renamed parameter "validRange" to "foundRange", to better reflect its meaning (and contrast it from QCPRange::validRange)
- QCPAxis low-level axis painting methods exported to QCPAxisPainterPrivate
#### Version 1.1.1 released on 09.12.13 ####
Bugfixes:
- Fixed bug causing legends blocking input events from reaching underlying axis rect even if legend is invisible
- Added missing Q_PROPERTY for QCPAxis::setDateTimeSpec
- Fixed behaviour of QCPAxisRect::setupFullAxesBox (now transfers more properties from bottom/left to top/right axes and sets visibility of bottom/left axes to true)
- Made sure PDF export doesn't default to grayscale output on some systems
Other:
- Plotting hint QCP::phForceRepaint is now enabled on all systems (and not only on windows) by default
- Documentation improvements
#### Version 1.1.0 released on 04.11.13 ####
Added features:
- Added QCPRange::expand and QCPRange::expanded
- Added QCPAxis::rescale to rescale axis to all associated plottables
- Added QCPAxis::setDateTimeSpec/dateTimeSpec to allow axis labels either in UTC or local time
- QCPAxis now additionally emits a rangeChanged signal overload that provides the old range as second parameter
Bugfixes:
- Fixed QCustomPlot::rescaleAxes not rescaling properly if first plottable has an empty range
- QCPGraph::rescaleAxes/rescaleKeyAxis/rescaleValueAxis are no longer virtual (never were in base class, was a mistake)
- Fixed bugs in QCPAxis::items and QCPAxisRect::items not properly returning associated items and potentially stalling
Other:
- Internal change from QWeakPointer to QPointer, thus got rid of deprecated Qt functionality
- Qt5.1 and Qt5.2 (beta1) compatibility
- Release packages now extract to single subdirectory and don't place multiple files in current working directory
#### Version 1.0.1 released on 05.09.13 ####
Bugfixes:
- using define flag QCUSTOMPLOT_CHECK_DATA caused debug output when data was correct, instead of invalid (fixed QCP::isInvalidData)
- documentation images are now properly shown when viewed with Qt Assistant
- fixed various documentation mistakes
Other:
- Adapted documentation style sheet to better match Qt5 documentation
#### Version 1.0.0 released on 01.08.13 ####
Quick Summary:
- Layout system for multiple axis rects in one plot
- Multiple axes per side
- Qt5 compatibility
- More flexible and consistent scatter configuration with QCPScatterStyle
- Various interface cleanups/refactoring
- Pixmap-cached axis labels for improved replot performance
Changes that break backward compatibility:
- QCustomPlot::axisRect() changed meaning due to the extensive changes to how axes and axis rects are handled
it now returns a pointer to a QCPAxisRect and takes an integer index as parameter.
- QCPAxis constructor changed to now take QCPAxisRect* as parent
- setAutoMargin, setMarginLeft/Right/Top/Bottom removed due to the axis rect changes (see QCPAxisRect::setMargins/setAutoMargins)
- setAxisRect removed due to the axis rect changes
- setAxisBackground(-Scaled/-ScaledMode) now moved to QCPAxisRect as setBackground(-Scaled/ScaledMode) (access via QCustomPlot::axisRects())
- QCPLegend now is a QCPLayoutElement
- QCPAbstractPlottable::drawLegendIcon parameter "rect" changed from QRect to QRectF
- QCPAbstractLegendItem::draw second parameter removed (position/size now handled via QCPLayoutElement base class)
- removed QCPLegend::setMargin/setMarginLeft/Right/Top/Bottom (now inherits the capability from QCPLayoutElement::setMargins)
- removed QCPLegend::setMinimumSize (now inherits the capability from QCPLayoutElement::setMinimumSize)
- removed enum QCPLegend::PositionStyle, QCPLegend::positionStyle/setPositionStyle/position/setPosition (replaced by capabilities of QCPLayoutInset)
- QCPLegend transformed to work with new layout system (almost everything changed)
- removed entire title interface: QCustomPlot::setTitle/setTitleFont/setTitleColor/setTitleSelected/setTitleSelectedFont/setTitleSelectedColor and
the QCustomPlot::iSelectTitle interaction flag (all functionality is now given by the layout element "QCPPlotTitle" which can be added to the plot layout)
- selectTest functions now take two additional parameters: bool onlySelectable and QVariant *details=0
- selectTest functions now ignores visibility of objects and (if parameter onlySelectable is true) does not anymore ignore selectability of the object
- moved QCustomPlot::Interaction/Interactions to QCP namespace as QCP::Interaction/Interactions
- moved QCustomPlot::setupFullAxesBox() to QCPAxisRect::setupFullAxesBox. Now also accepts parameter to decide whether to connect opposite axis ranges
- moved range dragging/zooming interface from QCustomPlot to QCPAxisRect (setRangeDrag, setRangeZoom, setRangeDragAxes, setRangeZoomAxes,...)
- rangeDrag/Zoom is now set to Qt::Horizontal|Qt::Vertical instead of 0 by default, on the other hand, iRangeDrag/Zoom is unset in interactions by
default (this makes enabling dragging/zooming easier by just adding the interaction flags)
- QCPScatterStyle takes over everything related to handling scatters in all plottables
- removed setScatterPen/Size on QCPGraph and QCPCurve, removed setOutlierPen/Size on QCPStatisticalBox (now handled via QCPScatterStyle)
- modified setScatterStyle on QCPGraph and QCPCurve, and setOutlierStyle on QCPStatisticalBox, to take QCPScatterStyle
- axis grid and subgrid are now reachable via the QCPGrid *QCPAxis::grid() method. (e.g. instead of xAxis->setGrid(true), write xAxis->grid()->setVisible(true))
Added features:
- Axis tick labels are now pixmap-cached, thus increasing replot performance (in usual setups by about 24%). See plotting hint phCacheLabels which is set by default
- Advanced layout system, including the classes QCPLayoutElement, QCPLayout, QCPLayoutGrid, QCPLayoutInset, QCPAxisRect
- QCustomPlot::axisRects() returns all the axis rects in the QCustomPlot.
- QCustomPlot::plotLayout() returns the top level layout (initially a QCPLayoutGrid with one QCPAxisRect inside)
- QCPAxis now may have an offset to the axis rect (setOffset)
- Multiple axes per QCPAxisRect side are now supported (see QCPAxisRect::addAxis)
- QCustomPlot::toPixmap renders the plot into a pixmap and returns it
- When setting tick label rotation to +90 or -90 degrees on a vertical axis, the labels are now centered vertically on the tick height
(This allows space saving vertical tick labels by having the text direction parallel to the axis)
- Substantially increased replot performance when using very large manual tick vectors (> 10000 ticks) via QCPAxis::setTickVector
- QCPAxis and QCPAxisRect now allow easy access to all plottables(), graphs() and items() that are associated with them
- Added QCustomPlot::hasItem method for consistency with plottable interface, hasPlottable
- Added QCPAxisRect::setMinimumMargins as replacement for hardcoded minimum axis margin (15 px) when auto margin is enabled
- Added Flags type QCPAxis::AxisTypes (from QCPAxis::AxisType), used in QCPAxisRect interface
- Automatic margin calculation can now be enabled/disabled on a per-side basis, see QCPAxisRect::setAutoMargins
- QCPAxisRect margins of multiple axis rects can be coupled via QCPMarginGroup
- Added new default layers "background" and "legend" (QCPAxisRect draws its background on the "background" layer, QCPLegend is on the "legend" layer by default)
- Custom scatter style via QCP::ssCustom and respective setCustomScatter functions that take a QPainterPath
- Filled scatters via QCPScatterStyle::setBrush
Added features after beta:
- Added QCustomPlot::toPainter method, to allow rendering with existing painter
- QCPItemEllipse now provides a center anchor
Bugfixes:
- Fixed compile error on ARM
- Wrong legend icons were displayed if using pixmaps for scatters that are smaller than the legend icon rect
- Fixed clipping inaccuracy for rotated tick labels (were hidden too early, because the non-rotated bounding box was used)
- Fixed bug that caused wrong clipping of axis ticks and subticks when the ticks were given manually by QCPAxis::setTickVector
- Fixed Qt5 crash when dragging graph out of view (iterator out of bounds in QCPGraph::getVisibleDataBounds)
- Fixed QCPItemText not scaling properly when using scaled raster export
Bugfixes after beta:
- Fixed bug that clipped the rightmost pixel column of tick labels when caching activated (only visible on windows for superscript exponents)
- Restored compatibility to Qt4.6
- Restored support for -no-RTTI compilation
- Empty manual tick labels are handled more gracefully (no QPainter qDebug messages anymore)
- Fixed type ambiguity in QCPLineEnding::draw causing compile error on ARM
- Fixed bug of grid layouts not propagating the minimum size from their child elements to the parent layout correctly
- Fixed bug of child elements (e.g. axis rects) of inset layouts not properly receiving mouse events
Other:
- Opened up non-amalgamated project structure to public via git repository
#### Version released on 09.06.12 ####
Quick Summary:
- Items (arrows, text,...)
- Layers (easier control over rendering order)
- New antialiasing system (Each objects controls own antialiasing with setAntialiased)
- Performance Improvements
- improved pixel-precise drawing
- easier shared library creation/usage
Changes that (might) break backward compatibility:
- enum QCPGraph::ScatterSymbol was moved to QCP namespace (now QCP::ScatterSymbol).
This replace should fix your code: "QCPGraph::ss" -> "QCP::ss"
- enum QCustomPlot::AntialiasedElement and flag QCustomPlot::AntialiasedElements was moved to QCP namespace
This replace should fix your code: "QCustomPlot::ae" -> "QCP::ae"
- the meaning of QCustomPlot::setAntialiasedElements has changed slightly: It is now an override to force elements to be antialiased. If you want to force
elements to not be drawn antialiased, use the new setNotAntialiasedElements. If an element is mentioned in neither of those functions, it now controls
its antialiasing itself via its "setAntialiased" function(s). (e.g. QCPAxis::setAntialiased(bool), QCPAbstractPlottable::setAntialiased(bool),
QCPAbstractPlottable::setAntialiasedScatters(bool), etc.)
- QCPAxis::setTickVector and QCPAxis::setTickVectorLabels no longer take a pointer but a const reference of the respective QVector as parameter.
(handing over a pointer didn't give any noticeable performance benefits but was inconsistent with the rest of the interface)
- Equally QCPAxis::tickVector and QCPAxis::tickVectorLabels don't return by pointer but by value now
- QCustomPlot::savePngScaled was removed, its purpose is now included as optional parameter "scale" of savePng.
- If you have derived from QCPAbstractPlottable: all selectTest functions now consistently take the argument "const QPointF &pos" which is the test point in pixel coordinates.
(the argument there was "double key, double value" in plot coordinates, before).
- QCPAbstractPlottable, QCPAxis and QCPLegend now inherit from QCPLayerable
- If you have derived from QCPAbstractPlottable: the draw method signature has changed from "draw (..) const" to "draw (..)", i.e. the method
is not const anymore. This allows the draw function of your plottable to perform buffering/caching operations, if necessary.
Added features:
- Item system: QCPAbstractItem, QCPItemAnchor, QCPItemPosition, QCPLineEnding. Allows placing of lines, arrows, text, pixmaps etc.
- New Items: QCPItemStraightLine, QCPItemLine, QCPItemCurve, QCPItemEllipse, QCPItemRect, QCPItemPixmap, QCPItemText, QCPItemBracket, QCPItemTracer
- QCustomPlot::addItem/itemCount/item/removeItem/selectedItems
- signals QCustomPlot::itemClicked/itemDoubleClicked
- the QCustomPlot interactions property now includes iSelectItems (for selection of QCPAbstractItem)
- QCPLineEnding. Represents the different styles a line/curve can end (e.g. different arrows, circle, square, bar, etc.), see e.g. QCPItemCurve::setHead
- Layer system: QCPLayerable, QCPLayer. Allows more sophisticated control over drawing order and a kind of grouping.
- QCPAbstractPlottable, QCPAbstractItem, QCPAxis, QCPGrid, QCPLegend are layerables and derive from QCPLayerable
- QCustomPlot::addLayer/moveLayer/removeLayer/setCurrentLayer/layer/currentLayer/layerCount
- Initially there are three layers: "grid", "main", and "axes". The "main" layer is initially empty and set as current layer, so new plottables/items are put there.
- QCustomPlot::viewport now makes the previously inaccessible viewport rect read-only-accessible (needed that for item-interface)
- PNG export now allows transparent background by calling QCustomPlot::setColor(Qt::transparent) before savePng
- QCPStatisticalBox outlier symbols may now be all scatter symbols, not only hardcoded circles.
- perfect precision of scatter symbol/error bar drawing and clipping in both antialiased and non-antialiased mode, by introducing QCPPainter
that works around some QPainter bugs/inconveniences. Further, more complex symbols like ssCrossSquare used to look crooked, now they look good.
- new antialiasing control system: Each drawing element now has its own "setAntialiased" function to control whether it is drawn antialiased.
- QCustomPlot::setAntialiasedElements and QCustomPlot::setNotAntialiasedElements can be used to override the individual settings.
- Subclasses of QCPAbstractPlottable can now use the convenience functions like applyFillAntialiasingHint or applyScattersAntialiasingHint to
easily make their drawing code comply with the overall antialiasing system.
- QCustomPlot::setNoAntialiasingOnDrag allows greatly improved performance and responsiveness by temporarily disabling all antialiasing while
the user is dragging axis ranges
- QCPGraph can now show scatter symbols at data points and hide its line (see QCPGraph::setScatterStyle, setScatterSize, setScatterPixmap, setLineStyle)
- Grid drawing code was sourced out from QCPAxis to QCPGrid. QCPGrid is mainly an internal class and every QCPAxis owns one. The grid interface still
works through QCPAxis and hasn't changed. The separation allows the grid to be drawn on a different layer as the axes, such that e.g. a graph can
be above the grid but below the axes.
- QCustomPlot::hasPlottable(plottable), returns whether the QCustomPlot contains the plottable
- QCustomPlot::setPlottingHint/setPlottingHints, plotting hints control details about the plotting quality/speed
- export to jpg and bmp added (QCustomPlot::saveJpg/saveBmp), as well as control over compression quality for png and jpg
- multi-select-modifier may now be specified with QCustomPlot::setMultiSelectModifier and is not fixed to Ctrl anymore
Bugfixes:
- fixed QCustomPlot ignores replot after it had size (0,0) even if size becomes valid again
- on Windows, a repaint used to be delayed during dragging/zooming of a complex plot, until the drag operation was done.
This was fixed, i.e. repaints are forced after a replot() call. See QCP::phForceRepaint and setPlottingHints.
- when using the raster paintengine and exporting to scaled PNG, pen widths are now scaled correctly (QPainter bug workaround via QCPPainter)
- PDF export now respects QCustomPlot background color (QCustomPlot::setColor), also Qt::transparent
- fixed a bug on QCPBars and QCPStatisticalBox where auto-rescaling of axis would fail when all data is very small (< 1e-11)
- fixed mouse event propagation bug that prevented range dragging from working on KDE (GNU/Linux)
- fixed a compiler warning on 64-bit systems due to pointer cast to int instead of quintptr in a qDebug output
Other:
- Added support for easier shared library creation (including examples for compiling and using QCustomPlot as shared library)
- QCustomPlot now has the Qt::WA_OpaquePaintEvent widget attribute (gives slightly improved performance).
- QCP::aeGraphs (enum QCP::AntialiasedElement, previously QCustomPlot::aeGraphs) has been marked deprecated since version 02.02.12 and
was now removed. Use QCP::aePlottables instead.
- optional performance-quality-tradeoff for solid graph lines (see QCustomPlot::setPlottingHints).
- marked data classes and QCPRange as Q_MOVABLE_TYPE
- replaced usage of own macro FUNCNAME with Qt macro Q_FUNC_INFO
- QCustomPlot now returns a minimum size hint of 50*50
#### Version released on 31.03.12 ####
Changes that (might) break backward compatibility:
- QCPAbstractLegendItem now inherits from QObject
- mousePress, mouseMove and mouseRelease signals are now emitted before and not after any QCustomPlot processing (range dragging, selecting, etc.)
Added features:
- Interaction system: now allows selecting of objects like plottables, axes, legend and plot title, see QCustomPlot::setInteractions documentation
- Interaction system for plottables:
- setSelectable, setSelected, setSelectedPen, setSelectedBrush, selectTest on QCPAbstractPlottable and all derived plottables
- setSelectionTolerance on QCustomPlot
- selectedPlottables and selectedGraphs on QCustomPlot (returns the list of currently selected plottables/graphs)
- Interaction system for axes:
- setSelectable, setSelected, setSelectedBasePen, setSelectedTickPen, setSelectedSubTickPen, setSelectedLabelFont, setSelectedTickLabelFont,
setSelectedLabelColor, setSelectedTickLabelColor, selectTest on QCPAxis
- selectedAxes on QCustomPlot (returns a list of the axes that currently have selected parts)
- Interaction system for legend:
- setSelectable, setSelected, setSelectedBorderPen, setSelectedIconBorderPen, setSelectedBrush, setSelectedFont, setSelectedTextColor, selectedItems on QCPLegend
- setSelectedFont, setSelectedTextColor, setSelectable, setSelected on QCPAbstractLegendItem
- selectedLegends on QCustomPlot
- Interaction system for title:
- setSelectedTitleFont, setSelectedTitleColor, setTitleSelected on QCustomPlot
- new signals in accordance with the interaction system:
- selectionChangedByUser on QCustomPlot
- selectionChanged on QCPAbstractPlottable
- selectionChanged on QCPAxis
- selectionChanged on QCPLegend and QCPAbstractLegendItem
- plottableClick, legendClick, axisClick, titleClick, plottableDoubleClick, legendDoubleClick, axisDoubleClick, titleDoubleClick on QCustomPlot
- QCustomPlot::deselectAll (deselects everything, i.e. axes and plottables)
- QCPAbstractPlottable::pixelsToCoords (inverse function to the already existing coordsToPixels function)
- QCPRange::contains(double value)
- QCPAxis::setLabelColor and setTickLabelColor
- QCustomPlot::setTitleColor
- QCustomPlot now emits beforeReplot and afterReplot signals. Note that it is safe to make two customPlots mutually call eachothers replot functions
in one of these slots, it will not cause an infinite loop. (usefull for synchronizing axes ranges between two customPlots, because setRange alone doesn't replot)
- If the Qt version is 4.7 or greater, the tick label strings in date-time-mode now support sub-second accuracy (e.g. with format like "hh:mm:ss.zzz").
Bugfixes:
- tick labels/margins should no longer oscillate by one pixel when dragging range or replotting repeatedly while changing e.g. data. This
was caused by a bug in Qt's QFontMetrics::boundingRect function when the font has an integer point size (probably some rounding problem).
The fix hence consists of creating a temporary font (only for bounding-box calculation) which is 0.05pt larger and thus avoiding the
jittering rounding outcome.
- tick label, axis label and plot title colors used to be undefined. This was fixed by providing explicit color properties.
Other:
- fixed some glitches in the documentation
- QCustomPlot::replot and QCustomPlot::rescaleAxes are now slots
#### Version released on 02.02.12 ####
Changes that break backward compatibility:
- renamed all secondary classes from QCustomPlot[...] to QCP[...]:
QCustomPlotAxis -> QCPAxis
QCustomPlotGraph -> QCPGraph
QCustomPlotRange -> QCPRange
QCustomPlotData -> QCPData
QCustomPlotDataMap -> QCPDataMap
QCustomPlotLegend -> QCPLegend
QCustomPlotDataMapIterator -> QCPDataMapIterator
QCustomPlotDataMutableMapIterator -> QCPDataMutableMapIterator
A simple search and replace on all code files should make your code run again, e.g. consider the regex "QCustomPlot(?=[AGRDL])" -> "QCP".
Make sure not to just replace "QCustomPlot" with "QCP" because the main class QCustomPlot hasn't changed to QCP.
This change was necessary because class names became unhandy, pardon my bad naming decision in the beginning.
- QCPAxis::tickLength() and QCPAxis::subTickLength() now each split into two functions for inward and outward ticks (tickLengthIn/tickLengthOut).
- QCPLegend now uses QCPAbstractLegendItem to carry item data (before, the legend was passed QCPGraphs directly)
- QCustomPlot::addGraph() now doesn't return the index of the created graph anymore, but a pointer to the created QCPGraph.
- QCustomPlot::setAutoAddGraphToLegend is replaced by setAutoAddPlottableToLegend
Added features:
- Reversed axis range with QCPAxis::setRangeReversed(bool)
- Tick labels are now only drawn if not clipped by the viewport (widget border) on the sides (e.g. left and right on a horizontal axis).
- Zerolines. Like grid lines only with a separate pen (QCPAxis::setZeroLinePen), at tick position zero.
- Outward ticks. QCPAxis::setTickLength/setSubTickLength now accepts two arguments for inward and outward tick length. This doesn't break
backward compatibility because the second argument (outward) has default value zero and thereby a call with one argument hasn't changed its meaning.
- QCPGraph now inherits from QCPAbstractPlottable
- QCustomPlot::addPlottable/plottable/removePlottable/clearPlottables added to interface with the new QCPAbstractPlottable-based system. The simpler interface
which only acts on QCPGraphs (addGraph, graph, removeGraph, etc.) was adapted internally and is kept for backward compatibility and ease of use.
- QCPLegend items for plottables (e.g. graphs) can automatically wrap their texts to fit the widths, see QCPLegend::setMinimumSize and QCPPlottableLegendItem::setTextWrap.
- QCustomPlot::rescaleAxes. Adapts axis ranges to show all plottables/graphs, by calling QCPAbstractPlottable::rescaleAxes on all plottables in the plot.
- QCPCurve. For plotting of parametric curves.
- QCPBars. For plotting of bar charts.
- QCPStatisticalBox. For statistical box plots.
Bugfixes:
- Fixed QCustomPlot::removeGraph(int) not being able to remove graph index 0
- made QCustomPlot::replot() abort painting when painter initialization fails (e.g. because width/height of QCustomPlot is zero)
- The distance of the axis label from the axis ignored the tick label padding, this could have caused overlapping axis labels and tick labels
- fixed memory leak in QCustomPlot (dtor didn't delete legend)
- fixed bug that prevented QCPAxis::setRangeLower/Upper from setting the value to exactly 0.
Other:
- Changed default error bar handle size (QCustomPlotGraph::setErrorBarSize) from 4 to 6.
- Removed QCustomPlotDataFetcher. Was deprecated and not used class.
- Extended documentation, especially class descriptions.
#### Version released on 15.01.12 ####
Changes that (might) break backward compatibility:
- QCustomPlotGraph now inherits from QObject
Added features:
- Added axis background pixmap (QCustomPlot::setAxisBackground, setAxisBackgroundScaled, setAxisBackgroundScaledMode)
- Added width and height parameter on PDF export function QCustomPlot::savePdf(). This now allows PDF export to
have arbitrary dimensions, independent of the current geometry of the QCustomPlot.
- Added overload of QCustomPlot::removeGraph that takes QCustomPlotGraph* as parameter, instead the index of the graph
- Added all enums to the Qt meta system via Q_ENUMS(). The enums can now be transformed
to QString values easily with the Qt meta system, which makes saving state e.g. as XML
significantly nicer.
- added typedef QMapIterator<double,QCustomPlotData> QCustomPlotDataMapIterator
and typedef QMutableMapIterator<double,QCustomPlotData> QCustomPlotDataMutableMapIterator
for improved information hiding, when using iterators outside QCustomPlot code
Bugfixes:
- Fixed savePngScaled. Axis/label drawing functions used to reset the painter transform
and thereby break savePngScaled. Now they buffer the current transform and restore it afterwards.
- Fixed some glitches in the doxygen comments (affects documentation only)
Other:
- Changed the default tickLabelPadding of top axis from 3 to 6 pixels. Looks better.
- Changed the default QCustomPlot::setAntialiasedElements setting: Graph fills are now antialiased
by default. That's a bit slower, but makes fill borders look better.
#### Version released on 19.11.11 ####
Changes that break backward compatibility:
- QCustomPlotAxis: tickFont and setTickFont renamed to tickLabelFont and setTickLabelFont (for naming consistency)
Other:
- QCustomPlotAxis: Added rotated tick labels, see setTickLabelRotation

23538
software/rover/nimbro_topic_transport/src/gui/contrib/qcustomplot/qcustomplot.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

3767
software/rover/nimbro_topic_transport/src/gui/contrib/qcustomplot/qcustomplot.h Normal file
View File

File diff suppressed because it is too large Load Diff

139
software/rover/nimbro_topic_transport/src/gui/dot_widget.cpp Normal file
View File

@@ -0,0 +1,139 @@
// Display a .dot graph
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "dot_widget.h"
#include <unistd.h>
#include <fcntl.h>
#include <stdexcept>
#include <sys/wait.h>
#include <QPainter>
static ssize_t runDot(const std::string& dot, int width, int height, uint8_t* pngOut, size_t pngOutSize)
{
int inputPipe[2];
int outputPipe[2];
if(pipe(inputPipe) != 0 || pipe(outputPipe) != 0)
throw std::runtime_error("Could not create pipe");
int pid = fork();
if(pid == 0)
{
// Child
close(inputPipe[1]);
close(outputPipe[0]);
dup2(inputPipe[0], STDIN_FILENO);
dup2(outputPipe[1], STDOUT_FILENO);
char sizeBuf[256];
int dpi = 400;
snprintf(sizeBuf, sizeof(sizeBuf), "-Gsize=%f,%f", ((float)width)/dpi, ((float)height)/dpi);
if(execlp("dot", "dot", "-Tpng", sizeBuf, "-Gdpi=400", "/dev/stdin", (char*)NULL) != 0)
{
perror("execlp() failed");
throw std::runtime_error("Could not execute dot");
exit(1);
}
// Cannot reach this
return -1;
}
else
{
// Parent
close(inputPipe[0]);
close(outputPipe[1]);
size_t written = 0;
size_t len = dot.length();
while(written != len)
{
int ret = write(inputPipe[1], dot.c_str() + written, len - written);
if(ret <= 0)
perror("Could not write");
written += ret;
}
close(inputPipe[1]);
size_t readBytes = 0;
while(1)
{
if(pngOutSize == readBytes)
{
throw std::runtime_error("png buffer is to small");
}
int ret = read(outputPipe[0], pngOut + readBytes, pngOutSize - readBytes);
if(ret == 0)
break;
else if(ret < 0)
{
perror("Could not read");
throw std::runtime_error("Could not read");
}
readBytes += ret;
}
close(outputPipe[0]);
int status;
waitpid(pid, &status, 0);
if(status != 0)
fprintf(stderr, "dot exited with error status %d\n", status);
return readBytes;
}
}
namespace nimbro_topic_transport
{
DotWidget::DotWidget()
: m_pngBuf(1024 * 1024)
{
}
DotWidget::~DotWidget()
{
}
void DotWidget::paintEvent(QPaintEvent*)
{
QPainter painter(this);
painter.fillRect(rect(), Qt::white);
painter.setRenderHint(QPainter::SmoothPixmapTransform);
float scale_x = ((float)width()) / m_pixmap.width();
float scale_y = ((float)height()) / m_pixmap.height();
if(scale_x > scale_y)
{
int newWidth = scale_y * m_pixmap.width();
QRect pixmapRect((width() - newWidth)/2, 0, newWidth, height());
painter.drawPixmap(pixmapRect, m_pixmap);
}
else
{
int newHeight = scale_x * m_pixmap.height();
QRect pixmapRect(0, (height() - newHeight)/2, width(), newHeight);
painter.drawPixmap(pixmapRect, m_pixmap);
}
}
void DotWidget::updateGraph(const std::string& dot)
{
ssize_t pngSize = runDot(dot, width(), height(), m_pngBuf.data(), m_pngBuf.size());
m_pixmap.loadFromData(m_pngBuf.data(), pngSize, "png");
update();
}
}

29
software/rover/nimbro_topic_transport/src/gui/dot_widget.h Normal file
View File

@@ -0,0 +1,29 @@
// Display a .dot graph
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef DOT_WIDGET_H
#define DOT_WIDGET_H
#include <QWidget>
namespace nimbro_topic_transport
{
class DotWidget : public QWidget
{
Q_OBJECT
public:
DotWidget();
virtual ~DotWidget();
void updateGraph(const std::string& dot);
virtual void paintEvent(QPaintEvent *) override;
private:
QPixmap m_pixmap;
std::vector<uint8_t> m_pngBuf;
};
}
#endif

201
software/rover/nimbro_topic_transport/src/gui/topic_gui.cpp Normal file
View File

@@ -0,0 +1,201 @@
// Diagnostic GUI for nimbro_topic_transport
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "topic_gui.h"
#include <QMetaType>
#include <ros/node_handle.h>
#include <set>
#include <pluginlib/class_list_macros.h>
#include <sstream>
Q_DECLARE_METATYPE(nimbro_topic_transport::SenderStatsConstPtr)
Q_DECLARE_METATYPE(nimbro_topic_transport::ReceiverStatsConstPtr)
namespace nimbro_topic_transport
{
TopicGUI::TopicGUI()
{
}
TopicGUI::~TopicGUI()
{
}
void TopicGUI::initPlugin(qt_gui_cpp::PluginContext& ctx)
{
m_w = new DotWidget;
ctx.addWidget(m_w);
qRegisterMetaType<nimbro_topic_transport::SenderStatsConstPtr>();
qRegisterMetaType<nimbro_topic_transport::ReceiverStatsConstPtr>();
connect(
this, SIGNAL(senderStatsReceived(nimbro_topic_transport::SenderStatsConstPtr)),
this, SLOT(handleSenderStats(nimbro_topic_transport::SenderStatsConstPtr)),
Qt::QueuedConnection
);
m_sub_senderStats = getPrivateNodeHandle().subscribe(
"/network/sender_stats", 1, &TopicGUI::senderStatsReceived, this
);
connect(
this, SIGNAL(receiverStatsReceived(nimbro_topic_transport::ReceiverStatsConstPtr)),
this, SLOT(handleReceiverStats(nimbro_topic_transport::ReceiverStatsConstPtr)),
Qt::QueuedConnection
);
m_sub_receiverStats = getPrivateNodeHandle().subscribe(
"/network/receiver_stats", 1, &TopicGUI::receiverStatsReceived, this
);
QTimer* updateTimer = new QTimer(this);
connect(updateTimer, SIGNAL(timeout()), SLOT(update()));
updateTimer->start(2000);
}
void TopicGUI::shutdownPlugin()
{
m_sub_senderStats.shutdown();
m_sub_receiverStats.shutdown();
}
void TopicGUI::handleSenderStats(const SenderStatsConstPtr& msg)
{
ConnectionIdentifier ident;
ident.dest = msg->destination;
ident.protocol = msg->protocol;
ident.sourcePort = msg->source_port;
ident.destPort = msg->destination_port;
m_senderStats[ident] = msg;
update();
}
void TopicGUI::handleReceiverStats(const ReceiverStatsConstPtr& msg)
{
ConnectionIdentifier ident;
ident.dest = msg->host;
ident.protocol = msg->protocol;
ident.sourcePort = msg->remote_port;
ident.destPort = msg->local_port;
m_receiverStats[ident] = msg;
update();
}
static std::string sanitize(std::string arg)
{
for(size_t i = 0; i < arg.length(); ++i)
{
if(!isalnum(arg[i]))
arg[i] = '_';
}
return arg;
}
void TopicGUI::update()
{
ros::Time now = ros::Time::now();
ros::Duration timeoutDuration(4.0);
std::stringstream ss;
ss << "digraph G {\n";
ss << "K=0.6;\n";
std::set<ConnectionIdentifier> connections;
std::set<std::string> hosts;
for(auto pair : m_receiverStats)
{
if(now - pair.second->header.stamp > timeoutDuration)
continue;
connections.insert(pair.first);
hosts.insert(pair.second->remote);
hosts.insert(pair.first.dest);
}
for(auto pair : m_senderStats)
{
if(now - pair.second->header.stamp > timeoutDuration)
continue;
connections.insert(pair.first);
hosts.insert(pair.second->host);
hosts.insert(pair.first.dest);
}
for(auto& host : hosts)
{
ss << "node [ label = \"" << host << "\" ]; node_" << sanitize(host) << ";\n";
}
for(const auto& connection : connections)
{
auto sender_it = m_senderStats.find(connection);
auto receiver_it = m_receiverStats.find(connection);
SenderStatsConstPtr sender;
if(sender_it != m_senderStats.end())
sender = sender_it->second;
ReceiverStatsConstPtr receiver;
if(receiver_it != m_receiverStats.end())
receiver = receiver_it->second;
std::string source = sender ? sender->host : receiver->remote;
ss << "node_" << sanitize(source) << " -> node_" << sanitize(connection.dest) << "[ ";
ss << "label=\"";
std::string label;
if(sender_it != m_senderStats.end() && !sender_it->second->label.empty())
label = sender_it->second->label;
else if(receiver_it != m_receiverStats.end() && !receiver_it->second->label.empty())
label = receiver_it->second->label;
if(!label.empty())
ss << label << "\n";
ss << connection.protocol << " " << connection.sourcePort << " -> " << connection.destPort << "\n";
float bandwidth = 0.0;
int div = 0;
if(sender_it != m_senderStats.end() && now - sender_it->second->header.stamp < timeoutDuration)
{
bandwidth += sender_it->second->bandwidth;
div++;
}
if(receiver_it != m_receiverStats.end() && now - receiver_it->second->header.stamp < timeoutDuration)
{
bandwidth += receiver_it->second->bandwidth;
div++;
}
char bwBuf[100];
snprintf(bwBuf, sizeof(bwBuf), "%.2f MBit/s", bandwidth / 1024 / 1024);
ss << bwBuf;
ss << "\"";
ss << " ];\n";
}
ss << "}";
m_w->updateGraph(ss.str());
}
}
PLUGINLIB_EXPORT_CLASS(nimbro_topic_transport::TopicGUI, rqt_gui_cpp::Plugin)

87
software/rover/nimbro_topic_transport/src/gui/topic_gui.h Normal file
View File

@@ -0,0 +1,87 @@
// Diagnostic GUI for nimbro_topic_transport
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TOPIC_GUI_H
#define TOPIC_GUI_H
#include <ros/subscriber.h>
#include <nimbro_topic_transport/SenderStats.h>
#include <nimbro_topic_transport/ReceiverStats.h>
#include <rqt_gui_cpp/plugin.h>
#include <string>
#include <map>
#include <QTimer>
#include "dot_widget.h"
namespace nimbro_topic_transport
{
struct ConnectionIdentifier
{
std::string dest;
std::string protocol;
int sourcePort;
int destPort;
bool operator==(const ConnectionIdentifier& other) const
{
return dest == other.dest
&& protocol == other.protocol
&& sourcePort == other.sourcePort && destPort == other.destPort;
}
bool operator<(const ConnectionIdentifier& other) const
{
if(dest < other.dest)
return true;
if(dest > other.dest)
return false;
if(protocol < other.protocol)
return true;
if(protocol > other.protocol)
return false;
if(sourcePort < other.sourcePort)
return true;
if(sourcePort > other.sourcePort)
return false;
return destPort < other.destPort;
}
};
class TopicGUI : public rqt_gui_cpp::Plugin
{
Q_OBJECT
public:
TopicGUI();
virtual ~TopicGUI();
virtual void initPlugin(qt_gui_cpp::PluginContext & ) override;
virtual void shutdownPlugin() override;
Q_SIGNALS:
void senderStatsReceived(const nimbro_topic_transport::SenderStatsConstPtr& msg);
void receiverStatsReceived(const nimbro_topic_transport::ReceiverStatsConstPtr& msg);
private Q_SLOTS:
void handleSenderStats(const nimbro_topic_transport::SenderStatsConstPtr& msg);
void handleReceiverStats(const nimbro_topic_transport::ReceiverStatsConstPtr& msg);
void update();
private:
ros::Subscriber m_sub_senderStats;
ros::Subscriber m_sub_receiverStats;
std::map<ConnectionIdentifier, SenderStatsConstPtr> m_senderStats;
std::map<ConnectionIdentifier, ReceiverStatsConstPtr> m_receiverStats;
DotWidget* m_w;
};
}
#endif

105
software/rover/nimbro_topic_transport/src/le_value.h Normal file
View File

@@ -0,0 +1,105 @@
// Little-Endian value
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef LE_VALUE_H
#define LE_VALUE_H
#include <stdint.h>
#include <endian.h>
namespace nimbro_topic_transport
{
/**
* The LEValue template provides automatic endianness correction. The bytes
* in memory are always ordered little-endian.
**/
template<int Width>
class LEValue
{
};
template<>
class LEValue<1>
{
public:
typedef uint8_t Type;
inline operator uint8_t() const
{ return m_value; }
inline uint8_t operator()() const
{ return m_value; }
inline uint8_t operator=(uint8_t value)
{ return m_value = value; }
private:
uint8_t m_value;
} __attribute__((packed));
template<>
class LEValue<2>
{
public:
typedef uint16_t Type;
inline operator uint16_t() const
{ return le16toh(m_value); }
inline uint16_t operator()() const
{ return le16toh(m_value); }
inline uint16_t operator=(uint16_t value)
{
m_value = htole16(value);
return value;
}
private:
uint16_t m_value;
} __attribute__((packed));
template<>
class LEValue<4>
{
public:
typedef uint32_t Type;
inline operator uint32_t() const
{ return le32toh(m_value); }
inline uint32_t operator()() const
{ return le32toh(m_value); }
inline uint32_t operator=(uint32_t value)
{
m_value = htole32(value);
return value;
}
private:
uint32_t m_value;
} __attribute__((packed));
template<>
class LEValue<8>
{
public:
typedef uint64_t Type;
inline operator uint64_t() const
{ return le64toh(m_value); }
inline uint64_t operator()() const
{ return le64toh(m_value); }
inline uint64_t operator=(uint64_t value)
{
m_value = htole64(value);
return value;
}
private:
uint64_t m_value;
} __attribute__((packed));
}
#endif

28
software/rover/nimbro_topic_transport/src/tcp/tcp_packet.h Normal file
View File

@@ -0,0 +1,28 @@
// TCP packet definition
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TCP_PACKET_H
#define TCP_PACKET_H
#include "../le_value.h"
namespace nimbro_topic_transport
{
enum TCPFlag
{
TCP_FLAG_COMPRESSED = (1 << 0)
};
struct TCPHeader
{
LEValue<2> topic_len;
LEValue<2> type_len;
LEValue<4> data_len;
LEValue<4> topic_md5sum[4];
LEValue<4> flags;
} __attribute__((packed));
}
#endif

409
software/rover/nimbro_topic_transport/src/tcp/tcp_receiver.cpp Normal file
View File

@@ -0,0 +1,409 @@
// TCP receiver
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "tcp_receiver.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include "tcp_packet.h"
#include "../topic_info.h"
#include <topic_tools/shape_shifter.h>
#include <bzlib.h>
#include <nimbro_topic_transport/CompressedMsg.h>
namespace nimbro_topic_transport
{
static bool sureRead(int fd, void* dest, ssize_t size)
{
uint8_t* destWPtr = (uint8_t*)dest;
while(size != 0)
{
ssize_t ret = read(fd, destWPtr, size);
if(ret < 0)
{
ROS_ERROR("Could not read(): %s", strerror(errno));
return false;
}
if(ret == 0)
{
// Client has closed connection (ignore silently)
return false;
}
size -= ret;
destWPtr += ret;
}
return true;
}
TCPReceiver::TCPReceiver()
: m_nh("~")
, m_receivedBytesInStatsInterval(0)
{
m_fd = socket(AF_INET, SOCK_STREAM, 0);
if(m_fd < 0)
{
ROS_FATAL("Could not create socket: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
int port;
m_nh.param("port", port, 5050);
sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = htons(port);
int on = 1;
if(setsockopt(m_fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) != 0)
{
ROS_FATAL("Could not enable SO_REUSEADDR: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
ROS_DEBUG("Binding to :%d", port);
if(bind(m_fd, (sockaddr*)&addr, sizeof(addr)) != 0)
{
ROS_FATAL("Could not bind socket: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
if(listen(m_fd, 10) != 0)
{
ROS_FATAL("Could not listen: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
m_nh.param("keep_compressed", m_keepCompressed, false);
char hostnameBuf[256];
gethostname(hostnameBuf, sizeof(hostnameBuf));
hostnameBuf[sizeof(hostnameBuf)-1] = 0;
m_stats.node = ros::this_node::getName();
m_stats.protocol = "TCP";
m_stats.host = hostnameBuf;
m_stats.local_port = port;
m_stats.fec = false;
m_nh.param("label", m_stats.label, std::string());
m_pub_stats = m_nh.advertise<ReceiverStats>("/network/receiver_stats", 1);
m_statsInterval = ros::WallDuration(2.0);
m_statsTimer = m_nh.createWallTimer(m_statsInterval,
boost::bind(&TCPReceiver::updateStats, this)
);
m_nh.param("topic_prefix", m_topicPrefix, std::string());
}
TCPReceiver::~TCPReceiver()
{
}
void TCPReceiver::run()
{
fd_set fds;
while(ros::ok())
{
ros::spinOnce();
// Clean up any exited threads
std::list<ClientHandler*>::iterator it = m_handlers.begin();
while(it != m_handlers.end())
{
if(!(*it)->isRunning())
{
delete *it;
it = m_handlers.erase(it);
}
else
it++;
}
FD_ZERO(&fds);
FD_SET(m_fd, &fds);
timeval timeout;
timeout.tv_usec = 0;
timeout.tv_sec = 1;
int ret = select(m_fd+1, &fds, 0, 0, &timeout);
if(ret < 0)
{
if(errno == EINTR || errno == EAGAIN)
continue;
ROS_ERROR("Could not select(): %s", strerror(errno));
throw std::runtime_error("Could not select");
}
if(ret == 0)
continue;
sockaddr_storage remoteAddr;
socklen_t remoteAddrLen = sizeof(remoteAddr);
int client_fd = accept(m_fd, (sockaddr*)&remoteAddr, &remoteAddrLen);
{
// Perform reverse lookup
char nameBuf[256];
char serviceBuf[256];
ros::WallTime startLookup = ros::WallTime::now();
if(getnameinfo((sockaddr*)&remoteAddr, remoteAddrLen, nameBuf, sizeof(nameBuf), serviceBuf, sizeof(serviceBuf), NI_NUMERICSERV) == 0)
{
ROS_INFO("New remote: %s:%s", nameBuf, serviceBuf);
m_stats.remote = nameBuf;
m_stats.remote_port = atoi(serviceBuf);
}
else
{
ROS_ERROR("Could not resolve remote address to name");
m_stats.remote = "unknown";
m_stats.remote_port = -1;
}
ros::WallTime endLookup = ros::WallTime::now();
// Warn if lookup takes up time (otherwise the user does not know
// what is going on)
if(endLookup - startLookup > ros::WallDuration(1.0))
{
ROS_WARN("Reverse address lookup took more than a second. "
"Consider adding '%s' to /etc/hosts",
m_stats.remote.c_str()
);
}
}
ClientHandler* handler = new ClientHandler(client_fd, m_topicPrefix);
handler->setKeepCompressed(m_keepCompressed);
m_handlers.push_back(handler);
}
}
TCPReceiver::ClientHandler::ClientHandler(int fd, const std::string& topicPrefix)
: m_fd(fd)
, m_uncompressBuf(1024)
, m_running(true)
, m_keepCompressed(false)
, m_topicPrefix(topicPrefix)
{
m_thread = boost::thread(boost::bind(&ClientHandler::start, this));
}
TCPReceiver::ClientHandler::~ClientHandler()
{
close(m_fd);
}
class VectorStream
{
public:
VectorStream(std::vector<uint8_t>* vector)
: m_vector(vector)
{}
inline const uint8_t* getData()
{ return m_vector->data(); }
inline size_t getLength()
{ return m_vector->size(); }
private:
std::vector<uint8_t>* m_vector;
};
void TCPReceiver::ClientHandler::start()
{
run();
m_running = false;
}
void TCPReceiver::ClientHandler::run()
{
while(1)
{
TCPHeader header;
if(!sureRead(m_fd, &header, sizeof(header)))
return;
std::vector<char> buf(header.topic_len + 1);
if(!sureRead(m_fd, buf.data(), header.topic_len))
return;
buf[buf.size()-1] = 0;
std::string topic(buf.data());
buf.resize(header.type_len+1);
if(!sureRead(m_fd, buf.data(), header.type_len))
return;
buf[buf.size()-1] = 0;
std::string type(buf.data());
std::string md5;
topic_info::unpackMD5(header.topic_md5sum, &md5);
std::vector<uint8_t> data(header.data_len);
if(!sureRead(m_fd, data.data(), header.data_len))
return;
ROS_DEBUG("Got msg with flags: %d", header.flags());
if(m_keepCompressed && (header.flags() & TCP_FLAG_COMPRESSED))
{
CompressedMsg compressed;
compressed.type = type;
memcpy(compressed.md5.data(), header.topic_md5sum, sizeof(header.topic_md5sum));
compressed.data.swap(data);
std::map<std::string, ros::Publisher>::iterator it = m_pub.find(topic);
if(it == m_pub.end())
{
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<CompressedMsg>(m_topicPrefix + topic, 2);
m_pub[topic] = pub;
pub.publish(compressed);
}
else
it->second.publish(compressed);
}
else
{
topic_tools::ShapeShifter shifter;
if(header.flags() & TCP_FLAG_COMPRESSED)
{
int ret = 0;
unsigned int len = m_uncompressBuf.size();
while(1)
{
ret = BZ2_bzBuffToBuffDecompress((char*)m_uncompressBuf.data(), &len, (char*)data.data(), data.size(), 0, 0);
if(ret == BZ_OUTBUFF_FULL)
{
len = 4 * m_uncompressBuf.size();
ROS_INFO("Increasing buffer size to %d KiB", (int)len / 1024);
m_uncompressBuf.resize(len);
continue;
}
else
break;
}
if(ret != BZ_OK)
{
ROS_ERROR("Could not decompress bz2 data (reason %d), dropping msg", ret);
continue;
}
ROS_DEBUG("decompress %d KiB (%d) to %d KiB (%d)", (int)data.size() / 1024, (int)data.size(), (int)len / 1024, (int)len);
m_uncompressBuf.resize(len);
VectorStream stream(&m_uncompressBuf);
shifter.read(stream);
}
else
{
VectorStream stream(&data);
shifter.read(stream);
}
ROS_DEBUG("Got message from topic '%s' (type '%s', md5 '%s')", topic.c_str(), type.c_str(), md5.c_str());
shifter.morph(md5, type, "", "");
std::map<std::string, ros::Publisher>::iterator it = m_pub.find(topic);
if(it == m_pub.end())
{
ROS_DEBUG("Advertising new topic '%s'", (m_topicPrefix + topic).c_str());
std::string msgDef = topic_info::getMsgDef(type);
ros::NodeHandle nh;
ros::AdvertiseOptions options(
m_topicPrefix + topic,
2,
md5,
type,
topic_info::getMsgDef(type)
);
// It will take subscribers some time to connect to our publisher.
// Therefore, latch messages so they will not be lost.
// No, this is often unexpected. Instead, wait before publishing.
// options.latch = true;
m_pub[topic] = nh.advertise(options);
it = m_pub.find(topic);
sleep(1);
}
it->second.publish(shifter);
}
uint8_t ack = 1;
if(write(m_fd, &ack, 1) != 1)
{
ROS_ERROR("Could not write(): %s", strerror(errno));
return;
}
}
}
bool TCPReceiver::ClientHandler::isRunning() const
{
return m_running;
}
void TCPReceiver::updateStats()
{
m_stats.header.stamp = ros::Time::now();
uint64_t totalBytes = 0;
for(auto handler : m_handlers)
{
totalBytes += handler->bytesReceived();
handler->resetByteCounter();
}
m_stats.bandwidth = totalBytes / m_statsInterval.toSec();
m_stats.drop_rate = 0;
// If there is no connection yet, drop the stats msg
if(m_handlers.empty())
return;
m_pub_stats.publish(m_stats);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "tcp_receiver");
nimbro_topic_transport::TCPReceiver recv;
recv.run();
return 0;
}

72
software/rover/nimbro_topic_transport/src/tcp/tcp_receiver.h Normal file
View File

@@ -0,0 +1,72 @@
// TCP receiver
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TCP_RECEIVER_H
#define TCP_RECEIVER_H
#include <list>
#include <ros/node_handle.h>
#include <boost/thread.hpp>
#include <nimbro_topic_transport/ReceiverStats.h>
namespace nimbro_topic_transport
{
class TCPReceiver
{
public:
TCPReceiver();
~TCPReceiver();
void run();
private:
class ClientHandler
{
public:
ClientHandler(int fd, const std::string& topicPrefix);
~ClientHandler();
void start();
void run();
void setKeepCompressed(bool keep)
{ m_keepCompressed = keep; }
bool isRunning() const;
inline uint64_t bytesReceived() const
{ return m_bytesReceived; }
inline void resetByteCounter()
{ m_bytesReceived = 0; }
private:
int m_fd;
boost::thread m_thread;
std::map<std::string, ros::Publisher> m_pub;
std::vector<uint8_t> m_uncompressBuf;
bool m_running;
bool m_keepCompressed;
uint64_t m_bytesReceived;
std::string m_topicPrefix;
};
void updateStats();
int m_fd;
std::list<ClientHandler*> m_handlers;
ros::NodeHandle m_nh;
bool m_keepCompressed;
nimbro_topic_transport::ReceiverStats m_stats;
uint64_t m_receivedBytesInStatsInterval;
ros::Publisher m_pub_stats;
ros::WallDuration m_statsInterval;
ros::WallTimer m_statsTimer;
std::string m_topicPrefix;
};
}
#endif

406
software/rover/nimbro_topic_transport/src/tcp/tcp_sender.cpp Normal file
View File

@@ -0,0 +1,406 @@
// TCP sender
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "tcp_sender.h"
#include "../topic_info.h"
#include <bzlib.h>
#include <netinet/tcp.h>
#include <boost/algorithm/string/replace.hpp>
#include "ros/message_traits.h"
#include <netdb.h>
namespace nimbro_topic_transport
{
TCPSender::TCPSender()
: m_nh("~")
, m_fd(-1)
, m_sentBytesInStatsInterval(0)
{
std::string addr;
if(!m_nh.getParam("destination_addr", addr) && !m_nh.getParam("address", addr))
{
ROS_FATAL("tcp_sender needs an 'destination_addr' parameter!");
throw std::runtime_error("tcp_sender needs an 'destination_addr' parameter!");
}
int port;
if(!m_nh.getParam("destination_port", port) && !m_nh.getParam("port", port))
{
ROS_FATAL("tcp_sender needs a 'destination_port' parameter!");
throw std::runtime_error("tcp_sender needs a 'destination_port' parameter!");
}
std::string portStr = boost::lexical_cast<std::string>(port);
if(m_nh.hasParam("source_port"))
{
if(!m_nh.getParam("source_port", m_sourcePort))
{
ROS_FATAL("Invalid source_port");
throw std::runtime_error("Invalid source port");
}
}
else
m_sourcePort = -1;
addrinfo* info = 0;
if(getaddrinfo(addr.c_str(), portStr.c_str(), 0, &info) != 0 || !info)
{
ROS_FATAL("Could not lookup host name '%s'", addr.c_str());
throw std::runtime_error("Could not lookup hostname");
}
m_addrFamily = info->ai_family;
memcpy(&m_addr, info->ai_addr, info->ai_addrlen);
m_addrLen = info->ai_addrlen;
freeaddrinfo(info);
XmlRpc::XmlRpcValue list;
m_nh.getParam("topics", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
for(int32_t i = 0; i < list.size(); ++i)
{
XmlRpc::XmlRpcValue& entry = list[i];
ROS_ASSERT(entry.getType() == XmlRpc::XmlRpcValue::TypeStruct);
ROS_ASSERT(entry.hasMember("name"));
std::string topic = entry["name"];
int flags = 0;
if(entry.hasMember("compress") && ((bool)entry["compress"]) == true)
flags |= TCP_FLAG_COMPRESSED;
boost::function<void(const ros::MessageEvent<topic_tools::ShapeShifter const>&)> func;
func = boost::bind(&TCPSender::messageCallback, this, topic, flags, _1);
ros::SubscribeOptions options;
options.initByFullCallbackType<const ros::MessageEvent<topic_tools::ShapeShifter const>&>(topic, 20, func);
if(entry.hasMember("type"))
{
std::string type = entry["type"];
std::string md5 = topic_info::getMd5Sum(type);
if(md5.empty())
{
ROS_ERROR("Could not get md5 sum of topic type '%s'", type.c_str());
continue;
}
options.datatype = type;
options.md5sum = md5;
}
m_subs.push_back(m_nh.subscribe(options));
#if WITH_CONFIG_SERVER
bool enabled = true;
if (entry.hasMember("enable"))
enabled = entry["enable"];
std::string parameterName(topic);
boost::replace_first(parameterName, "/", "");
boost::replace_all(parameterName, "/", "_");
boost::shared_ptr<config_server::Parameter<bool>> parameter( new config_server::Parameter<bool>(parameterName, enabled));
m_enableTopic[topic] = parameter;
#endif
}
if (m_nh.hasParam("ignored_publishers")) {
m_nh.getParam("ignored_publishers", m_ignoredPubs);
for (std::vector<std::string>::iterator ignoredPublisher = m_ignoredPubs.begin();
ignoredPublisher != m_ignoredPubs.end(); ignoredPublisher++) {
*ignoredPublisher = ros::names::resolve(*ignoredPublisher);
}
}
char hostnameBuf[256];
gethostname(hostnameBuf, sizeof(hostnameBuf));
hostnameBuf[sizeof(hostnameBuf)-1] = 0;
m_stats.node = ros::this_node::getName();
m_stats.protocol = "TCP";
m_stats.host = hostnameBuf;
m_stats.destination = addr;
m_stats.destination_port = port;
m_stats.source_port = m_sourcePort;
m_stats.fec = false;
m_nh.param("label", m_stats.label, std::string());
m_pub_stats = m_nh.advertise<SenderStats>("/network/sender_stats", 1);
for(auto& pair : m_topicSendBytesInStatsInteral)
pair.second = 0;
m_statsInterval = ros::WallDuration(2.0);
m_statsTimer = m_nh.createWallTimer(m_statsInterval,
boost::bind(&TCPSender::updateStats, this)
);
m_statsTimer.start();
}
TCPSender::~TCPSender()
{
}
bool TCPSender::connect()
{
m_fd = socket(m_addrFamily, SOCK_STREAM, 0);
if(m_fd < 0)
{
ROS_ERROR("Could not create socket: %s", strerror(errno));
return false;
}
if(m_sourcePort != -1)
{
std::string source_port_str = boost::lexical_cast<std::string>(m_sourcePort);
addrinfo hints;
memset(&hints, 0, sizeof(hints));
hints.ai_flags = AI_PASSIVE;
hints.ai_family = m_addrFamily;
hints.ai_socktype = SOCK_STREAM;
addrinfo* localInfo = 0;
if(getaddrinfo(NULL, source_port_str.c_str(), &hints, &localInfo) != 0 || !localInfo)
{
ROS_FATAL("Could not get local address: %s", strerror(errno));
throw std::runtime_error("Could not get local address");
}
if(bind(m_fd, localInfo->ai_addr, localInfo->ai_addrlen) != 0)
{
ROS_FATAL("Could not bind to source port: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
freeaddrinfo(localInfo);
}
if(::connect(m_fd, (sockaddr*)&m_addr, m_addrLen) != 0)
{
ROS_ERROR("Could not connect: %s", strerror(errno));
return false;
}
if(m_sourcePort == -1)
{
sockaddr_storage addr;
socklen_t addrlen = sizeof(addr);
char nameBuf[256];
char serviceBuf[256];
if(getsockname(m_fd, (sockaddr*)&addr, &addrlen) != 0)
{
ROS_ERROR("Could not get local socket addr: %s", strerror(errno));
}
if(getnameinfo((sockaddr*)&addr, addrlen, nameBuf, sizeof(nameBuf), serviceBuf, sizeof(serviceBuf), NI_NUMERICSERV | NI_NUMERICHOST) != 0)
{
ROS_ERROR("Could not resolve remote address to name");
}
m_stats.source_port = atoi(serviceBuf);
}
#ifdef TCP_USER_TIMEOUT
int timeout = 8000;
if(setsockopt(m_fd, SOL_TCP, TCP_USER_TIMEOUT, &timeout, sizeof(timeout)) != 0)
{
ROS_ERROR("Could not set TCP_USER_TIMEOUT: %s", strerror(errno));
return false;
}
#else
ROS_WARN("Not setting TCP_USER_TIMEOUT");
#endif
return true;
}
class PtrStream
{
public:
PtrStream(uint8_t* ptr)
: m_ptr(ptr)
{}
inline uint8_t* advance(int)
{ return m_ptr; }
private:
uint8_t* m_ptr;
};
void TCPSender::messageCallback(const std::string& topic, int flags,
const ros::MessageEvent<topic_tools::ShapeShifter const>& event)
{
#if WITH_CONFIG_SERVER
if (! (*m_enableTopic[topic])() )
return;
#endif
// check if the message wasn't published by an ignored publisher
std::string const &messagePublisher = event.getConnectionHeader()["callerid"];
if (std::find(m_ignoredPubs.begin(), m_ignoredPubs.end(), messagePublisher) != m_ignoredPubs.end())
return;
send(topic, flags, event.getMessage());
}
void TCPSender::send(const std::string& topic, int flags, const topic_tools::ShapeShifter::ConstPtr& shifter)
{
#if WITH_CONFIG_SERVER
if (! (*m_enableTopic[topic])() )
return;
#endif
std::string type = shifter->getDataType();
std::string md5 = shifter->getMD5Sum();
uint32_t size = shifter->size();
uint32_t maxDataSize = size;
if(flags & TCP_FLAG_COMPRESSED)
maxDataSize = size + size / 100 + 1200; // taken from bzip2 docs
m_packet.resize(
sizeof(TCPHeader) + topic.length() + type.length() + maxDataSize
);
TCPHeader* header = (TCPHeader*)m_packet.data();
// Fill in topic & type
uint8_t* wptr = m_packet.data() + sizeof(TCPHeader);
memcpy(wptr, topic.c_str(), topic.length());
wptr += topic.length();
memcpy(wptr, type.c_str(), type.length());
wptr += type.length();
if(flags & TCP_FLAG_COMPRESSED)
{
unsigned int len = m_packet.size() - (wptr - m_packet.data());
m_compressionBuf.resize(shifter->size());
PtrStream stream(m_compressionBuf.data());
shifter->write(stream);
if(BZ2_bzBuffToBuffCompress((char*)wptr, &len, (char*)m_compressionBuf.data(), m_compressionBuf.size(), 7, 0, 30) == BZ_OK)
{
header->data_len = len;
wptr += len;
size = len;
}
else
{
ROS_ERROR("Could not compress with bzip2 library, sending uncompressed");
flags &= ~TCP_FLAG_COMPRESSED;
memcpy(wptr, m_compressionBuf.data(), m_compressionBuf.size());
header->data_len = m_compressionBuf.size();
wptr += m_compressionBuf.size();
}
}
else
{
PtrStream stream(wptr);
shifter->write(stream);
header->data_len = size;
wptr += size;
}
header->topic_len = topic.length();
header->type_len = type.length();
header->data_len = size;
header->flags = flags;
topic_info::packMD5(md5, header->topic_md5sum);
// Resize to final size
m_packet.resize(
wptr - m_packet.data()
);
// Try to send the packet
for(int tries = 0; tries < 10; ++tries)
{
if(m_fd == -1)
{
if(!connect())
{
ROS_WARN("Connection failed, trying again");
continue;
}
}
if(write(m_fd, m_packet.data(), m_packet.size()) != (int)m_packet.size())
{
ROS_WARN("Could not send data, trying again");
close(m_fd);
m_fd = -1;
continue;
}
m_sentBytesInStatsInterval += m_packet.size();
m_topicSendBytesInStatsInteral[topic] += m_packet.size();
// Read ACK
uint8_t ack;
if(read(m_fd, &ack, 1) != 1)
{
ROS_WARN("Could not read ACK, sending again (!)");
close(m_fd);
m_fd = -1;
continue;
}
return;
}
ROS_ERROR("Could not send TCP packet. Dropping message from topic %s!", topic.c_str());
}
void TCPSender::updateStats()
{
m_stats.header.stamp = ros::Time::now();
m_stats.bandwidth = 8 * m_sentBytesInStatsInterval / m_statsInterval.toSec();
m_stats.topics.clear();
for(auto& pair : m_topicSendBytesInStatsInteral)
{
nimbro_topic_transport::TopicBandwidth tp;
tp.name = pair.first;
tp.bandwidth = pair.second / m_statsInterval.toSec();
pair.second = 0;
m_stats.topics.push_back(tp);
}
m_pub_stats.publish(m_stats);
m_sentBytesInStatsInterval = 0;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "tcp_sender");
nimbro_topic_transport::TCPSender sender;
ros::spin();
return 0;
}

67
software/rover/nimbro_topic_transport/src/tcp/tcp_sender.h Normal file
View File

@@ -0,0 +1,67 @@
// TCP sender
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TCP_SENDER_H
#define TCP_SENDER_H
#include <ros/node_handle.h>
#include <topic_tools/shape_shifter.h>
#include <arpa/inet.h>
#include "tcp_packet.h"
#if WITH_CONFIG_SERVER
#include <config_server/parameter.h>
#endif
#include <map>
#include "ros/message_event.h"
#include <nimbro_topic_transport/SenderStats.h>
namespace nimbro_topic_transport
{
class TCPSender
{
public:
TCPSender();
~TCPSender();
bool connect();
void send(const std::string& topic, int flags, const topic_tools::ShapeShifter::ConstPtr& shifter);
void messageCallback(const std::string& topic, int flags,
const ros::MessageEvent<topic_tools::ShapeShifter const>& shifter);
private:
void updateStats();
ros::NodeHandle m_nh;
int m_fd;
int m_addrFamily;
sockaddr_storage m_addr;
socklen_t m_addrLen;
int m_sourcePort;
std::vector<ros::Subscriber> m_subs;
std::vector<uint8_t> m_packet;
std::vector<uint8_t> m_compressionBuf;
std::vector<std::string> m_ignoredPubs;
#if WITH_CONFIG_SERVER
std::map<std::string, boost::shared_ptr<config_server::Parameter<bool>>> m_enableTopic;
#endif
nimbro_topic_transport::SenderStats m_stats;
ros::Publisher m_pub_stats;
ros::WallDuration m_statsInterval;
ros::WallTimer m_statsTimer;
uint64_t m_sentBytesInStatsInterval;
std::map<std::string, uint64_t> m_topicSendBytesInStatsInteral;
};
}
#endif

124
software/rover/nimbro_topic_transport/src/topic_info.cpp Normal file
View File

@@ -0,0 +1,124 @@
// Provides information about topic types
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "topic_info.h"
#include <ros/names.h>
#include <sys/wait.h>
#include <stdio.h>
#include <unistd.h>
#include <string>
namespace nimbro_topic_transport
{
namespace topic_info
{
/**
* Execute a rosmsg query and return result.
*
* The executed command is "rosmsg @a cmd @a type".
*
* @param stripNL If true, strip off the final newline.
* @return rosmsg output
**/
static std::string msgQuery(const std::string& cmd, const std::string& type, bool stripNL)
{
std::vector<char> buf(1024);
int idx = 0;
std::string error;
if(!ros::names::validate(type, error))
{
ROS_WARN("Got invalid message type '%s'", type.c_str());
return "";
}
int fds[2];
if(pipe(fds) != 0)
throw std::runtime_error("Could not create pipe");
int pid = fork();
if(pid == 0)
{
close(fds[0]);
dup2(fds[1], STDOUT_FILENO);
if(execlp("rosmsg", "rosmsg", cmd.c_str(), type.c_str(), 0) != 0)
{
throw std::runtime_error("Could not execlp() rosmsg");
}
}
close(fds[1]);
while(1)
{
buf.resize(idx + 1024);
int size = read(fds[0], buf.data() + idx, 1024);
if(size < 0)
throw std::runtime_error("Could not read()");
else if(size == 0)
break;
idx += size;
}
close(fds[0]);
int status;
waitpid(pid, &status, 0);
if(!WIFEXITED(status) || WEXITSTATUS(status) != 0 || idx == 0)
{
ROS_WARN("Could not get rosmsg %s for type '%s'", cmd.c_str(), type.c_str());
return "";
}
std::string ret(buf.data(), idx);
if(stripNL)
return std::string(buf.data(), idx-1);
else
return std::string(buf.data(), idx);
}
std::string getMsgDef(const std::string& type)
{
return msgQuery("show", type, false);
}
std::string getMd5Sum(const std::string& type)
{
return msgQuery("md5", type, true);
}
void packMD5(const std::string& str, LEValue< 4 >* dest)
{
for(int i = 0; i < 4; ++i)
{
std::string md5_part = str.substr(8*i, 8);
uint32_t md5_num = strtoll(md5_part.c_str(), 0, 16);
dest[i] = md5_num;
}
}
void unpackMD5(const LEValue< 4 >* src, std::string* dest)
{
dest->clear();
for(int i = 0; i < 4; ++i)
{
char buf[10];
snprintf(buf, sizeof(buf), "%08x", src[i]());
*dest += buf;
}
}
}
}

45
software/rover/nimbro_topic_transport/src/topic_info.h Normal file
View File

@@ -0,0 +1,45 @@
// Provides information about topic types
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TOPIC_INFO_H
#define TOPIC_INFO_H
#include "le_value.h"
#include <string>
namespace nimbro_topic_transport
{
namespace topic_info
{
/**
* Get the message definition string (from the .msg file)
*
* @return msg definition, or empty on failure (unknown type, etc)
**/
std::string getMsgDef(const std::string& type);
/**
* Get the message md5 string
*
* @return md5 sum (32 chars), or empty on failure (unknown type, etc)
**/
std::string getMd5Sum(const std::string& type);
/**
* md5 string -> LEValue<4>
**/
void packMD5(const std::string& str, LEValue<4>* dest);
/**
* LEValue<4> -> md5 string
**/
void unpackMD5(const LEValue<4>* src, std::string* dest);
}
}
#endif

166
software/rover/nimbro_topic_transport/src/udp/topic_receiver.cpp Normal file
View File

@@ -0,0 +1,166 @@
// Topic receiver (part of udp_receiver)
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "topic_receiver.h"
#include <bzlib.h>
namespace nimbro_topic_transport
{
bool Message::decompress(Message* dest)
{
unsigned int destLen = 1024;
dest->payload.resize(destLen);
while(1)
{
int ret = BZ2_bzBuffToBuffDecompress((char*)dest->payload.data(), &destLen, (char*)payload.data(), payload.size(), 0, 0);
if(ret == BZ_OUTBUFF_FULL)
{
destLen *= 2;
dest->payload.resize(destLen);
continue;
}
if(ret != BZ_OK)
{
ROS_ERROR("Could not decompress message");
return false;
}
break;
}
dest->payload.resize(destLen);
dest->header = header;
dest->id = id;
dest->size = destLen;
return true;
}
TopicReceiver::TopicReceiver()
: waiting_for_subscriber(true)
, m_decompressionThreadRunning(false)
{
}
TopicReceiver::~TopicReceiver()
{
{
boost::unique_lock<boost::mutex> lock(m_mutex);
m_decompressionThreadShouldExit = true;
}
m_cond.notify_one();
}
void TopicReceiver::takeForDecompression(const boost::shared_ptr<Message>& msg)
{
if(!m_decompressionThreadRunning)
{
m_decompressionThreadShouldExit = false;
m_decompressionThread = boost::thread(boost::bind(&TopicReceiver::decompress, this));
m_decompressionThreadRunning = true;
}
boost::unique_lock<boost::mutex> lock(m_mutex);
m_compressedMsg = msg;
m_cond.notify_one();
}
void TopicReceiver::decompress()
{
while(1)
{
boost::shared_ptr<Message> currentMessage;
{
boost::unique_lock<boost::mutex> lock(m_mutex);
while(!m_compressedMsg && !m_decompressionThreadShouldExit)
m_cond.wait(lock);
if(m_decompressionThreadShouldExit)
return;
currentMessage = m_compressedMsg;
m_compressedMsg.reset();
}
Message decompressed;
currentMessage->decompress(&decompressed);
boost::shared_ptr<topic_tools::ShapeShifter> shapeShifter(new topic_tools::ShapeShifter);
shapeShifter->morph(md5_str, decompressed.header.topic_type, msg_def, "");
shapeShifter->read(decompressed);
publish(shapeShifter);
}
}
void TopicReceiver::publish(const boost::shared_ptr<topic_tools::ShapeShifter>& msg)
{
if(!waiting_for_subscriber)
{
publisher.publish(msg);
}
else
{
m_msgInQueue = msg;
m_queueTime = ros::WallTime::now();
}
}
void TopicReceiver::publishCompressed(const CompressedMsgConstPtr& msg)
{
if(!waiting_for_subscriber)
{
publisher.publish(msg);
}
else
{
m_compressedMsgInQueue = msg;
m_queueTime = ros::WallTime::now();
}
}
void TopicReceiver::handleSubscriber()
{
if(!waiting_for_subscriber)
return;
if(!m_compressedMsg && !m_msgInQueue)
{
// No msg arrived before the first subscriber
waiting_for_subscriber = false;
return;
}
ros::WallTime now = ros::WallTime::now();
if(now - m_queueTime > ros::WallDuration(1.0))
{
// Message to old, drop it
waiting_for_subscriber = false;
m_msgInQueue.reset();
m_compressedMsgInQueue.reset();
return;
}
if(m_msgInQueue)
{
publisher.publish(m_msgInQueue);
m_msgInQueue.reset();
}
else if(m_compressedMsgInQueue)
{
publisher.publish(m_compressedMsgInQueue);
m_compressedMsgInQueue.reset();
}
waiting_for_subscriber = false;
}
}

111
software/rover/nimbro_topic_transport/src/udp/topic_receiver.h Normal file
View File

@@ -0,0 +1,111 @@
// Topic receiver (part of udp_receiver)
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TOPIC_RECEIVER_H
#define TOPIC_RECEIVER_H
#include <stdint.h>
#include <stdlib.h>
#include <vector>
#include <ros/publisher.h>
#include <topic_tools/shape_shifter.h>
#include <nimbro_topic_transport/CompressedMsg.h>
#include <boost/thread.hpp>
#if WITH_OPENFEC
extern "C"
{
#include <of_openfec_api.h>
}
#endif
#include "udp_packet.h"
namespace nimbro_topic_transport
{
struct Message
{
Message(uint16_t id)
: id(id)
, size(0)
, complete(false)
{}
Message()
{}
~Message()
{
}
uint32_t getLength() const
{ return size; }
uint8_t* getData()
{ return payload.data(); }
bool decompress(Message* dest);
uint16_t id;
UDPFirstPacket::Header header;
std::vector<uint8_t> payload;
size_t size;
std::vector<bool> msgs;
bool complete;
#if WITH_OPENFEC
boost::shared_ptr<of_session_t> decoder;
boost::shared_ptr<of_parameters_t> params;
unsigned int received_symbols;
std::vector<boost::shared_ptr<std::vector<uint8_t>>> fecPackets;
#endif
};
struct TopicReceiver
{
TopicReceiver();
~TopicReceiver();
ros::Publisher publisher;
uint32_t md5[4];
std::string md5_str;
std::string msg_def;
bool compressed;
int last_message_counter;
bool waiting_for_subscriber;
void takeForDecompression(const boost::shared_ptr<Message>& compressed);
void publish(const boost::shared_ptr<topic_tools::ShapeShifter>& msg);
void publishCompressed(const CompressedMsgConstPtr& msg);
void handleSubscriber();
private:
boost::shared_ptr<Message> m_compressedMsg;
boost::condition_variable m_cond;
boost::mutex m_mutex;
boost::thread m_decompressionThread;
bool m_decompressionThreadRunning;
bool m_decompressionThreadShouldExit;
boost::shared_ptr<topic_tools::ShapeShifter> m_msgInQueue;
CompressedMsgConstPtr m_compressedMsgInQueue;
ros::WallTime m_queueTime;
void decompress();
};
}
#endif

476
software/rover/nimbro_topic_transport/src/udp/topic_sender.cpp Normal file
View File

@@ -0,0 +1,476 @@
// Sends a single topic
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "topic_sender.h"
#include "udp_sender.h"
#include "udp_packet.h"
#include "../topic_info.h"
#include <bzlib.h>
#include <nimbro_topic_transport/CompressedMsg.h>
#include <boost/algorithm/string/replace.hpp>
#include <random>
#include <algorithm>
#include <chrono>
#if WITH_OPENFEC
extern "C"
{
#include <of_openfec_api.h>
}
#endif
#include <fcntl.h>
namespace nimbro_topic_transport
{
TopicSender::TopicSender(UDPSender* sender, ros::NodeHandle* nh, const std::string& topic, double rate, bool resend, int flags, bool enable, const std::string& type)
: m_sender(sender)
, m_flags(flags)
, m_updateBuf(true)
, m_msgCounter(0)
, m_inputMsgCounter(0)
, m_directTransmission(true)
#if WITH_CONFIG_SERVER
, m_enable(escapeTopicName(topic), enable)
#endif
{
ros::SubscribeOptions ops;
boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> func = boost::bind(&TopicSender::handleData, this, _1);
ops.initByFullCallbackType(topic, 20, func);
if(!type.empty())
{
ops.datatype = type;
ops.md5sum = topic_info::getMd5Sum(type);
}
m_subscriber = nh->subscribe(ops);
m_topicName = topic;
if(rate == 0.0)
m_durationBetweenPackets = ros::Duration(0.0);
else
m_durationBetweenPackets = ros::Duration(1.0 / rate);
if(resend)
{
m_resendTimer = nh->createTimer(m_durationBetweenPackets, boost::bind(&TopicSender::resend, this));
m_resendTimer.start();
}
}
TopicSender::~TopicSender()
{
ROS_DEBUG("Topic '%s': Sent %d messages", m_topicName.c_str(), m_msgCounter);
}
void TopicSender::send()
{
if(m_updateBuf)
{
boost::lock_guard<boost::mutex> lock(m_dataMutex);
if(!m_lastData)
return;
// If the data was sent over our CompressedMsg format, do not recompress it
if((m_flags & UDP_FLAG_COMPRESSED) && m_lastData->getDataType() == "nimbro_topic_transport/CompressedMsg")
{
CompressedMsg::Ptr compressed = m_lastData->instantiate<CompressedMsg>();
if(!compressed)
{
ROS_ERROR("Could not instantiate CompressedMsg");
return;
}
m_buf.swap(compressed->data);
memcpy(m_md5, compressed->md5.data(), sizeof(m_md5));
m_topicType = compressed->type;
}
else
{
m_buf.resize(m_lastData->size());
m_lastData->write(*this);
if(m_flags & UDP_FLAG_COMPRESSED)
{
unsigned int len = m_buf.size() + m_buf.size() / 100 + 1200;
m_compressionBuf.resize(len);
int ret = BZ2_bzBuffToBuffCompress((char*)m_compressionBuf.data(), &len, (char*)m_buf.data(), m_buf.size(), 3, 0, 30);
if(ret == BZ_OK)
{
m_buf.swap(m_compressionBuf);
m_buf.resize(len);
}
else
{
ROS_ERROR("Could not compress data, sending uncompressed");
}
}
std::string md5 = m_lastData->getMD5Sum();
for(int i = 0; i < 4; ++i)
{
std::string md5_part = md5.substr(8*i, 8);
uint32_t md5_num = strtoll(md5_part.c_str(), 0, 16);
m_md5[i] = md5_num;
}
m_topicType = m_lastData->getDataType();
}
m_updateBuf = false;
}
// Do we want to do forward error correction?
if(m_sender->fec() != 0.0)
{
sendWithFEC();
}
else
{
sendWithoutFEC();
}
m_msgCounter++;
}
inline uint64_t div_round_up(uint64_t a, uint64_t b)
{
return (a + b - 1) / b;
}
void TopicSender::sendWithFEC()
{
#if WITH_OPENFEC
uint16_t msg_id = m_sender->allocateMessageID();
uint64_t dataSize = sizeof(FECHeader) + m_buf.size();
// If the message fits in a single packet, use that as the buffer size
uint64_t symbolSize;
uint64_t sourceSymbols;
if(dataSize <= FECPacket::MaxDataSize)
{
sourceSymbols = 1;
symbolSize = dataSize;
}
else
{
// We need to pad the data to a multiple of our packet payload size.
sourceSymbols = div_round_up(dataSize, FECPacket::MaxDataSize);
symbolSize = FECPacket::MaxDataSize;
}
ROS_DEBUG("dataSize: %lu, symbol size: %lu, sourceSymbols: %lu", dataSize, symbolSize, sourceSymbols);
uint64_t packetSize = sizeof(FECPacket::Header) + symbolSize;
ROS_DEBUG("=> packetSize: %lu", packetSize);
uint64_t repairSymbols = std::ceil(m_sender->fec() * sourceSymbols);
uint64_t numPackets = sourceSymbols + repairSymbols;
of_session_t* ses = 0;
uint32_t prng_seed = rand();
if(sourceSymbols >= MIN_PACKETS_LDPC)
{
ROS_DEBUG("%s: Choosing LDPC-Staircase codec", m_topicName.c_str());
if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_ENCODER, 1) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not create LDPC codec instance", m_topicName.c_str());
return;
}
of_ldpc_parameters_t params;
params.nb_source_symbols = sourceSymbols;
params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
params.encoding_symbol_length = symbolSize;
params.prng_seed = prng_seed;
params.N1 = 7;
ROS_DEBUG("LDPC seed: 7, 0x%X", params.prng_seed);
if(of_set_fec_parameters(ses, (of_parameters_t*)&params) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
else
{
ROS_DEBUG("%s: Choosing Reed-Solomon codec", m_topicName.c_str());
if(of_create_codec_instance(&ses, OF_CODEC_REED_SOLOMON_GF_2_M_STABLE, OF_ENCODER, 0) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not create REED_SOLOMON codec instance", m_topicName.c_str());
return;
}
of_rs_2_m_parameters params;
params.nb_source_symbols = sourceSymbols;
params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
params.encoding_symbol_length = symbolSize;
params.m = 8;
if(of_set_fec_parameters(ses, (of_parameters_t*)&params) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
std::vector<uint8_t> packetBuffer(numPackets * packetSize);
std::vector<void*> symbols(sourceSymbols + repairSymbols);
uint64_t writtenData = 0;
// Fill the source packets
for(uint64_t i = 0; i < sourceSymbols; ++i)
{
uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
header->msg_id = msg_id;
header->symbol_id = i;
header->symbol_length = symbolSize;
header->source_symbols = sourceSymbols;
header->repair_symbols = repairSymbols;
header->prng_seed = prng_seed;
uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
uint64_t remainingSpace = symbolSize;
symbols[i] = dataPtr;
if(i == 0)
{
// First packet includes the FECHeader
FECHeader* msgHeader = reinterpret_cast<FECHeader*>(dataPtr);
// Fill in header fields
msgHeader->flags = m_flags;
msgHeader->topic_msg_counter = m_inputMsgCounter;
strncpy(msgHeader->topic_name, m_topicName.c_str(), sizeof(msgHeader->topic_name));
if(msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] != 0)
{
ROS_ERROR("Topic '%s' is too long. Please shorten the name.", m_topicName.c_str());
msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] = 0;
}
strncpy(msgHeader->topic_type, m_topicType.c_str(), sizeof(msgHeader->topic_type));
if(msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] != 0)
{
ROS_ERROR("Topic type '%s' is too long. Please shorten the name.", m_topicType.c_str());
msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] = 0;
}
for(int i = 0; i < 4; ++i)
msgHeader->topic_md5[i] = m_md5[i];
dataPtr += sizeof(FECHeader);
remainingSpace -= sizeof(FECHeader);
}
uint64_t chunkSize = std::min(remainingSpace, m_buf.size() - writtenData);
memcpy(dataPtr, m_buf.data() + writtenData, chunkSize);
writtenData += chunkSize;
// Set any padding to zero
if(chunkSize < remainingSpace)
memset(dataPtr + chunkSize, 0, remainingSpace - chunkSize);
}
// Fill the repair packets
for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
{
uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
header->msg_id = msg_id;
header->symbol_id = i;
header->symbol_length = symbolSize;
header->source_symbols = sourceSymbols;
header->repair_symbols = repairSymbols;
header->prng_seed = prng_seed;
uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
symbols[i] = dataPtr;
}
for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
{
if(of_build_repair_symbol(ses, symbols.data(), i) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not build repair symbol", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
// FEC work is done
of_release_codec_instance(ses);
std::vector<unsigned int> packetOrder(numPackets);
std::iota(packetOrder.begin(), packetOrder.end(), 0);
// Send the packets in random order
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
std::mt19937 mt(seed);
std::shuffle(packetOrder.begin(), packetOrder.end(), mt);
ROS_DEBUG("Sending %d packets", (int)packetOrder.size());
for(unsigned int idx : packetOrder)
{
if(!m_sender->send(packetBuffer.data() + idx * packetSize, packetSize, m_topicName))
return;
}
#else
throw std::runtime_error("Forward error correction requested, but I was not compiled with FEC support...");
#endif
}
void TopicSender::sendWithoutFEC()
{
uint32_t size = m_buf.size();
uint8_t buf[PACKET_SIZE];
uint32_t buf_size = std::min<uint32_t>(PACKET_SIZE, sizeof(UDPFirstPacket) + size);
UDPFirstPacket* first = (UDPFirstPacket*)buf;
uint16_t msg_id = m_sender->allocateMessageID();
first->header.frag_id = 0;
first->header.msg_id = msg_id;
first->header.flags = m_flags;
first->header.topic_msg_counter = m_inputMsgCounter;
// Calculate number of packets
first->header.remaining_packets = std::max<uint32_t>(0,
(size - UDPFirstPacket::MaxDataSize + (UDPDataPacket::MaxDataSize-1)) / UDPDataPacket::MaxDataSize
);
strncpy(first->header.topic_name, m_topicName.c_str(), sizeof(first->header.topic_name));
if(first->header.topic_name[sizeof(first->header.topic_name)-1] != 0)
{
ROS_ERROR("Topic '%s' is too long. Please shorten the name.", m_topicName.c_str());
first->header.topic_name[sizeof(first->header.topic_name)-1] = 0;
}
strncpy(first->header.topic_type, m_topicType.c_str(), sizeof(first->header.topic_type));
if(first->header.topic_type[sizeof(first->header.topic_type)-1] != 0)
{
ROS_ERROR("Topic type '%s' is too long. Please shorten the name.", m_topicType.c_str());
first->header.topic_type[sizeof(first->header.topic_type)-1] = 0;
}
for(int i = 0; i < 4; ++i)
first->header.topic_md5[i] = m_md5[i];
uint8_t* rptr = m_buf.data();
uint32_t psize = std::min<uint32_t>(UDPFirstPacket::MaxDataSize, size);
memcpy(first->data, rptr, psize);
rptr += psize;
size -= psize;
if(!m_sender->send(buf, buf_size, m_topicName))
return;
if(m_sender->duplicateFirstPacket())
{
if(!m_sender->send(buf, buf_size, m_topicName))
return;
}
uint16_t frag_id = 1;
while(size > 0)
{
buf_size = std::min<uint32_t>(PACKET_SIZE, sizeof(UDPDataPacket) + size);
UDPDataPacket* next_packet = (UDPDataPacket*)buf;
next_packet->header.frag_id = frag_id++;
next_packet->header.msg_id = msg_id;
psize = std::min<uint32_t>(UDPDataPacket::MaxDataSize, size);
memcpy(next_packet->data, rptr, psize);
rptr += psize;
size -= psize;
if(!m_sender->send(buf, buf_size, m_topicName))
return;
}
}
void TopicSender::handleData(const topic_tools::ShapeShifter::ConstPtr& shapeShifter)
{
#if WITH_CONFIG_SERVER
if (!m_enable() )
return;
#endif
{
boost::lock_guard<boost::mutex> lock(m_dataMutex);
m_lastData = shapeShifter;
m_updateBuf = true;
ros::Time now = ros::Time::now();
if(now - m_lastTime < m_durationBetweenPackets)
return;
m_lastTime = now;
m_inputMsgCounter++;
}
if(m_directTransmission)
send();
}
void TopicSender::resend()
{
if(!m_lastData)
return;
ros::Time now = ros::Time::now();
if(now - m_lastTime < m_durationBetweenPackets)
return;
sendCurrentMessage();
}
void TopicSender::sendCurrentMessage()
{
if(!m_lastData)
return;
send();
}
void TopicSender::setDirectTransmissionEnabled(bool value)
{
m_directTransmission = value;
if(m_directTransmission && m_resendTimer.isValid())
m_resendTimer.start();
else
m_resendTimer.stop();
}
std::string TopicSender::escapeTopicName(std::string topicName)
{
boost::replace_first(topicName, "/", "");
boost::replace_all(topicName, "/", "_");
return topicName;
}
}

73
software/rover/nimbro_topic_transport/src/udp/topic_sender.h Normal file
View File

@@ -0,0 +1,73 @@
// Sends a single topic
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef TOPIC_SENDER_H
#define TOPIC_SENDER_H
#include <ros/subscriber.h>
#include <topic_tools/shape_shifter.h>
#include <boost/thread.hpp>
#if WITH_CONFIG_SERVER
#include <config_server/parameter.h>
#endif
namespace nimbro_topic_transport
{
class UDPSender;
class TopicSender
{
public:
TopicSender(UDPSender* sender, ros::NodeHandle* nh, const std::string& topic, double rate, bool resend, int flags, bool enable = true, const std::string& type = "");
~TopicSender();
void handleData(const topic_tools::ShapeShifter::ConstPtr& shapeShifter);
inline uint8_t* advance(int)
{ return m_buf.data(); }
bool isDirectTransmissionEnabled() const
{ return m_directTransmission; }
void setDirectTransmissionEnabled(bool value);
void sendCurrentMessage();
private:
void send();
void resend();
void sendWithFEC();
void sendWithoutFEC();
static std::string escapeTopicName(std::string topicName);
UDPSender* m_sender;
ros::Subscriber m_subscriber;
std::string m_topicName;
std::string m_topicType;
int m_flags;
std::vector<uint8_t> m_buf;
uint32_t m_md5[4];
std::vector<uint8_t> m_compressionBuf;
ros::Duration m_durationBetweenPackets;
ros::Time m_lastTime;
ros::Timer m_resendTimer;
topic_tools::ShapeShifter::ConstPtr m_lastData;
bool m_updateBuf;
unsigned int m_msgCounter;
unsigned int m_inputMsgCounter;
bool m_directTransmission;
boost::mutex m_dataMutex;
#if WITH_CONFIG_SERVER
config_server::Parameter<bool> m_enable;
#endif
};
};
#endif

104
software/rover/nimbro_topic_transport/src/udp/udp_packet.h Normal file
View File

@@ -0,0 +1,104 @@
// UDP packet definition
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef UDP_PACKET_H
#define UDP_PACKET_H
#include "../le_value.h"
namespace nimbro_topic_transport
{
// Since we sometimes tunnel nimbro_network packets through QuickTun,
// leave space for one additional UDP/IP envelope.
const int PACKET_SIZE = 1500 - 20 - 8 - 20 - 8;
// const int PACKET_SIZE = 1024 + 12; // for symbol size == 1024
enum PacketType
{
PACKET_INFO,
PACKET_DATA
};
enum UDPFlag
{
UDP_FLAG_COMPRESSED = (1 << 0),
UDP_FLAG_RELAY_MODE = (1 << 1)
};
struct UDPGenericPacket
{
LEValue<2> frag_id;
LEValue<2> msg_id;
} __attribute__((packed));
struct UDPFirstPacket
{
struct Header
{
LEValue<2> frag_id;
LEValue<2> msg_id;
char topic_name[64];
char topic_type[64];
LEValue<4> topic_md5[4];
LEValue<2> remaining_packets;
LEValue<2> flags;
LEValue<2> topic_msg_counter;
} __attribute__((packed));
enum { MaxDataSize = PACKET_SIZE - sizeof(Header) };
Header header;
uint8_t data[];
} __attribute__((packed));
struct UDPDataPacket
{
struct Header
{
LEValue<2> frag_id;
LEValue<2> msg_id;
} __attribute__((packed));
enum { MaxDataSize = PACKET_SIZE - sizeof(Header) };
Header header;
uint8_t data[];
} __attribute__((packed));
// Minimum number of packets for choosing the LDPC-Staircase algorithm
const int MIN_PACKETS_LDPC = 255;
struct FECHeader
{
char topic_name[64];
char topic_type[64];
LEValue<4> topic_md5[4];
LEValue<2> flags;
LEValue<2> topic_msg_counter;
uint8_t data[];
} __attribute__((packed));
struct FECPacket
{
struct Header
{
LEValue<2> msg_id;
LEValue<4> symbol_id;
LEValue<2> symbol_length;
LEValue<2> source_symbols;
LEValue<2> repair_symbols;
LEValue<4> prng_seed;
} __attribute__((packed));
enum { MaxDataSize = PACKET_SIZE - sizeof(Header) };
Header header;
uint8_t data[];
} __attribute__((packed));
}
#endif

715
software/rover/nimbro_topic_transport/src/udp/udp_receiver.cpp Normal file
View File

@@ -0,0 +1,715 @@
// UDP receiver node
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "udp_receiver.h"
#include "udp_packet.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <ros/console.h>
#include <ros/node_handle.h>
#include <topic_tools/shape_shifter.h>
#include <std_msgs/Time.h>
#include <stdio.h>
#include <ros/names.h>
#include "../topic_info.h"
#include <nimbro_topic_transport/CompressedMsg.h>
#if WITH_PLOTTING
# include <plot_msgs/Plot.h>
#endif
#include <fcntl.h>
namespace nimbro_topic_transport
{
UDPReceiver::UDPReceiver()
: m_receivedBytesInStatsInterval(0)
, m_expectedPacketsInStatsInterval(0)
, m_missingPacketsInStatsInterval(0)
, m_remoteAddrLen(0)
{
ros::NodeHandle nh("~");
m_pub_heartbeat = nh.advertise<std_msgs::Time>("heartbeat", 1);
#if WITH_PLOTTING
m_pub_plot = nh.advertise<plot_msgs::Plot>("/plot", 1000);
#endif
m_fd = socket(AF_INET, SOCK_DGRAM, 0);
if(m_fd < 0)
{
ROS_FATAL("Could not create socket: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
int port;
nh.param("port", port, 5050);
sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = htons(port);
ROS_INFO("Binding to :%d", port);
if(bind(m_fd, (sockaddr*)&addr, sizeof(addr)) != 0)
{
ROS_FATAL("Could not bind socket: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
int on = 1;
if(setsockopt(m_fd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) != 0)
{
ROS_FATAL("Could not set broadcast flag: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
nh.param("drop_repeated_msgs", m_dropRepeatedMessages, true);
nh.param("warn_drop_incomplete", m_warnDropIncomplete, true);
nh.param("keep_compressed", m_keepCompressed, false);
nh.param("fec", m_fec, false);
#if !(WITH_OPENFEC)
if(m_fec)
throw std::runtime_error("Please compile with FEC support to enable FEC");
#endif
char hostnameBuf[256];
gethostname(hostnameBuf, sizeof(hostnameBuf));
hostnameBuf[sizeof(hostnameBuf)-1] = 0;
m_stats.node = ros::this_node::getName();
m_stats.protocol = "UDP";
m_stats.host = hostnameBuf;
m_stats.local_port = port;
m_stats.fec = m_fec;
nh.param("label", m_stats.label, std::string());
m_pub_stats = nh.advertise<ReceiverStats>("/network/receiver_stats", 1);
m_statsInterval = ros::WallDuration(2.0);
m_statsTimer = nh.createWallTimer(m_statsInterval,
boost::bind(&UDPReceiver::updateStats, this)
);
nh.param("topic_prefix", m_topicPrefix, std::string());
}
UDPReceiver::~UDPReceiver()
{
}
template<class HeaderType>
void UDPReceiver::handleFinishedMessage(Message* msg, HeaderType* header)
{
if(msg->complete)
return;
// Packet complete
msg->complete = true;
// Enforce termination
header->topic_type[sizeof(header->topic_type)-1] = 0;
header->topic_name[sizeof(header->topic_name)-1] = 0;
ROS_DEBUG("Got a packet of type %s, topic %s, (msg id %d), size %d", header->topic_type, header->topic_name, msg->id, (int)msg->payload.size());
// Find topic
TopicMap::iterator topic_it = m_topics.find(header->topic_name);
boost::shared_ptr<TopicReceiver> topic;
if(topic_it == m_topics.end())
{
m_topics.insert(std::pair<std::string, boost::shared_ptr<TopicReceiver>>(
header->topic_name,
boost::make_shared<TopicReceiver>()
));
topic = m_topics[header->topic_name];
topic->last_message_counter = -1;
}
else
topic = topic_it->second;
// Send heartbeat message
ros::Time now = ros::Time::now();
if(now - m_lastHeartbeatTime > ros::Duration(0.2))
{
std_msgs::Time time;
time.data = now;
m_pub_heartbeat.publish(time);
m_lastHeartbeatTime = now;
}
#if WITH_PLOTTING
// Plot packet size
plot_msgs::Plot plot;
plot.header.stamp = ros::Time::now();
plot_msgs::PlotPoint point;
std::string safe_topic = header->topic_name;
std::replace(safe_topic.begin(), safe_topic.end(), '/', '_');
point.name = "/udp_receiver/mbyte/" + safe_topic;
point.value = ((double)msg->getLength()) / 1024 / 1024;
plot.points.push_back(point);
m_pub_plot.publish(plot);
#endif
if(m_dropRepeatedMessages && header->topic_msg_counter() == topic->last_message_counter)
{
// This is the same message, probably sent in relay mode
return;
}
bool compressed = header->flags & UDP_FLAG_COMPRESSED;
// Compare md5
if(topic->last_message_counter == -1 || memcmp(topic->md5, header->topic_md5, sizeof(topic->md5)) != 0 || (m_keepCompressed && topic->compressed != compressed))
{
topic->msg_def = topic_info::getMsgDef(header->topic_type);
topic->md5_str = topic_info::getMd5Sum(header->topic_type);
if(topic->msg_def.empty() || topic->md5_str.size() != 8*4)
{
ROS_ERROR("Could not find msg type '%s', please make sure msg definitions are up to date", header->topic_type);
return;
}
ROS_INFO("Received first message on topic '%s'", header->topic_name);
for(int i = 0; i < 4; ++i)
{
std::string md5_part = topic->md5_str.substr(8*i, 8);
uint32_t md5_num = strtoll(md5_part.c_str(), 0, 16);
topic->md5[i] = md5_num;
}
if(memcmp(topic->md5, header->topic_md5, sizeof(topic->md5)) != 0)
{
ROS_ERROR("Invalid md5 sum for topic type '%s', please make sure msg definitions are up to date", header->topic_type);
return;
}
if(m_keepCompressed && compressed)
{
// If we are requested to keep the messages compressed, we advertise our compressed msg type
topic->publisher = m_nh.advertise<CompressedMsg>(
m_topicPrefix + header->topic_name,
1,
boost::bind(&TopicReceiver::handleSubscriber, topic)
);
}
else
{
// ... otherwise, we advertise the native type
ros::AdvertiseOptions options(
m_topicPrefix + header->topic_name,
1,
topic->md5_str,
header->topic_type,
topic->msg_def,
boost::bind(&TopicReceiver::handleSubscriber, topic)
);
// Latching is often unexpected. Better to lose the first msg.
//options.latch = 1;
topic->publisher = m_nh.advertise(options);
}
topic->compressed = compressed;
}
if(compressed && m_keepCompressed)
{
CompressedMsgPtr compressed(new CompressedMsg);
compressed->type = header->topic_type;
memcpy(compressed->md5.data(), topic->md5, sizeof(topic->md5));
compressed->data.swap(msg->payload);
topic->publishCompressed(compressed);
}
else if(header->flags & UDP_FLAG_COMPRESSED)
topic->takeForDecompression(boost::make_shared<Message>(*msg));
else
{
boost::shared_ptr<topic_tools::ShapeShifter> shapeShifter(new topic_tools::ShapeShifter);
shapeShifter->morph(topic->md5_str, header->topic_type, topic->msg_def, "");
shapeShifter->read(*msg);
topic->publish(shapeShifter);
}
topic->last_message_counter = header->topic_msg_counter();
}
void UDPReceiver::run()
{
std::vector<uint8_t> buf;
ROS_INFO("UDP receiver ready");
while(1)
{
ros::spinOnce();
// handleMessagePacket() below might swap() our buffer out, so make
// sure that we have enough room.
buf.resize(PACKET_SIZE);
fd_set fds;
FD_ZERO(&fds);
FD_SET(m_fd, &fds);
timeval timeout;
timeout.tv_usec = 50 * 1000;
timeout.tv_sec = 0;
int ret = select(m_fd+1, &fds, 0, 0, &timeout);
if(ret < 0)
{
if(errno == EINTR || errno == EAGAIN)
continue;
ROS_FATAL("Could not select(): %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
if(ret == 0)
continue;
sockaddr_storage addr;
socklen_t addrlen = sizeof(addr);
ssize_t size = recvfrom(m_fd, buf.data(), buf.size(), 0, (sockaddr*)&addr, &addrlen);
if(size < 0)
{
ROS_FATAL("Could not recv(): %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
if(addrlen != m_remoteAddrLen || memcmp(&addr, &m_remoteAddr, addrlen) != 0)
{
// Perform reverse lookup
char nameBuf[256];
char serviceBuf[256];
ros::WallTime startLookup = ros::WallTime::now();
if(getnameinfo((sockaddr*)&addr, addrlen, nameBuf, sizeof(nameBuf), serviceBuf, sizeof(serviceBuf), NI_NUMERICSERV) == 0)
{
ROS_INFO("New remote: %s:%s", nameBuf, serviceBuf);
m_stats.remote = nameBuf;
m_stats.remote_port = atoi(serviceBuf);
}
else
{
ROS_ERROR("Could not resolve remote address to name");
m_stats.remote = "unknown";
m_stats.remote_port = -1;
}
ros::WallTime endLookup = ros::WallTime::now();
// Warn if lookup takes up time (otherwise the user does not know
// what is going on)
if(endLookup - startLookup > ros::WallDuration(1.0))
{
ROS_WARN("Reverse address lookup took more than a second. "
"Consider adding '%s' to /etc/hosts",
m_stats.remote.c_str()
);
}
m_remoteAddr = addr;
m_remoteAddrLen = addrlen;
}
ROS_DEBUG("packet of size %lu", size);
m_receivedBytesInStatsInterval += size;
uint16_t msg_id;
// Obtain the message ID from the packet
if(m_fec)
{
FECPacket::Header* header = (FECPacket::Header*)buf.data();
msg_id = header->msg_id();
}
else
{
UDPGenericPacket* generic = (UDPGenericPacket*)buf.data();
msg_id = generic->msg_id();
}
// Look up the message ID in our list of incomplete messages
MessageBuffer::iterator it = std::find_if(m_incompleteMessages.begin(), m_incompleteMessages.end(),
[=](const Message& msg) { return msg.id == msg_id; }
);
if(it == m_incompleteMessages.end())
{
// Insert a new message
m_incompleteMessages.push_front(Message(msg_id));
it = m_incompleteMessages.begin();
pruneMessages();
}
handleMessagePacket(it, &buf, size);
}
}
void UDPReceiver::updateStats()
{
m_stats.header.stamp = ros::Time::now();
m_stats.bandwidth = m_receivedBytesInStatsInterval / m_statsInterval.toSec();
m_stats.drop_rate = ((double)m_missingPacketsInStatsInterval) / m_expectedPacketsInStatsInterval;
m_pub_stats.publish(m_stats);
// Reset all stats counters
m_receivedBytesInStatsInterval = 0;
m_missingPacketsInStatsInterval = 0;
m_expectedPacketsInStatsInterval = 0;
}
void UDPReceiver::pruneMessages()
{
// Erase messages that are too old (after index 31)
MessageBuffer::iterator itr = m_incompleteMessages.begin();
MessageBuffer::iterator it_end = m_incompleteMessages.end();
for(int i = 0; i < 31; ++i)
{
itr++;
if(itr == it_end)
break;
}
// Collect statistics on packets which will be deleted
for(MessageBuffer::iterator itd = itr; itd != it_end; ++itd)
{
const Message& msg = *itd;
#if WITH_OPENFEC
if(m_fec)
{
if(msg.params)
{
int total = msg.params->nb_source_symbols + msg.params->nb_repair_symbols;
m_expectedPacketsInStatsInterval += total;
m_missingPacketsInStatsInterval += total - msg.received_symbols;
}
}
else
#endif
{
int num_fragments = msg.msgs.size();
int received = 0;
for(unsigned int i = 0; i < msg.msgs.size(); ++i)
{
if(msg.msgs[i])
received++;
}
m_expectedPacketsInStatsInterval += num_fragments;
m_missingPacketsInStatsInterval += num_fragments - received;
}
}
// If enabled, warn each time we drop an incomplete message
if(m_warnDropIncomplete)
{
for(MessageBuffer::iterator itd = itr; itd != it_end; ++itd)
{
const Message& msg = *itd;
if(msg.complete)
continue;
int num_fragments = msg.msgs.size();
int received = 0;
for(unsigned int i = 0; i < msg.msgs.size(); ++i)
{
if(msg.msgs[i])
received++;
}
#if WITH_OPENFEC
if(msg.decoder)
{
ROS_WARN("Dropping FEC message %d (%u/%u symbols)", msg.id, msg.received_symbols, msg.params->nb_source_symbols);
}
else
#endif
{
ROS_WARN("Dropping message %d, %.2f%% of fragments received (%d/%d)",
msg.id, 100.0 * received / num_fragments, received, num_fragments
);
}
}
}
// Finally, delete all messages after index 31
m_incompleteMessages.erase(itr, it_end);
}
void UDPReceiver::handleMessagePacket(MessageBuffer::iterator it, std::vector<uint8_t>* buf, std::size_t size)
{
Message* msg = &*it;
// If the message is already completed (happens especially with FEC),
// drop this packet.
if(msg->complete)
{
#if WITH_OPENFEC
// ... but still include it in the statistics!
msg->received_symbols++;
#endif
return;
}
if(m_fec)
{
#if WITH_OPENFEC
// Save the received packet (OpenFEC expects all symbols to stay
// available until end of decoding)
boost::shared_ptr<std::vector<uint8_t>> fecBuffer(new std::vector<uint8_t>);
fecBuffer->swap(*buf);
msg->fecPackets.push_back(fecBuffer);
FECPacket* packet = (FECPacket*)fecBuffer->data();
if(!msg->decoder)
{
of_session_t* ses = 0;
of_parameters_t* params = 0;
if(packet->header.source_symbols() >= MIN_PACKETS_LDPC)
{
if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_DECODER, 1) != OF_STATUS_OK)
{
ROS_ERROR("Could not create LDPC decoder");
return;
}
of_ldpc_parameters_t* ldpc_params = (of_ldpc_parameters_t*)malloc(sizeof(of_ldpc_parameters_t));
ldpc_params->nb_source_symbols = packet->header.source_symbols();
ldpc_params->nb_repair_symbols = packet->header.repair_symbols();
ldpc_params->encoding_symbol_length = packet->header.symbol_length();
ldpc_params->prng_seed = packet->header.prng_seed();
ldpc_params->N1 = 7;
ROS_DEBUG("LDPC parameters: %d, %d, %d, 0x%X, %d", ldpc_params->nb_source_symbols, ldpc_params->nb_repair_symbols, ldpc_params->encoding_symbol_length, ldpc_params->prng_seed, ldpc_params->N1);
params = (of_parameters*)ldpc_params;
}
else
{
if(of_create_codec_instance(&ses, OF_CODEC_REED_SOLOMON_GF_2_M_STABLE, OF_DECODER, 1) != OF_STATUS_OK)
{
ROS_ERROR("Could not create REED_SOLOMON decoder");
return;
}
of_rs_2_m_parameters* rs_params = (of_rs_2_m_parameters_t*)malloc(sizeof(of_rs_2_m_parameters_t));
rs_params->nb_source_symbols = packet->header.source_symbols();
rs_params->nb_repair_symbols = packet->header.repair_symbols();
rs_params->encoding_symbol_length = packet->header.symbol_length();
rs_params->m = 8;
params = (of_parameters_t*)rs_params;
}
if(of_set_fec_parameters(ses, params) != OF_STATUS_OK)
{
ROS_ERROR("Could not set FEC parameters");
of_release_codec_instance(ses);
return;
}
msg->decoder.reset(ses, of_release_codec_instance);
msg->params.reset(params, free);
msg->received_symbols = 0;
}
msg->received_symbols++;
ROS_DEBUG("msg: %10d, symbol: %10d/%10d", msg->id, packet->header.symbol_id(), packet->header.source_symbols());
uint8_t* symbol_begin = packet->data;
if(size - sizeof(FECPacket::Header) != msg->params->encoding_symbol_length)
{
ROS_ERROR("Symbol size mismatch: got %d, expected %d",
(int)(size - sizeof(FECPacket::Header)),
(int)(msg->params->encoding_symbol_length)
);
return;
}
// FEC iterative decoding
if(of_decode_with_new_symbol(msg->decoder.get(), symbol_begin, packet->header.symbol_id()) != OF_STATUS_OK)
{
ROS_ERROR("Could not decode symbol");
return;
}
bool done = false;
if(msg->received_symbols >= msg->params->nb_source_symbols)
{
// Are we finished using the iterative decoding already?
done = of_is_decoding_complete(msg->decoder.get());
// As it is implemented in OpenFEC right now, we can only
// try the ML decoding (gaussian elimination) *once*.
// After that the internal state is screwed up and the message
// has to be discarded.
// So we have to be sure that it's worth it ;-)
if(!done && msg->received_symbols >= msg->params->nb_source_symbols + msg->params->nb_repair_symbols / 2)
{
of_status_t ret = of_finish_decoding(msg->decoder.get());
if(ret == OF_STATUS_OK)
done = true;
else
{
ROS_ERROR("ML decoding failed, dropping message...");
msg->complete = true;
return;
}
}
}
if(done)
{
ROS_DEBUG("FEC: Decoding done!");
std::vector<void*> symbols(msg->params->nb_source_symbols, 0);
if(of_get_source_symbols_tab(msg->decoder.get(), symbols.data()) != OF_STATUS_OK)
{
ROS_ERROR("Could not get decoded symbols");
return;
}
uint64_t payloadLength = msg->params->nb_source_symbols * msg->params->encoding_symbol_length;
if(msg->params->encoding_symbol_length < sizeof(FECHeader) || payloadLength < sizeof(FECHeader))
{
ROS_ERROR("Invalid short payload");
m_incompleteMessages.erase(it);
return;
}
FECHeader msgHeader;
memcpy(&msgHeader, symbols[0], sizeof(FECHeader));
payloadLength -= sizeof(FECHeader);
msg->payload.resize(payloadLength);
uint8_t* writePtr = msg->payload.data();
memcpy(
msg->payload.data(),
((uint8_t*)symbols[0]) + sizeof(FECHeader),
msg->params->encoding_symbol_length - sizeof(FECHeader)
);
writePtr += msg->params->encoding_symbol_length - sizeof(FECHeader);
for(unsigned int symbol = 1; symbol < msg->params->nb_source_symbols; ++symbol)
{
memcpy(writePtr, symbols[symbol], msg->params->encoding_symbol_length);
writePtr += msg->params->encoding_symbol_length;
}
msg->size = payloadLength;
handleFinishedMessage(msg, &msgHeader);
// keep completed messages in the buffer so that we know that
// we have to ignore any additional symbols of that message.
}
#endif
}
else
{
UDPGenericPacket* generic = (UDPGenericPacket*)buf->data();
if(generic->frag_id == 0)
{
UDPFirstPacket* first = (UDPFirstPacket*)buf->data();
msg->header = first->header;
// We can calculate an approximate size now
uint32_t required_size = (msg->header.remaining_packets()+1) * PACKET_SIZE;
uint32_t my_size = size - sizeof(UDPFirstPacket);
if(msg->payload.size() < required_size)
msg->payload.resize(required_size);
memcpy(msg->payload.data(), first->data, my_size);
if(msg->size < my_size)
msg->size = my_size;
if(((uint16_t)msg->msgs.size()) < msg->header.remaining_packets()+1)
msg->msgs.resize(msg->header.remaining_packets()+1, false);
}
else
{
UDPDataPacket* data = (UDPDataPacket*)buf->data();
uint32_t offset = UDPFirstPacket::MaxDataSize + (data->header.frag_id-1) * UDPDataPacket::MaxDataSize;
uint32_t required_size = offset + size - sizeof(UDPDataPacket);
if(msg->payload.size() < required_size)
msg->payload.resize(required_size);
memcpy(msg->payload.data() + offset, data->data, size - sizeof(UDPDataPacket));
if(msg->size < required_size)
msg->size = required_size;
}
if(generic->frag_id >= msg->msgs.size())
msg->msgs.resize(generic->frag_id+1, false);
msg->msgs[generic->frag_id] = true;
if(std::all_of(msg->msgs.begin(), msg->msgs.end(), [](bool x){return x;}))
{
handleFinishedMessage(msg, &msg->header);
// as we delete the message from the buffer here, immediately
// add it to the statistics
m_expectedPacketsInStatsInterval += msg->msgs.size();
m_incompleteMessages.erase(it);
}
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "udp_receiver", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
nimbro_topic_transport::UDPReceiver recv;
recv.run();
return 0;
}

84
software/rover/nimbro_topic_transport/src/udp/udp_receiver.h Normal file
View File

@@ -0,0 +1,84 @@
// UDP receiver node
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef UDP_RECEIVER_H
#define UDP_RECEIVER_H
#include <map>
#include <vector>
#include <string>
#include <list>
#include <sys/socket.h>
#include <ros/publisher.h>
#include <ros/wall_timer.h>
#include <boost/thread.hpp>
#include <nimbro_topic_transport/ReceiverStats.h>
#include <nimbro_topic_transport/CompressedMsg.h>
#include "udp_packet.h"
#include "topic_receiver.h"
namespace nimbro_topic_transport
{
class UDPReceiver
{
public:
UDPReceiver();
~UDPReceiver();
void run();
private:
typedef std::map<std::string, boost::shared_ptr<TopicReceiver>> TopicMap;
typedef std::list<Message> MessageBuffer;
void handleMessagePacket(MessageBuffer::iterator it, std::vector<uint8_t>* buf, std::size_t size);
template<class HeaderType>
void handleFinishedMessage(Message* msg, HeaderType* header);
void pruneMessages();
void updateStats();
int m_fd;
MessageBuffer m_incompleteMessages;
TopicMap m_topics;
bool m_dropRepeatedMessages;
bool m_warnDropIncomplete;
bool m_keepCompressed;
ros::NodeHandle m_nh;
ros::Publisher m_pub_heartbeat;
ros::Time m_lastHeartbeatTime;
ros::Publisher m_pub_plot;
Message m_decompressedMessage;
bool m_fec;
nimbro_topic_transport::ReceiverStats m_stats;
uint64_t m_receivedBytesInStatsInterval;
uint64_t m_expectedPacketsInStatsInterval;
uint64_t m_missingPacketsInStatsInterval;
ros::Publisher m_pub_stats;
ros::WallDuration m_statsInterval;
ros::WallTimer m_statsTimer;
sockaddr_storage m_remoteAddr;
socklen_t m_remoteAddrLen;
std::string m_topicPrefix;
};
}
#endif

351
software/rover/nimbro_topic_transport/src/udp/udp_sender.cpp Normal file
View File

@@ -0,0 +1,351 @@
// UDP sender node
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include "udp_sender.h"
#include "topic_sender.h"
#include "udp_packet.h"
#include <ros/init.h>
#include <ros/node_handle.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <sys/timerfd.h>
#include <netdb.h>
#include <ros/console.h>
#include <stdio.h>
#include <errno.h>
#include <XmlRpcValue.h>
#include <signal.h>
#include <nimbro_topic_transport/SenderStats.h>
namespace nimbro_topic_transport
{
UDPSender::UDPSender()
: m_msgID(0)
, m_sentBytesInStatsInterval(0)
{
ros::NodeHandle nh("~");
nh.param("relay_mode", m_relayMode, false);
std::string dest_host;
nh.param("destination_addr", dest_host, std::string("localhost"));
int dest_port;
nh.param("destination_port", dest_port, 5050);
std::string dest_port_str = boost::lexical_cast<std::string>(dest_port);
addrinfo *info = 0;
if(getaddrinfo(dest_host.c_str(), dest_port_str.c_str(), 0, &info) != 0 || !info)
{
ROS_FATAL("Could not lookup destination address\n '%s': %s",
dest_host.c_str(), strerror(errno)
);
throw std::runtime_error(strerror(errno));
}
m_fd = socket(info->ai_family, SOCK_DGRAM, 0);
if(m_fd < 0)
{
ROS_FATAL("Could not create socket: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
int on = 1;
if(setsockopt(m_fd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) != 0)
{
ROS_FATAL("Could not enable SO_BROADCAST flag: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
memcpy(&m_addr, info->ai_addr, info->ai_addrlen);
m_addrLen = info->ai_addrlen;
int source_port = dest_port;
if(nh.hasParam("source_port"))
{
if(!nh.getParam("source_port", source_port))
{
ROS_FATAL("Invalid source_port");
throw std::runtime_error("Invalid source port");
}
std::string source_port_str = boost::lexical_cast<std::string>(source_port);
addrinfo hints;
memset(&hints, 0, sizeof(hints));
hints.ai_flags = AI_PASSIVE;
hints.ai_family = info->ai_family;
hints.ai_socktype = SOCK_DGRAM;
addrinfo* localInfo = 0;
if(getaddrinfo(NULL, source_port_str.c_str(), &hints, &localInfo) != 0 || !localInfo)
{
ROS_FATAL("Could not get local address: %s", strerror(errno));
throw std::runtime_error("Could not get local address");
}
if(bind(m_fd, localInfo->ai_addr, localInfo->ai_addrlen) != 0)
{
ROS_FATAL("Could not bind to source port: %s", strerror(errno));
throw std::runtime_error(strerror(errno));
}
freeaddrinfo(localInfo);
}
freeaddrinfo(info);
nh.param("fec", m_fec, 0.0);
XmlRpc::XmlRpcValue list;
nh.getParam("topics", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
for(int32_t i = 0; i < list.size(); ++i)
{
ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
ROS_ASSERT(list[i].hasMember("name"));
int flags = 0;
bool resend = false;
double rate = 0.0; // unlimited
if(list[i].hasMember("rate"))
rate = list[i]["rate"];
if(list[i].hasMember("compress") && ((bool)list[i]["compress"]))
flags |= UDP_FLAG_COMPRESSED;
if(list[i].hasMember("resend") && ((bool)list[i]["resend"]))
resend = true;
bool enabled = true;
if(list[i].hasMember("enable") && (!(bool)list[i]["enable"]))
enabled = false;
std::string type;
if(list[i].hasMember("type"))
type = (std::string)(list[i]["type"]);
TopicSender* sender = new TopicSender(this, &nh, list[i]["name"], rate, resend, flags, enabled, type);
if(m_relayMode)
sender->setDirectTransmissionEnabled(false);
m_senders.push_back(sender);
}
nh.param("duplicate_first_packet", m_duplicateFirstPacket, false);
if(m_relayMode)
{
double target_bitrate;
if(!nh.getParam("relay_target_bitrate", target_bitrate))
{
throw std::runtime_error("relay mode needs relay_target_bitrate param");
}
double relay_control_rate;
nh.param("relay_control_rate", relay_control_rate, 100.0);
m_relayTokens = 0;
m_relayIndex = 0;
m_relayTokensPerStep = target_bitrate / 8.0 / relay_control_rate;
m_relayThreadShouldExit = false;
m_relayRate = relay_control_rate;
m_relayThread = boost::thread(boost::bind(&UDPSender::relay, this));
ROS_INFO("udp_sender: relay mode configured with control rate %f, target bitrate %f bit/s and token increment %d",
relay_control_rate, target_bitrate, m_relayTokensPerStep
);
}
char hostnameBuf[256];
gethostname(hostnameBuf, sizeof(hostnameBuf));
hostnameBuf[sizeof(hostnameBuf)-1] = 0;
m_stats.node = ros::this_node::getName();
m_stats.protocol = "UDP";
m_stats.host = hostnameBuf;
m_stats.destination = dest_host;
m_stats.destination_port = dest_port;
m_stats.source_port = source_port;
m_stats.fec = m_fec != 0.0;
nh.param("label", m_stats.label, std::string());
m_pub_stats = nh.advertise<nimbro_topic_transport::SenderStats>("/network/sender_stats", 1);
m_statsInterval = ros::WallDuration(2.0);
m_statsTimer = nh.createWallTimer(m_statsInterval,
boost::bind(&UDPSender::updateStats, this)
);
m_statsTimer.start();
}
UDPSender::~UDPSender()
{
if(m_relayMode)
{
m_relayThreadShouldExit = true;
m_relayThread.join();
}
for(unsigned int i = 0; i < m_senders.size(); ++i)
delete m_senders[i];
}
uint16_t UDPSender::allocateMessageID()
{
return m_msgID++;
}
bool UDPSender::send(const void* data, uint32_t size, const std::string& topic)
{
if(m_relayMode)
{
std::vector<uint8_t> packet(size);
memcpy(packet.data(), data, size);
m_relayBuffer.emplace_back(std::move(packet));
m_relayNameBuffer.push_back(topic);
return true;
}
else
{
return internalSend(data, size, topic);
}
}
bool UDPSender::internalSend(const void* data, uint32_t size, const std::string& topic)
{
if(sendto(m_fd, data, size, 0, (sockaddr*)&m_addr, m_addrLen) != size)
{
ROS_ERROR("Could not send data of size %d: %s", size, strerror(errno));
return false;
}
m_sentBytesInStatsInterval += size;
if (!topic.empty())
m_sentTopicBytesInStatsInterval[topic] += size;
return true;
}
void UDPSender::relay()
{
ros::WallRate rate(m_relayRate);
ROS_INFO("Relay thread starting...");
while(!m_relayThreadShouldExit)
{
// New tokens! Bound to 100*m_relayTokensPerStep to prevent token buildup.
m_relayTokens = std::min<uint64_t>(
100*m_relayTokensPerStep,
m_relayTokens + m_relayTokensPerStep
);
if(m_senders.empty())
throw std::runtime_error("No senders configured");
// While we have enough token, send something!
while(1)
{
unsigned int tries = 0;
bool noData = false;
// Find a topic sender that has some data for us. Note that we
// do not consume the message!
while(m_relayBuffer.empty())
{
if(tries++ == m_senders.size())
{
noData = true;
break; // No data yet
}
m_senders[m_relayIndex]->sendCurrentMessage();
m_relayIndex = (m_relayIndex + 1) % m_senders.size();
}
if(noData)
break;
const std::vector<uint8_t>& packet = m_relayBuffer.front();
// Get the topic name the packet is from
const std::string& topic = m_relayNameBuffer.front();
std::size_t sizeOnWire = packet.size() + 20 + 8;
// out of tokens? Wait for next iteration.
if(sizeOnWire > m_relayTokens)
break;
if(!internalSend(packet.data(), packet.size(), topic))
{
ROS_ERROR("Could not send packet");
break;
}
// Consume tokens
m_relayTokens -= sizeOnWire;
m_relayBuffer.pop_front();
m_relayNameBuffer.pop_front();
}
rate.sleep();
}
ROS_INFO("Relay thread exiting...");
}
void UDPSender::updateStats()
{
m_stats.header.stamp = ros::Time::now();
m_stats.bandwidth = 8 * m_sentBytesInStatsInterval / m_statsInterval.toSec();
// Get Bytes per topic in the message
m_stats.topics.clear();
for(auto& pair : m_sentTopicBytesInStatsInterval)
{
nimbro_topic_transport::TopicBandwidth tp;
tp.name = pair.first;
tp.bandwidth = 8 * pair.second / m_statsInterval.toSec();
pair.second = 0;
m_stats.topics.emplace_back(tp);
}
m_pub_stats.publish(m_stats);
m_sentBytesInStatsInterval = 0;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "udp_sender");
ros::NodeHandle nh("~");
nimbro_topic_transport::UDPSender sender;
ros::spin();
return 0;
}

80
software/rover/nimbro_topic_transport/src/udp/udp_sender.h Normal file
View File

@@ -0,0 +1,80 @@
// UDP sender node
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#ifndef UDP_SENDER_H
#define UDP_SENDER_H
#include <arpa/inet.h>
#include <ros/time.h>
#include <ros/timer.h>
#include <ros/rate.h>
#include <ros/wall_timer.h>
#include <ros/publisher.h>
#include <deque>
#include <map>
#include <boost/thread.hpp>
#include <nimbro_topic_transport/SenderStats.h>
namespace nimbro_topic_transport
{
class TopicSender;
class UDPSender
{
public:
UDPSender();
~UDPSender();
uint16_t allocateMessageID();
bool send(const void* data, uint32_t size, const std::string& topic);
inline bool duplicateFirstPacket() const
{ return m_duplicateFirstPacket; }
inline double fec() const
{ return m_fec; }
private:
void relay();
bool internalSend(const void* data, uint32_t size, const std::string& topic);
void updateStats();
uint16_t m_msgID;
int m_fd;
sockaddr_storage m_addr;
socklen_t m_addrLen;
ros::Time m_lastTime;
bool m_duplicateFirstPacket;
std::vector<TopicSender*> m_senders;
bool m_relayMode;
double m_relayRate;
int m_relayTokensPerStep;
uint64_t m_relayTokens;
std::deque<std::vector<uint8_t>> m_relayBuffer;
std::deque<std::string> m_relayNameBuffer;
unsigned int m_relayIndex;
boost::thread m_relayThread;
bool m_relayThreadShouldExit;
double m_fec;
nimbro_topic_transport::SenderStats m_stats;
ros::Publisher m_pub_stats;
ros::WallDuration m_statsInterval;
ros::WallTimer m_statsTimer;
uint64_t m_sentBytesInStatsInterval;
// Count sent bytes for each topic seperately for logging
std::map<std::string, uint64_t> m_sentTopicBytesInStatsInterval;
};
}
#endif

11
software/rover/nimbro_topic_transport/test/bidirectional.test Normal file
View File

@@ -0,0 +1,11 @@
<launch>
<arg name="allow_bidirectional" default="true" />
<include file="$(find nimbro_topic_transport)/launch/bidirectional_topics.launch">
<arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
</include>
<test test-name="test_bidirectional_$(arg allow_bidirectional)" pkg="nimbro_topic_transport" type="test_bidirectional">
<param name="allow_bidirectional" value="$(arg allow_bidirectional)" />
</test>
</launch>

96
software/rover/nimbro_topic_transport/test/test_bidirectional.cpp Normal file
View File

@@ -0,0 +1,96 @@
// Unit tests for nimbro_topic_transport
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include <catch_ros/catch.hpp>
#include <ros/package.h>
#include <ros/param.h>
#include <ros/node_handle.h>
#include <std_msgs/Int64.h>
int g_counter = 0;
void handleMessageLocal(const std_msgs::Int64 &msg)
{
//REQUIRE(g_counter == msg.data);
g_counter++;
}
void
testOneDirection(ros::NodeHandle &nh, bool allow_bidirectional, std::string source_topic, std::string sink_topic) {
ros::Publisher pub = nh.advertise<std_msgs::Int64>(source_topic, 2);
ros::Subscriber sub = nh.subscribe(sink_topic, 2, &handleMessageLocal);
int timeout = 50;
while(pub.getNumSubscribers() == 0)
{
ros::spinOnce();
if(--timeout == 0)
{
CAPTURE(pub.getNumSubscribers());
FAIL("sender did not subscribe on our topic");
return;
}
usleep(20 * 1000);
}
g_counter = 0;
std_msgs::Int64 msg;
msg.data = 0;
pub.publish(msg);
ROS_INFO("Sent first message, waiting for 5 seconds to receive it.");
for(int i = 0; i < 5000; ++i)
{
ros::spinOnce();
usleep(1000);
}
msg.data = 1;
pub.publish(msg);
ROS_INFO("Sent second message, waiting for 5 seconds to receive it.");
for(int i = 0; i < 5000; ++i)
{
ros::spinOnce();
usleep(1000);
}
if (allow_bidirectional) {
if (g_counter == 2) {
ROS_INFO("With bidirectional support, each message was received "
"exactly once.");
return;
}
} else {
if (g_counter > 10) {
ROS_INFO_STREAM("Without bidirectional support, the relay entered "
"an infinite loop; the messsages were received "
<< g_counter << " times.");
CAPTURE(g_counter);
return;
}
}
CAPTURE(allow_bidirectional);
CAPTURE(g_counter);
FAIL();
}
TEST_CASE("bidirectional_communication", "[topic]")
{
ros::NodeHandle nh("~");
bool allow_bidirectional;
nh.getParam("allow_bidirectional", allow_bidirectional);
ROS_INFO("Testing machine1 to machine2 direction.");
testOneDirection(nh, allow_bidirectional, "/my_first_topic", "/recv/my_first_topic");
ROS_INFO("Testing machine2 to machine1 direction.");
testOneDirection(nh, allow_bidirectional, "/recv/my_first_topic", "/my_first_topic");
}

156
software/rover/nimbro_topic_transport/test/test_comm.cpp Normal file
View File

@@ -0,0 +1,156 @@
// Unit tests for nimbro_topic_transport
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
#include <catch_ros/catch.hpp>
#include <ros/package.h>
#include <ros/node_handle.h>
#include <ros/init.h>
#include <std_msgs/Int64.h>
#include <std_msgs/UInt64MultiArray.h>
int g_counter = 0;
void handleMessageLocal(const std_msgs::Int64 &msg)
{
REQUIRE(g_counter == msg.data);
g_counter++;
}
int g_arrayCounter = 0;
void handle_array(const std_msgs::UInt64MultiArray& msg)
{
REQUIRE(msg.data.size() == 512);
for(int i = 0; i < 512; ++i)
{
REQUIRE(msg.data[i] == i);
}
g_arrayCounter++;
}
const int HUGE_SIZE = 3 * 1024;
void handle_huge(const std_msgs::UInt64MultiArray& msg)
{
REQUIRE(msg.data.size() == HUGE_SIZE);
for(int i = 0; i < HUGE_SIZE; ++i)
{
REQUIRE(msg.data[i] == i);
}
g_arrayCounter++;
}
TEST_CASE("simple", "[topic]")
{
std_msgs::Int64 msg;
g_counter = 0;
ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::Int64>("/test_topic", 2);
ros::Subscriber sub = nh.subscribe("/receive/test_topic", 2, &handleMessageLocal);
int timeout = 50;
while(pub.getNumSubscribers() == 0)
{
ros::spinOnce();
if(--timeout == 0)
{
CAPTURE(pub.getNumSubscribers());
FAIL("sender did not subscribe on our topic");
return;
}
usleep(20 * 1000);
}
g_counter = 0;
msg.data = 0;
pub.publish(msg);
for(int i = 0; i < 1000; ++i)
{
ros::spinOnce();
usleep(1000);
}
msg.data = 1;
pub.publish(msg);
for(int i = 0; i < 1000; ++i)
{
ros::spinOnce();
usleep(1000);
if(g_counter == 2)
return;
}
FAIL();
}
TEST_CASE("array", "[topic]")
{
std_msgs::UInt64MultiArray msg;
g_arrayCounter = 0;
ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::UInt64MultiArray>("/array_topic", 2);
ros::Subscriber sub;
SECTION("small")
{
sub = nh.subscribe("/receive/array_topic", 2, &handle_array);
for(int i = 0; i < 512; ++i)
msg.data.push_back(i);
}
SECTION("huge")
{
sub = nh.subscribe("/receive/array_topic", 2, &handle_huge);
for(int i = 0; i < HUGE_SIZE; ++i)
msg.data.push_back(i);
}
int timeout = 50;
while(pub.getNumSubscribers() == 0)
{
ros::spinOnce();
if(--timeout == 0)
{
CAPTURE(pub.getNumSubscribers());
FAIL("sender did not subscribe on our topic");
return;
}
usleep(20 * 1000);
}
g_arrayCounter = 0;
pub.publish(msg);
for(int i = 0; i < 1000; ++i)
{
ros::spinOnce();
usleep(1000);
}
pub.publish(msg);
for(int i = 0; i < 1000; ++i)
{
ros::spinOnce();
usleep(1000);
if(g_arrayCounter == 2)
return;
}
CAPTURE(g_arrayCounter);
FAIL();
}

30
software/rover/nimbro_topic_transport/test/topic_transport.test Normal file
View File

@@ -0,0 +1,30 @@
<launch>
<arg name="protocol" />
<arg name="fec" default="false" />
<!-- Skip long wait on config server, not needed here -->
<param name="config_server/enabled" value="false" />
<node name="sender" pkg="nimbro_topic_transport" type="$(arg protocol)_sender">
<!-- Destination: localhost -->
<param name="destination_addr" value="127.0.0.1" />
<param name="destination_port" value="5777" />
<param name="source_port" value="5776" />
<param if="$(arg fec)" name="fec" value="0.5" />
<!-- Load topics from yaml file -->
<rosparam command="load" file="$(find nimbro_topic_transport)/test/topics.yaml" />
</node>
<node name="receiver" pkg="nimbro_topic_transport" type="$(arg protocol)_receiver">
<param name="port" value="5777" />
<remap from="/test_topic" to="/receive/test_topic" />
<remap from="/array_topic" to="/receive/array_topic" />
<param name="fec" value="$(arg fec)" />
</node>
<test test-name="test_$(arg protocol)_fec_$(arg fec)" pkg="nimbro_topic_transport" type="test_comm">
</test>
</launch>

5
software/rover/nimbro_topic_transport/test/topics.yaml Normal file
View File

@@ -0,0 +1,5 @@
topics:
- name: "/test_topic"
- name: "/throttled_topic"
rate: 1.0
- name: "/array_topic"

33
software/rover/nimbro_topic_transport/utils/nimbro_network.lua Normal file
View File

@@ -0,0 +1,33 @@
do
local nimbro_proto = Proto("nimbro_topic", "nimbro_topic_transport");
nimbro_proto.prefs.port = Pref.uint( "nimbro_port", 5000, "UDP port" )
nimbro_proto.fields.msg = ProtoField.uint16("nimbro.msg", "Message ID", base.DEC)
nimbro_proto.fields.frag = ProtoField.uint16("nimbro.frag", "Fragment ID", base.DEC)
local prev_proto
local f_udp = Field.new("udp")
function nimbro_proto.dissector(tvb, pinfo, tree)
pcall(function()prev_proto:call(tvb, pinfo, tree)end)
if not f_udp() then return end
-- this is just to add text to "nimbro.data" field,
-- which you should display as column.
-- as an alternate, you may remove set_hidden() and view selected data in the treeview
tree:add_le(nimbro_proto.fields.msg, tvb(2,2));
tree:add_le(nimbro_proto.fields.frag, tvb(0,2));
end
-- if we hook upon UDP port, then offset will mean the beginning of the UDP data
udp_table = DissectorTable.get("udp.port")
prev_proto = udp_table:get_dissector(nimbro_proto.prefs.port)
udp_table:add(nimbro_proto.prefs.port, nimbro_proto)
-- if we hook as post dissector, the offset will be from start of the frame.
-- don't forget to remove the prev_proto call if you'll use that kind of hook
-- register_postdissector(nimbro_proto)
end

33
software/rover/nimbro_topic_transport/utils/nimbro_network_fec.lua Normal file
View File

@@ -0,0 +1,33 @@
do
local nimbro_proto = Proto("nimbro_topic", "nimbro_topic_transport");
nimbro_proto.prefs.port = Pref.uint( "nimbro_port", 5777, "UDP port" )
nimbro_proto.fields.msg = ProtoField.uint16("nimbro.msg", "Message ID", base.DEC)
nimbro_proto.fields.frag = ProtoField.uint32("nimbro.frag", "Symbol ID", base.DEC)
local prev_proto
local f_udp = Field.new("udp")
function nimbro_proto.dissector(tvb, pinfo, tree)
pcall(function()prev_proto:call(tvb, pinfo, tree)end)
if not f_udp() then return end
-- this is just to add text to "nimbro.data" field,
-- which you should display as column.
-- as an alternate, you may remove set_hidden() and view selected data in the treeview
tree:add_le(nimbro_proto.fields.msg, tvb(0,2));
tree:add_le(nimbro_proto.fields.frag, tvb(2,4));
end
-- if we hook upon UDP port, then offset will mean the beginning of the UDP data
udp_table = DissectorTable.get("udp.port")
prev_proto = udp_table:get_dissector(nimbro_proto.prefs.port)
udp_table:add(nimbro_proto.prefs.port, nimbro_proto)
-- if we hook as post dissector, the offset will be from start of the frame.
-- don't forget to remove the prev_proto call if you'll use that kind of hook
-- register_postdissector(nimbro_proto)
end

17
software/rover/rover_drive/CMakeLists.txt → software/rover/rover_control/CMakeLists.txt
View File

@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
project(rover_drive)
project(rover_control)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
@@ -10,6 +10,7 @@ project(rover_drive)
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
geometry_msgs
message_generation
)
@@ -49,7 +50,8 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(
FILES
RoverMotorDrive.msg
DriveCommandMessage.msg
DriveControlMessage.msg
)
## Generate services in the 'srv' folder
@@ -70,6 +72,7 @@ find_package(catkin REQUIRED COMPONENTS
generate_messages(
DEPENDENCIES
std_msgs # Or other packages containing msgs
geometry_msgs
)
################################################
@@ -103,8 +106,8 @@ find_package(catkin REQUIRED COMPONENTS
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES rover_drive
# CATKIN_DEPENDS rospy
# LIBRARIES rover_control
# CATKIN_DEPENDS ros_msgs rospy
# DEPENDS system_lib
)
@@ -121,7 +124,7 @@ include_directories(
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/rover_drive.cpp
# src/${PROJECT_NAME}/rover_control.cpp
# )
## Add cmake target dependencies of the library
@@ -132,7 +135,7 @@ include_directories(
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/rover_drive_node.cpp)
# add_executable(${PROJECT_NAME}_node src/rover_control_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -189,7 +192,7 @@ include_directories(
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_drive.cpp)
# catkin_add_gtest(${PROJECT_NAME}-test test/test_rover_control.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

27
software/rover/rover_control/launch/rover_control.launch Normal file
View File

@@ -0,0 +1,27 @@
<launch>
<group ns="rover_control">
<node name="iris_controller" pkg="rover_control" type="iris_controller.py" output="screen">
<param name="port" value="/dev/rover/ttyIRIS_2_2"/>
</node>
<node name="control_coordinator" pkg="rover_control" type="control_coordinator.py" output="screen"/>
<node name="rear_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<param name="port" value="/dev/rover/ttyIRIS_0_0"/>
<param name="drive_control_topic" value="drive_control/rear"/>
</node>
<node name="left_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<param name="port" value="/dev/rover/ttyIRIS_0_1"/>
<param name="drive_control_topic" value="drive_control/left"/>
<param name="invert_first_motor" value="True"/>
<param name="invert_second_motor" value="True"/>
</node>
<node name="right_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<param name="port" value="/dev/rover/ttyIRIS_0_2"/>
<param name="drive_control_topic" value="drive_control/right"/>
</node>
</group>
</launch>

2
software/rover/rover_control/msg/DriveCommandMessage.msg Normal file
View File

@@ -0,0 +1,2 @@
bool ignore_drive_control
geometry_msgs/Twist drive_twist

0
software/rover/rover_drive/msg/RoverMotorDrive.msg → software/rover/rover_control/msg/DriveControlMessage.msg
View File

9
software/rover/launcher/package.xml → software/rover/rover_control/package.xml
View File

@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package format="2">
<name>launcher</name>
<name>rover_control</name>
<version>0.0.0</version>
<description>The launcher package</description>
<description>The rover_control package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
@@ -19,7 +19,7 @@
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/launcher</url> -->
<!-- <url type="website">http://wiki.ros.org/rover_control</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -49,8 +49,11 @@
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>ros_msgs</build_depend>
<build_depend>rospy</build_depend>
<build_export_depend>ros_msgs</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>ros_msgs</exec_depend>
<exec_depend>rospy</exec_depend>

85
software/rover/rover_control/src/control_coordinator/control_coordinator.py Executable file
View File

@@ -0,0 +1,85 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
# Custom Imports
from rover_control.msg import DriveCommandMessage, DriveControlMessage
#####################################
# Global Variables
#####################################
NODE_NAME = "control_coordinator"
DEFAULT_IRIS_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
DEFAULT_REAR_BOGIE_TOPIC = "drive_control/rear"
DEFAULT_LEFT_BOGIE_TOPIC = "drive_control/left"
DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right"
UINT16_MAX = 65535
#####################################
# ControlCoordinator Class Definition
#####################################
class ControlCoordinator(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.iris_topic = rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
self.rear_bogie_topic = rospy.get_param("~rear_bogie_control_topic", DEFAULT_REAR_BOGIE_TOPIC)
self.left_bogie_topic = rospy.get_param("~left_bogie_control_topic", DEFAULT_LEFT_BOGIE_TOPIC)
self.right_bogie_topic = rospy.get_param("~right_bogie_control_topic", DEFAULT_RIGHT_BOGIE_TOPIC)
self.iris_drive_command_subscriber = \
rospy.Subscriber(self.iris_topic, DriveCommandMessage, self.iris_drive_command_callback)
self.rear_bogie_publisher = rospy.Publisher(self.rear_bogie_topic, DriveControlMessage, queue_size=1)
self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)
self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
self.run()
def run(self):
while not rospy.is_shutdown():
sleep(0.25)
def iris_drive_command_callback(self, drive_command):
rear_drive = DriveControlMessage()
left_drive = DriveControlMessage()
right_drive = DriveControlMessage()
left = drive_command.drive_twist.linear.x - drive_command.drive_twist.angular.z
right = drive_command.drive_twist.linear.x + drive_command.drive_twist.angular.z
left_direction = 1 if left >= 0 else 0
rear_drive.first_motor_direction = left_direction
left_drive.first_motor_direction = left_direction
left_drive.second_motor_direction = left_direction
right_direction = 1 if right >= 0 else 0
rear_drive.second_motor_direction = right_direction
right_drive.first_motor_direction = right_direction
right_drive.second_motor_direction = right_direction
left_speed = abs(left * UINT16_MAX)
right_speed = abs(right * UINT16_MAX)
rear_drive.first_motor_speed = left_speed
left_drive.first_motor_speed = left_speed
left_drive.second_motor_speed = left_speed
rear_drive.second_motor_speed = right_speed
right_drive.first_motor_speed = right_speed
right_drive.second_motor_speed = right_speed
self.rear_bogie_publisher.publish(rear_drive)
self.left_bogie_publisher.publish(left_drive)
self.right_bogie_publisher.publish(right_drive)
if __name__ == "__main__":
ControlCoordinator()

114
software/rover/rover_control/src/drive_control/drive_control.py Executable file
View File

@@ -0,0 +1,114 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial.rs485
import minimalmodbus
# Custom Imports
from rover_control.msg import DriveControlMessage
#####################################
# Global Variables
#####################################
NODE_NAME = "drive_control"
DEFAULT_PORT = "/dev/rover/ttyBogie"
DEFAULT_BAUD = 115200
DEFAULT_INVERT = False
DEFAULT_DRIVE_CONTROL_TOPIC = "drive_control/rear"
FIRST_MOTOR_ID = 1
SECOND_MOTOR_ID = 2
COMMUNICATIONS_TIMEOUT = 0.15 # Seconds
RX_DELAY = 0.01
TX_DELAY = 0.01
MODBUS_REGISTERS = {
"DIRECTION": 0,
"SPEED": 1,
"SLEEP": 2,
"CURRENT": 3,
"FAULT": 4,
"TEMPERATURE": 5
}
MOTOR_DRIVER_DEFAULT_MESSAGE = [
1, # Forwards
0, # 0 Speed
1 # Not in sleep mode
]
#####################################
# DriveControl Class Definition
#####################################
class DriveControl(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
self.first_motor_inverted = rospy.get_param("~invert_first_motor", DEFAULT_INVERT)
self.second_motor_inverted = rospy.get_param("~invert_second_motor", DEFAULT_INVERT)
self.drive_control_subscriber_topic = rospy.get_param("~drive_control_topic", DEFAULT_DRIVE_CONTROL_TOPIC)
self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
self.__setup_minimalmodbus_for_485()
self.drive_control_subscriber = \
rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback)
self.run()
def __setup_minimalmodbus_for_485(self):
self.first_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.first_motor.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
self.second_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.second_motor.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
def run(self):
while not rospy.is_shutdown():
sleep(0.25)
def drive_control_callback(self, drive_control):
try:
first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
first_direction = \
not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction
first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction
first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = drive_control.first_motor_speed
second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction
second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction
second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = drive_control.second_motor_speed
self.first_motor.write_registers(0, first_motor_register_data)
self.second_motor.write_registers(0, second_motor_register_data)
except Exception, error:
print "Error occurred:", error
if __name__ == "__main__":
DriveControl()

154
software/rover/rover_control/src/iris_controller/iris_controller.py Executable file
View File

@@ -0,0 +1,154 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial.rs485
import minimalmodbus
# Custom Imports
from rover_control.msg import DriveCommandMessage
#####################################
# Global Variables
#####################################
NODE_NAME = "iris_controller"
DEFAULT_PORT = "/dev/rover/ttyIRIS"
DEFAULT_BAUD = 115200
DEFAULT_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
DEFAULT_HERTZ = 10
COMMUNICATIONS_TIMEOUT = 0.15 # Seconds
MODBUS_ID = 1
RX_DELAY = 0.01
TX_DELAY = 0.01
SBUS_VALUES = {
"SBUS_MAX": 1811,
"SBUS_MID": 991,
"SBUS_MIN": 172,
"SBUS_RANGE": 820.0,
"SBUS_DEADZONE": 5
}
MODBUS_REGISTERS = {
"LEFT_STICK_Y_AXIS": 0,
"RIGHT_STICK_Y_AXIS": 1,
"RIGHT_STICK_X_AXIS": 2,
"LEFT_STICK_X_AXIS": 3,
"LEFT_POT": 4,
"S1_POT": 5,
"S2_POT": 6,
"RIGHT_POT": 7,
"SA_SWITCH": 8,
"SB_SWITCH": 9,
"SC_SWITCH": 10,
"SD_SWITCH": 11,
"SE_SWITCH": 12,
"SF_SWITCH": 13,
"SG_SWITCH": 14,
"SH_SWITCH": 15,
"VOLTAGE_24V": 16,
"VOLTAGE_5V": 17,
"USB_VOLTAGE_5V": 18,
"VOLTAGE_3V3": 19
}
REGISTER_STATE_MAPPING = {
"IGNORE_CONTROL": "SF_SWITCH",
"DRIVE_VS_ARM": "SE_SWITCH"
}
#####################################
# IrisController Class Definition
#####################################
class IrisController(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
self.drive_command_publisher_topic = rospy.get_param("~drive_command_topic", DEFAULT_DRIVE_COMMAND_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.iris = minimalmodbus.Instrument(self.port, MODBUS_ID)
self.__setup_minimalmodbus_for_485()
self.drive_command_publisher = rospy.Publisher(self.drive_command_publisher_topic, DriveCommandMessage,
queue_size=1)
self.registers = []
self.run()
def __setup_minimalmodbus_for_485(self):
self.iris.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.iris.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY, delay_before_tx=TX_DELAY)
def run(self):
while not rospy.is_shutdown():
start_time = time()
try:
self.read_registers()
self.broadcast_drive_if_current_mode()
self.broadcast_arm_if_current_mode()
except Exception, error:
print "Error occurred:", error
time_diff = time() - start_time
sleep(max(self.wait_time - time_diff, 0))
def read_registers(self):
self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
def broadcast_drive_if_current_mode(self):
if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]:
command = DriveCommandMessage()
left_y_axis = self.registers[MODBUS_REGISTERS["LEFT_STICK_Y_AXIS"]]
right_y_axis = self.registers[MODBUS_REGISTERS["RIGHT_STICK_Y_AXIS"]]
if left_y_axis == 0 and right_y_axis == 0:
command.ignore_drive_control = True
command.drive_twist.linear.x = 0
command.drive_twist.angular.z = 0
else:
left = (left_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
"SBUS_RANGE"]
right = (right_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
"SBUS_RANGE"]
command.ignore_drive_control = \
self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["IGNORE_CONTROL"]]] > SBUS_VALUES["SBUS_MID"]
command.drive_twist.linear.x = (left + right) / 2.0
command.drive_twist.angular.z = (right - left) / 2.0
self.drive_command_publisher.publish(command)
def broadcast_arm_if_current_mode(self):
if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] > \
SBUS_VALUES["SBUS_MIN"] + SBUS_VALUES["SBUS_DEADZONE"]:
print "Arm"
if __name__ == "__main__":
IrisController()

15
software/rover/rover_drive/.idea/inspectionProfiles/Project_Default.xml generated
View File

@@ -1,15 +0,0 @@
<component name="InspectionProjectProfileManager">
<profile version="1.0">
<option name="myName" value="Project Default" />
<inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
<option name="ignoredIdentifiers">
<list>
<option value="dict.exit_requested_signal" />
<option value="dict.primary_video_label" />
<option value="dict.secondary_video_label" />
<option value="dict.tertiary_video_label" />
</list>
</option>
</inspection_tool>
</profile>
</component>

7
software/rover/rover_drive/.idea/inspectionProfiles/profiles_settings.xml generated
View File

@@ -1,7 +0,0 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="PROJECT_PROFILE" value="Project Default" />
<option name="USE_PROJECT_PROFILE" value="true" />
<version value="1.0" />
</settings>
</component>

14
software/rover/rover_drive/.idea/misc.xml generated
View File

@@ -1,14 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectLevelVcsManager" settingsEditedManually="false">
<OptionsSetting value="true" id="Add" />
<OptionsSetting value="true" id="Remove" />
<OptionsSetting value="true" id="Checkout" />
<OptionsSetting value="true" id="Update" />
<OptionsSetting value="true" id="Status" />
<OptionsSetting value="true" id="Edit" />
<ConfirmationsSetting value="0" id="Add" />
<ConfirmationsSetting value="0" id="Remove" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="Python 2.7" project-jdk-type="Python SDK" />
</project>

8
software/rover/rover_drive/.idea/modules.xml generated
View File

@@ -1,8 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/rover_drive.iml" filepath="$PROJECT_DIR$/.idea/rover_drive.iml" />
</modules>
</component>
</project>

12
software/rover/rover_drive/.idea/rover_drive.iml generated
View File

@@ -1,12 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="TestRunnerService">
<option name="projectConfiguration" value="Nosetests" />
<option name="PROJECT_TEST_RUNNER" value="Nosetests" />
</component>
</module>

817
software/rover/rover_drive/.idea/workspace.xml generated
View File

@@ -1,817 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ChangeListManager">
<list default="true" id="3407228a-beb9-4e3b-a11a-9d0898be4da8" name="Default" comment="" />
<ignored path="rover_drive.iws" />
<ignored path=".idea/workspace.xml" />
<ignored path=".idea/dataSources.local.xml" />
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="TRACKING_ENABLED" value="true" />
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
<option name="LAST_RESOLUTION" value="IGNORE" />
</component>
<component name="CoverageDataManager">
<SUITE FILE_PATH="coverage/rover_drive$Local_Launch.coverage" NAME="Local Launch Coverage Results" MODIFIED="1518128882987" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/src" />
</component>
<component name="CreatePatchCommitExecutor">
<option name="PATCH_PATH" value="" />
</component>
<component name="ExecutionTargetManager" SELECTED_TARGET="default_target" />
<component name="FavoritesManager">
<favorites_list name="rover_drive" />
</component>
<component name="FileEditorManager">
<leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
<file leaf-file-name="test.py" pinned="false" current-in-tab="false">
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="36">
<caret line="4" column="0" selection-start-line="4" selection-start-column="0" selection-end-line="22" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
</file>
<file leaf-file-name="rover_drive_node.py" pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1134">
<caret line="63" column="23" selection-start-line="63" selection-start-column="23" selection-end-line="63" selection-end-column="23" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
</file>
<file leaf-file-name="rover_drive_node_tester.py" pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="13" column="17" selection-start-line="13" selection-start-column="17" selection-end-line="13" selection-end-column="17" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
</file>
<file leaf-file-name="example.launch" pinned="false" current-in-tab="true">
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="54">
<caret line="3" column="54" selection-start-line="3" selection-start-column="54" selection-end-line="3" selection-end-column="54" />
<folding />
</state>
</provider>
</entry>
</file>
</leaf>
</component>
<component name="FileTemplateManagerImpl">
<option name="RECENT_TEMPLATES">
<list>
<option value="Python Script" />
</list>
</option>
</component>
<component name="IdeDocumentHistory">
<option name="CHANGED_PATHS">
<list>
<option value="$PROJECT_DIR$/msg/RoverMotorDrive.msg" />
<option value="$PROJECT_DIR$/CMakeLists.txt" />
<option value="$PROJECT_DIR$/src/rover_drive.py" />
<option value="$PROJECT_DIR$/package.xml" />
<option value="$PROJECT_DIR$/src/rover_drive_node.py" />
<option value="$PROJECT_DIR$/src/rover_drive_node_tester.py" />
<option value="$USER_HOME$/PycharmProjects/485_test/test.py" />
<option value="$PROJECT_DIR$/launch/example.launch" />
</list>
</option>
</component>
<component name="JsBuildToolGruntFileManager" detection-done="true" sorting="DEFINITION_ORDER" />
<component name="JsBuildToolPackageJson" detection-done="true" sorting="DEFINITION_ORDER" />
<component name="JsGulpfileManager">
<detection-done>true</detection-done>
<sorting>DEFINITION_ORDER</sorting>
</component>
<component name="ProjectFrameBounds">
<option name="x" value="1920" />
<option name="width" value="1678" />
<option name="height" value="1020" />
</component>
<component name="ProjectLevelVcsManager" settingsEditedManually="false">
<OptionsSetting value="true" id="Add" />
<OptionsSetting value="true" id="Remove" />
<OptionsSetting value="true" id="Checkout" />
<OptionsSetting value="true" id="Update" />
<OptionsSetting value="true" id="Status" />
<OptionsSetting value="true" id="Edit" />
<ConfirmationsSetting value="0" id="Add" />
<ConfirmationsSetting value="0" id="Remove" />
</component>
<component name="ProjectView">
<navigator currentView="ProjectPane" proportions="" version="1">
<flattenPackages />
<showMembers />
<showModules />
<showLibraryContents />
<hideEmptyPackages />
<abbreviatePackageNames />
<autoscrollToSource />
<autoscrollFromSource />
<sortByType />
<manualOrder />
<foldersAlwaysOnTop value="true" />
</navigator>
<panes>
<pane id="Scope" />
<pane id="Scratches" />
<pane id="ProjectPane">
<subPane>
<PATH>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.ProjectViewProjectNode" />
</PATH_ELEMENT>
</PATH>
<PATH>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.ProjectViewProjectNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
</PATH>
<PATH>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.ProjectViewProjectNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="src" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
</PATH>
<PATH>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.ProjectViewProjectNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="msg" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
</PATH>
<PATH>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.ProjectViewProjectNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="rover_drive" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
<PATH_ELEMENT>
<option name="myItemId" value="launch" />
<option name="myItemType" value="com.intellij.ide.projectView.impl.nodes.PsiDirectoryNode" />
</PATH_ELEMENT>
</PATH>
</subPane>
</pane>
</panes>
</component>
<component name="PropertiesComponent">
<property name="last_opened_file_path" value="$PROJECT_DIR$/src/rover_drive_node_tester.py" />
<property name="WebServerToolWindowFactoryState" value="false" />
<property name="restartRequiresConfirmation" value="false" />
</component>
<component name="RecentsManager">
<key name="CopyFile.RECENT_KEYS">
<recent name="$PROJECT_DIR$/launch" />
</key>
</component>
<component name="RunManager" selected="Python.Local Launch">
<configuration default="true" type="DjangoTestsConfigurationType" factoryName="Django tests">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="TARGET" value="" />
<option name="SETTINGS_FILE" value="" />
<option name="CUSTOM_SETTINGS" value="false" />
<option name="USE_OPTIONS" value="false" />
<option name="OPTIONS" value="" />
<method />
</configuration>
<configuration default="true" type="JavascriptDebugType" factoryName="JavaScript Debug">
<method />
</configuration>
<configuration default="true" type="PyBehaveRunConfigurationType" factoryName="Behave">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="ADDITIONAL_ARGS" value="" />
<method />
</configuration>
<configuration default="true" type="PyLettuceRunConfigurationType" factoryName="Lettuce">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="ADDITIONAL_ARGS" value="" />
<method />
</configuration>
<configuration default="true" type="PythonConfigurationType" factoryName="Python">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<method />
</configuration>
<configuration default="true" type="Tox" factoryName="Tox">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<module name="rover_drive" />
<method />
</configuration>
<configuration default="true" type="js.build_tools.gulp" factoryName="Gulp.js">
<node-interpreter>project</node-interpreter>
<node-options />
<gulpfile />
<tasks />
<arguments />
<envs />
<method />
</configuration>
<configuration default="true" type="js.build_tools.npm" factoryName="npm">
<command value="run-script" />
<scripts />
<node-interpreter value="project" />
<envs />
<method />
</configuration>
<configuration default="true" type="tests" factoryName="Attests">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="" />
<option name="CLASS_NAME" value="" />
<option name="METHOD_NAME" value="" />
<option name="FOLDER_NAME" value="" />
<option name="TEST_TYPE" value="TEST_SCRIPT" />
<option name="PATTERN" value="" />
<option name="USE_PATTERN" value="false" />
<method />
</configuration>
<configuration default="true" type="tests" factoryName="Doctests">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="" />
<option name="CLASS_NAME" value="" />
<option name="METHOD_NAME" value="" />
<option name="FOLDER_NAME" value="" />
<option name="TEST_TYPE" value="TEST_SCRIPT" />
<option name="PATTERN" value="" />
<option name="USE_PATTERN" value="false" />
<method />
</configuration>
<configuration default="true" type="tests" factoryName="Nosetests">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="" />
<option name="CLASS_NAME" value="" />
<option name="METHOD_NAME" value="" />
<option name="FOLDER_NAME" value="" />
<option name="TEST_TYPE" value="TEST_SCRIPT" />
<option name="PATTERN" value="" />
<option name="USE_PATTERN" value="false" />
<option name="PARAMS" value="" />
<option name="USE_PARAM" value="false" />
<method />
</configuration>
<configuration default="true" type="tests" factoryName="Unittests">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="" />
<option name="CLASS_NAME" value="" />
<option name="METHOD_NAME" value="" />
<option name="FOLDER_NAME" value="" />
<option name="TEST_TYPE" value="TEST_SCRIPT" />
<option name="PATTERN" value="" />
<option name="USE_PATTERN" value="false" />
<option name="PUREUNITTEST" value="true" />
<option name="PARAMS" value="" />
<option name="USE_PARAM" value="false" />
<method />
</configuration>
<configuration default="true" type="tests" factoryName="py.test">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs />
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="" />
<option name="CLASS_NAME" value="" />
<option name="METHOD_NAME" value="" />
<option name="FOLDER_NAME" value="" />
<option name="TEST_TYPE" value="TEST_SCRIPT" />
<option name="PATTERN" value="" />
<option name="USE_PATTERN" value="false" />
<option name="testToRun" value="" />
<option name="keywords" value="" />
<option name="params" value="" />
<option name="USE_PARAM" value="false" />
<option name="USE_KEYWORD" value="false" />
<method />
</configuration>
<configuration default="false" name="Local Launch" type="PythonConfigurationType" factoryName="Python" singleton="true">
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="/usr/bin/python2" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/src" />
<option name="IS_MODULE_SDK" value="false" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<module name="rover_drive" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" enabled="false" sample_coverage="true" runner="coverage.py" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/src/rover_drive_node_tester.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<method />
</configuration>
<list size="1">
<item index="0" class="java.lang.String" itemvalue="Python.Local Launch" />
</list>
</component>
<component name="ShelveChangesManager" show_recycled="false">
<option name="remove_strategy" value="false" />
</component>
<component name="SvnConfiguration">
<configuration />
</component>
<component name="TaskManager">
<task active="true" id="Default" summary="Default task">
<changelist id="3407228a-beb9-4e3b-a11a-9d0898be4da8" name="Default" comment="" />
<created>1517699292320</created>
<option name="number" value="Default" />
<option name="presentableId" value="Default" />
<updated>1517699292320</updated>
</task>
<servers />
</component>
<component name="ToolWindowManager">
<frame x="1920" y="0" width="1678" height="1020" extended-state="5" />
<editor active="true" />
<layout>
<window_info id="Project" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.18383233" sideWeight="0.5" order="0" side_tool="false" content_ui="combo" />
<window_info id="TODO" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="6" side_tool="false" content_ui="tabs" />
<window_info id="Event Log" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="true" content_ui="tabs" />
<window_info id="Database" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
<window_info id="Version Control" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
<window_info id="Python Console" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.32897604" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
<window_info id="Run" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="true" show_stripe_button="true" weight="0.3278867" sideWeight="0.5" order="2" side_tool="false" content_ui="tabs" />
<window_info id="Structure" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
<window_info id="Terminal" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="7" side_tool="false" content_ui="tabs" />
<window_info id="Favorites" active="false" anchor="left" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="2" side_tool="true" content_ui="tabs" />
<window_info id="Debug" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="3" side_tool="false" content_ui="tabs" />
<window_info id="Cvs" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="4" side_tool="false" content_ui="tabs" />
<window_info id="Message" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="0" side_tool="false" content_ui="tabs" />
<window_info id="Commander" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="0" side_tool="false" content_ui="tabs" />
<window_info id="Inspection" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.4" sideWeight="0.5" order="5" side_tool="false" content_ui="tabs" />
<window_info id="Hierarchy" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="2" side_tool="false" content_ui="combo" />
<window_info id="Find" active="false" anchor="bottom" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.33" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
<window_info id="Ant Build" active="false" anchor="right" auto_hide="false" internal_type="DOCKED" type="DOCKED" visible="false" show_stripe_button="true" weight="0.25" sideWeight="0.5" order="1" side_tool="false" content_ui="tabs" />
</layout>
</component>
<component name="Vcs.Log.UiProperties">
<option name="RECENTLY_FILTERED_USER_GROUPS">
<collection />
</option>
<option name="RECENTLY_FILTERED_BRANCH_GROUPS">
<collection />
</option>
</component>
<component name="VcsContentAnnotationSettings">
<option name="myLimit" value="2678400000" />
</component>
<component name="XDebuggerManager">
<breakpoint-manager />
<watches-manager />
</component>
<component name="editorHistoryManager">
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="0">
<caret line="0" column="0" selection-start-line="0" selection-start-column="0" selection-end-line="0" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1134">
<caret line="63" column="23" selection-start-line="63" selection-start-column="23" selection-end-line="63" selection-end-column="23" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="13" column="17" selection-start-line="13" selection-start-column="17" selection-end-line="13" selection-end-column="17" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="8" column="0" selection-start-line="8" selection-start-column="0" selection-end-line="8" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="252">
<caret line="16" column="0" selection-start-line="16" selection-start-column="0" selection-end-line="18" selection-end-column="12" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1746">
<caret line="97" column="45" selection-start-line="97" selection-start-column="45" selection-end-line="97" selection-end-column="45" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="0">
<caret line="0" column="0" selection-start-line="0" selection-start-column="0" selection-end-line="0" selection-end-column="0" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/package.xml">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1044">
<caret line="58" column="0" selection-start-line="58" selection-start-column="0" selection-end-line="58" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/msg/RoverMotorDrive.msg">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="108">
<caret line="6" column="25" selection-start-line="6" selection-start-column="25" selection-end-line="6" selection-end-column="25" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="8" column="0" selection-start-line="8" selection-start-column="0" selection-end-line="8" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="288">
<caret line="16" column="0" selection-start-line="16" selection-start-column="0" selection-end-line="18" selection-end-column="12" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1602">
<caret line="94" column="107" selection-start-line="94" selection-start-column="107" selection-end-line="94" selection-end-column="107" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="0">
<caret line="0" column="0" selection-start-line="0" selection-start-column="0" selection-end-line="0" selection-end-column="0" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/package.xml">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1044">
<caret line="58" column="0" selection-start-line="58" selection-start-column="0" selection-end-line="58" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/msg/RoverMotorDrive.msg">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="108">
<caret line="6" column="25" selection-start-line="6" selection-start-column="25" selection-end-line="6" selection-end-column="25" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="8" column="0" selection-start-line="8" selection-start-column="0" selection-end-line="8" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="252">
<caret line="16" column="0" selection-start-line="16" selection-start-column="0" selection-end-line="18" selection-end-column="12" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1602">
<caret line="94" column="107" selection-start-line="94" selection-start-column="107" selection-end-line="94" selection-end-column="107" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="0">
<caret line="0" column="0" selection-start-line="0" selection-start-column="0" selection-end-line="0" selection-end-column="0" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/package.xml">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1044">
<caret line="58" column="0" selection-start-line="58" selection-start-column="0" selection-end-line="58" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/msg/RoverMotorDrive.msg">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="108">
<caret line="6" column="25" selection-start-line="6" selection-start-column="25" selection-end-line="6" selection-end-column="25" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="8" column="0" selection-start-line="8" selection-start-column="0" selection-end-line="8" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="252">
<caret line="16" column="0" selection-start-line="16" selection-start-column="0" selection-end-line="18" selection-end-column="12" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1602">
<caret line="94" column="107" selection-start-line="94" selection-start-column="107" selection-end-line="94" selection-end-column="107" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="0">
<caret line="0" column="0" selection-start-line="0" selection-start-column="0" selection-end-line="0" selection-end-column="0" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/package.xml">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1044">
<caret line="58" column="0" selection-start-line="58" selection-start-column="0" selection-end-line="58" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/msg/RoverMotorDrive.msg">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="108">
<caret line="6" column="25" selection-start-line="6" selection-start-column="25" selection-end-line="6" selection-end-column="25" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="8" column="0" selection-start-line="8" selection-start-column="0" selection-end-line="8" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="0">
<caret line="0" column="0" selection-start-line="0" selection-start-column="0" selection-end-line="0" selection-end-column="0" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/msg/RoverMotorDrive.msg">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="108">
<caret line="6" column="25" selection-start-line="6" selection-start-column="25" selection-end-line="6" selection-end-column="25" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="72">
<caret line="4" column="32" selection-start-line="4" selection-start-column="32" selection-end-line="4" selection-end-column="32" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/CMakeLists.txt">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="360">
<caret line="71" column="20" selection-start-line="71" selection-start-column="20" selection-end-line="71" selection-end-column="20" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/msg/RoverMotorDrive.msg">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="108">
<caret line="6" column="25" selection-start-line="6" selection-start-column="25" selection-end-line="6" selection-end-column="25" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/package.xml">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="524">
<caret line="58" column="0" selection-start-line="58" selection-start-column="0" selection-end-line="58" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node_tester.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="144">
<caret line="13" column="17" selection-start-line="13" selection-start-column="17" selection-end-line="13" selection-end-column="17" />
<folding>
<element signature="e#0#12#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/rover_drive_node.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1134">
<caret line="63" column="23" selection-start-line="63" selection-start-column="23" selection-end-line="63" selection-end-column="23" />
<folding>
<element signature="e#132#151#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$USER_HOME$/PycharmProjects/485_test/test.py">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="36">
<caret line="4" column="0" selection-start-line="4" selection-start-column="0" selection-end-line="22" selection-end-column="0" />
<folding />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/launch/example.launch">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="54">
<caret line="3" column="54" selection-start-line="3" selection-start-column="54" selection-end-line="3" selection-end-column="54" />
<folding />
</state>
</provider>
</entry>
</component>
</project>

8
software/rover/rover_drive/launch/example.launch
View File

@@ -1,8 +0,0 @@
<launch>
<group ns="drive">
<node name="" pkg="rover_drive" type="rover_drive_node.py" launch-prefix="taskset -c 5" output="screen">
<param name="port" value="/dev/rover/ttyIRIS_1_0"/>
<param name="baud" value="2000000"/>
</node>
</group>
</launch>

65
software/rover/rover_drive/package.xml
View File

@@ -1,65 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>rover_drive</name>
<version>0.0.0</version>
<description>The rover_drive package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="caperren@todo.todo">caperren</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rover_drive</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<build_depend>message_generation</build_depend>
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<exec_depend>message_runtime</exec_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

60
software/rover/rover_drive/src/controllertest.py
View File

@@ -1,60 +0,0 @@
import rospy
import time
import atexit
import signal
import serial.rs485
import minimalmodbus
from rover_drive.msg import RoverMotorDrive
signal.signal(signal.SIGINT, signal.SIG_DFL)
rospy.init_node("drive_tester")
pub = rospy.Publisher("/drive/motoroneandtwo", RoverMotorDrive, queue_size=1)
def make_instr(port_name, baud):
instr = minimalmodbus.Instrument(port_name, 1)
instr.serial = serial.rs485.RS485(port_name, baudrate=baud, timeout=0.05)
instr.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=0,
delay_before_tx=0)
return instr
sbus = make_instr("/dev/rover/ttyIRIS_2_2", 2000000)
max = 1811
mid = 990
min = 172
scalar = 75
drive = RoverMotorDrive()
while not rospy.is_shutdown():
left, right = sbus.read_registers(0, 2)
# print left
drive.first_motor_direction = 1 if left > mid else 0
drive.first_motor_speed = abs(left - mid ) * scalar
# print drive.first_motor_speed
pub.publish(drive)
# try:
# drive = RoverMotorDrive()
#
# last_dir = not last_dir
# for i in range(0, 65535, speed_step):
# drive.first_motor_direction = last_dir
# drive.first_motor_speed = i
# pub.publish(drive)
# time.sleep(sleep_time)
# print i
# for i in range(65535, 0, -speed_step):
# drive.first_motor_direction = last_dir
# drive.first_motor_speed = i
# pub.publish(drive)
# time.sleep(sleep_time)
# print i
# except KeyboardInterrupt:
# exit()

108
software/rover/rover_drive/src/rover_drive_node.py
View File

@@ -1,108 +0,0 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import serial.rs485
import minimalmodbus
import time
import rospy
from rover_drive.msg import RoverMotorDrive
# Custom Imports
#####################################
# Global Variables
#####################################
NODE_NAME = "rover_drive"
DEFAULT_PORT = "/dev/ttyUSB0"
DEFAULT_BAUD = 2000000
MOTOR_TOPIC = "motoroneandtwo"
FIRST_MOTOR_ID = 1
SECOND_MOTOR_ID = 2
COMMUNICATIONS_TIMEOUT = 0.3 # Seconds
MOTOR_DRIVERS_DIRECTION = {
"FORWARDS": 1,
"BACKWARDS": 0
}
MOTOR_DRIVER_REGISTER_MAP = {
"DIRECTION": 0,
"SPEED": 1
}
MOTOR_DRIVER_DEFAULT_MESSAGE = [
MOTOR_DRIVERS_DIRECTION["FORWARDS"],
0
]
#####################################
# RoverDrive Class Definition
#####################################
class RoverDrive(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("port", DEFAULT_PORT)
self.baud = rospy.get_param("baud", DEFAULT_BAUD)
self.first_motor_modbus_id = rospy.get_param("first_motor_modbus_id", FIRST_MOTOR_ID)
self.second_motor_modbus_id = rospy.get_param("second_motor_modbus_id", SECOND_MOTOR_ID)
self.motors_subscriber_topic = rospy.get_param("topic", MOTOR_TOPIC)
self.first_motor = minimalmodbus.Instrument(self.port, self.first_motor_modbus_id)
self.second_motor = minimalmodbus.Instrument(self.port, self.second_motor_modbus_id)
print self.port
self.motors_subscriber = rospy.Subscriber(MOTOR_TOPIC, RoverMotorDrive, self.__motor_message_callback)
self.__setup_minimalmodbus_for_485()
self.run()
def run(self):
while not rospy.is_shutdown():
time.sleep(0.25)
# try:
# self.first_motor.write_register(1, 10000)
# except IOError, error:
# pass
#
# try:
# self.second_motor.write_register(1, 30000)
# except IOError, error:
# pass
def __setup_minimalmodbus_for_485(self):
self.first_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.first_motor.serial.rs485_mode = \
serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=0, delay_before_tx=0)
self.second_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.second_motor.serial.rs485_mode = \
serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=0, delay_before_tx=0)
def __motor_message_callback(self, data):
try:
first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) # Makes a copy
first_motor_register_data[MOTOR_DRIVER_REGISTER_MAP["DIRECTION"]] = int(data.first_motor_direction)
first_motor_register_data[MOTOR_DRIVER_REGISTER_MAP["SPEED"]] = data.first_motor_speed
# print first_motor_register_data
self.first_motor.write_registers(0, first_motor_register_data)
except:
print "Drive exception"
#####################################
# Main Definition
#####################################
if __name__ == "__main__":
drive = RoverDrive()

50
software/rover/rover_drive/src/rover_drive_node_tester.py
View File

@@ -1,50 +0,0 @@
import rospy
import time
import atexit
import signal
from rover_drive.msg import RoverMotorDrive
signal.signal(signal.SIGINT, signal.SIG_DFL)
rospy.init_node("drive_tester")
pub = rospy.Publisher("/drive/motoroneandtwo", RoverMotorDrive, queue_size=1)
speed_step = 1000
last_dir = 0
sleep_time = 0.1
def stop_motors():
global pub
drive.first_motor_direction = last_dir
drive.first_motor_speed = 0
pub.publish(drive)
atexit.register(stop_motors)
while not rospy.is_shutdown():
try :
drive = RoverMotorDrive()
last_dir = not last_dir
for i in range(0, 65535, speed_step):
drive.first_motor_direction = last_dir
drive.first_motor_speed = i
pub.publish(drive)
time.sleep(sleep_time)
print i
for i in range(65535, 0, -speed_step):
drive.first_motor_direction = last_dir
drive.first_motor_speed = i
pub.publish(drive)
time.sleep(sleep_time)
print i
except KeyboardInterrupt:
exit()