diff --git a/.gitignore b/.gitignore index 47ef7c19a..5971b57c0 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,6 @@ *.swp rosserial_server/doc/ *.user* +*.out +*.geany + diff --git a/.travis.yml b/.travis.yml index 6b8901f3d..ef24686cf 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,7 +4,7 @@ language: generic env: - CI_ROS_DISTRO=indigo CC=gcc CXX=g++ PATH=/usr/sbin:/usr/bin:/sbin:/bin - - CI_ROS_DISTRO=indigo CC=clang CXX=clang++ PATH=/usr/local/clang-3.9.0/bin:/usr/sbin:/usr/bin:/sbin:/bin + - CI_ROS_DISTRO=indigo CC=clang CXX=clang++ PATH=/usr/local/clang-5.0.0/bin:/usr/sbin:/usr/bin:/sbin:/bin before_install: - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' diff --git a/rosserial/CHANGELOG.rst b/rosserial/CHANGELOG.rst index f8fa2d25c..4fcb236aa 100644 --- a/rosserial/CHANGELOG.rst +++ b/rosserial/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rosserial ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial/package.xml b/rosserial/package.xml index 588b8e205..aaea0cecf 100644 --- a/rosserial/package.xml +++ b/rosserial/package.xml @@ -1,6 +1,6 @@ rosserial - 0.7.6 + 0.7.7 Metapackage for core of rosserial. diff --git a/rosserial_arduino/CHANGELOG.rst b/rosserial_arduino/CHANGELOG.rst index 5626fd1b3..880fc5fef 100644 --- a/rosserial_arduino/CHANGELOG.rst +++ b/rosserial_arduino/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rosserial_arduino ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Add ArduinoTcpHardware to use Arduino Ethernet shield. (`#324 `_) +* Add an example to use Subscriber and ServiceServer as class members. (`#321 `_) +* Added support for the Particle Photon (`#292 `_) +* Fix POLICY CMP0054 unknown (`#291 `_) +* Use ESP8266 header only if defined. (`#288 `_) +* Add Esp8266 support and an example (`#279 `_) +* Add support for STM32F1 with stm32duino. (`#281 `_) +* Contributors: Bei Chen Liu, David Portugal, Dmitry Kargin, Mike Purvis, Romain Reignier, Valentin VERGEZ, khancyr + 0.7.6 (2017-03-01) ------------------ * Fixed issue with CMake CMP0054 (`#273 `_) diff --git a/rosserial_arduino/CMakeLists.txt b/rosserial_arduino/CMakeLists.txt index 592c95246..2a0795c31 100644 --- a/rosserial_arduino/CMakeLists.txt +++ b/rosserial_arduino/CMakeLists.txt @@ -1,34 +1,38 @@ cmake_minimum_required(VERSION 2.8.3) project(rosserial_arduino) -find_package(catkin REQUIRED COMPONENTS message_generation) +find_package(catkin REQUIRED COMPONENTS + message_generation +) add_message_files(FILES - Adc.msg - ) + Adc.msg +) add_service_files(FILES - Test.srv - ) + Test.srv +) + +catkin_python_setup() generate_messages() catkin_package( CATKIN_DEPENDS message_runtime - CFG_EXTRAS rosserial_arduino-extras.cmake + CFG_EXTRAS ${PROJECT_NAME}-extras.cmake ) -install(DIRECTORY src/ros_lib +install( + DIRECTORY src/ros_lib DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src ) -install(DIRECTORY arduino-cmake +install( + DIRECTORY arduino-cmake DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install(PROGRAMS src/rosserial_arduino/make_libraries.py +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_libraries.py nodes/serial_node.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - - - diff --git a/rosserial_arduino/nodes/serial_node.py b/rosserial_arduino/nodes/serial_node.py new file mode 100755 index 000000000..557e3e2e5 --- /dev/null +++ b/rosserial_arduino/nodes/serial_node.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python + +##################################################################### +# Software License Agreement (BSD License) +# +# Copyright (c) 2011, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import rospy +from rosserial_arduino import SerialClient +from serial import SerialException +from time import sleep + +import sys + +if __name__=="__main__": + + rospy.init_node("serial_node") + rospy.loginfo("ROS Serial Python Node") + + port_name = rospy.get_param('~port','/dev/ttyUSB0') + baud = int(rospy.get_param('~baud','57600')) + + # Number of seconds of sync failure after which Arduino is auto-reset. + # 0 = no timeout, auto-reset disabled + auto_reset_timeout = int(rospy.get_param('~auto_reset_timeout','0')) + + # for systems where pyserial yields errors in the fcntl.ioctl(self.fd, TIOCMBIS, \ + # TIOCM_DTR_str) line, which causes an IOError, when using simulated port + fix_pyserial_for_test = rospy.get_param('~fix_pyserial_for_test', False) + + # TODO: do we really want command line params in addition to parameter server params? + sys.argv = rospy.myargv(argv=sys.argv) + if len(sys.argv) >= 2 : + port_name = sys.argv[1] + + while not rospy.is_shutdown(): + rospy.loginfo("Connecting to %s at %d baud" % (port_name, baud)) + try: + client = SerialClient(port_name, baud, fix_pyserial_for_test=fix_pyserial_for_test, auto_reset_timeout=auto_reset_timeout) + client.run() + except KeyboardInterrupt: + break + except SerialException: + sleep(1.0) + continue + except OSError: + sleep(1.0) + continue diff --git a/rosserial_arduino/package.xml b/rosserial_arduino/package.xml index 44f10d93f..18673a938 100644 --- a/rosserial_arduino/package.xml +++ b/rosserial_arduino/package.xml @@ -1,8 +1,8 @@ rosserial_arduino - 0.7.6 + 0.7.7 - Libraries and examples for ROSserial usage on Arduino/AVR Platforms. + rosserial for Arduino/AVR platforms. Michael Ferguson Adam Stambler @@ -20,6 +20,5 @@ rosserial_msgs rosserial_client message_runtime + rosserial_python - - diff --git a/rosserial_arduino/setup.py b/rosserial_arduino/setup.py new file mode 100644 index 000000000..4ef426a5e --- /dev/null +++ b/rosserial_arduino/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['rosserial_arduino'], + package_dir={'': 'src'}, + ) + +setup(**d) diff --git a/rosserial_arduino/src/ros_lib/ArduinoTcpHardware.h b/rosserial_arduino/src/ros_lib/ArduinoTcpHardware.h index 49cb5c596..c98334fea 100644 --- a/rosserial_arduino/src/ros_lib/ArduinoTcpHardware.h +++ b/rosserial_arduino/src/ros_lib/ArduinoTcpHardware.h @@ -38,6 +38,8 @@ #include #if defined(ESP8266) #include +#elif defined(ESP32) + #include // Using Espressif's WiFi.h #else #include #include @@ -57,7 +59,7 @@ class ArduinoHardware { IPAddress getLocalIP() { -#if defined(ESP8266) +#if defined(ESP8266) or defined(ESP32) return tcp_.localIP(); #else return Ethernet.localIP(); @@ -92,7 +94,7 @@ class ArduinoHardware { } protected: -#if defined(ESP8266) +#if defined(ESP8266) or defined(ESP32) WiFiClient tcp_; #else EthernetClient tcp_; diff --git a/rosserial_arduino/src/ros_lib/examples/Blink/Blink.pde b/rosserial_arduino/src/ros_lib/examples/Blink/Blink.pde index 4e9e18500..041c4df11 100644 --- a/rosserial_arduino/src/ros_lib/examples/Blink/Blink.pde +++ b/rosserial_arduino/src/ros_lib/examples/Blink/Blink.pde @@ -9,14 +9,14 @@ ros::NodeHandle nh; void messageCb( const std_msgs::Empty& toggle_msg){ - digitalWrite(13, HIGH-digitalRead(13)); // blink the led + digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN)); // blink the led } ros::Subscriber sub("toggle_led", &messageCb ); void setup() { - pinMode(13, OUTPUT); + pinMode(LED_BUILTIN, OUTPUT); nh.initNode(); nh.subscribe(sub); } diff --git a/rosserial_arduino/src/ros_lib/examples/ServiceClient/client.py b/rosserial_arduino/src/ros_lib/examples/ServiceClient/client.py old mode 100755 new mode 100644 diff --git a/rosserial_arduino/src/ros_lib/ros.h b/rosserial_arduino/src/ros_lib/ros.h index c65b0d01d..8a2c71314 100644 --- a/rosserial_arduino/src/ros_lib/ros.h +++ b/rosserial_arduino/src/ros_lib/ros.h @@ -1,4 +1,4 @@ -/* +/* * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. @@ -37,7 +37,7 @@ #include "ros/node_handle.h" -#if defined(ESP8266) or defined(ROSSERIAL_ARDUINO_TCP) +#if defined(ESP8266) or defined(ESP32) or defined(ROSSERIAL_ARDUINO_TCP) #include "ArduinoTcpHardware.h" #else #include "ArduinoHardware.h" @@ -61,7 +61,7 @@ namespace ros typedef NodeHandle_ NodeHandle; // default 25, 25, 512, 512 -#endif +#endif } #endif diff --git a/rosserial_arduino/src/rosserial_arduino/SerialClient.py b/rosserial_arduino/src/rosserial_arduino/SerialClient.py new file mode 100644 index 000000000..89894ba5e --- /dev/null +++ b/rosserial_arduino/src/rosserial_arduino/SerialClient.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python + +##################################################################### +# Software License Agreement (BSD License) +# +# Copyright (c) 2011, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import time + +import rospy +from std_srvs.srv import Empty, EmptyResponse + +from serial import Serial + +from rosserial_python import SerialClient as _SerialClient + +MINIMUM_RESET_TIME = 30 + +class SerialClient(_SerialClient): + + def __init__(self, *args, **kwargs): + # The number of seconds to wait after a sync failure for a sync success before automatically performing a reset. + # If 0, no reset is performed. + self.auto_reset_timeout = kwargs.pop('auto_reset_timeout', 0) + self.lastsync_reset = rospy.Time.now() + rospy.Service('~reset_arduino', Empty, self.resetArduino) + super(SerialClient, self).__init__(*args, **kwargs) + + def resetArduino(self, *args, **kwargs): + """ + Forces the Arduino to perform a reset, as though its reset button was pressed. + """ + with self.read_lock, self.write_lock: + rospy.loginfo('Beginning Arduino reset on port %s. Closing serial port...' % self.port.portstr) + self.port.close() + with Serial(self.port.portstr) as arduino: + arduino.setDTR(False) + time.sleep(3) + arduino.flushInput() + arduino.setDTR(True) + time.sleep(5) + rospy.loginfo('Reopening serial port...') + self.port.open() + rospy.loginfo('Arduino reset complete.') + self.lastsync_reset = rospy.Time.now() + self.requestTopics() + return EmptyResponse() + + def sendDiagnostics(self, level, msg_text): + super(SerialClient, self).sendDiagnostics(level, msg_text) + # Reset when we haven't received any data from the Arduino in over N seconds. + if self.auto_reset_timeout and (rospy.Time.now() - self.last_read).secs >= self.auto_reset_timeout: + if (rospy.Time.now() - self.lastsync_reset).secs < MINIMUM_RESET_TIME: + rospy.loginfo('Sync has failed, but waiting for last reset to complete.') + else: + rospy.loginfo('Sync has failed for over %s seconds. Initiating automatic reset.' % self.auto_reset_timeout) + self.resetArduino() diff --git a/rosserial_arduino/src/rosserial_arduino/__init__.py b/rosserial_arduino/src/rosserial_arduino/__init__.py new file mode 100644 index 000000000..2146287ff --- /dev/null +++ b/rosserial_arduino/src/rosserial_arduino/__init__.py @@ -0,0 +1 @@ +from SerialClient import * diff --git a/rosserial_arduino/src/rosserial_arduino/make_libraries.py b/rosserial_arduino/src/rosserial_arduino/make_libraries.py index 21eb1af22..05d36d099 100755 --- a/rosserial_arduino/src/rosserial_arduino/make_libraries.py +++ b/rosserial_arduino/src/rosserial_arduino/make_libraries.py @@ -60,7 +60,7 @@ 'int32' : ('int32_t', 4, PrimitiveDataType, []), 'uint32' : ('uint32_t', 4, PrimitiveDataType, []), 'int64' : ('int64_t', 8, PrimitiveDataType, []), - 'uint64' : ('uint64_t', 4, PrimitiveDataType, []), + 'uint64' : ('uint64_t', 8, PrimitiveDataType, []), 'float32' : ('float', 4, PrimitiveDataType, []), 'float64' : ('float', 4, AVR_Float64DataType, []), 'time' : ('ros::Time', 8, TimeDataType, ['ros/time']), diff --git a/rosserial_client/CHANGELOG.rst b/rosserial_client/CHANGELOG.rst index d710dc131..c0a6ebd51 100644 --- a/rosserial_client/CHANGELOG.rst +++ b/rosserial_client/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rosserial_client ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Add overall spin timeout to rosserial read. (`#334 `_) +* Fixing formatting on files. (`#333 `_) +* Fix catkin lint errors (`#296 `_) +* fix spinOnce timeout : 5ms -> 5s (`#326 `_) +* [Client] Fix a warning in comparison. (`#323 `_) +* Use const in ros namespace instead of #define for constants. Fix `#283 `_ (`#318 `_) +* Fix CMP0046 warnings in catkin-built firmwares (`#320 `_) +* Prevent time variable overflow leading to parameter timeout error (`#293 `_) +* Add class member method callback support for Service Server. (`#282 `_) +* Added capability to specify timeout in getParam methods (`#278 `_) +* Contributors: 1r0b1n0, Alessandro Francescon, Bei Chen Liu, David Portugal, Dmitry Kargin, Mike O'Driscoll, Mike Purvis, Romain Reignier + 0.7.6 (2017-03-01) ------------------ * Fixing message has no attribute _md5sum (`#257 `_) diff --git a/rosserial_client/CMakeLists.txt b/rosserial_client/CMakeLists.txt index 39a63c4a8..4fb28e52b 100644 --- a/rosserial_client/CMakeLists.txt +++ b/rosserial_client/CMakeLists.txt @@ -3,21 +3,29 @@ project(rosserial_client) find_package(catkin REQUIRED) catkin_package( - CFG_EXTRAS rosserial_client-extras.cmake + CFG_EXTRAS ${PROJECT_NAME}-extras.cmake ) catkin_python_setup() -install(DIRECTORY src/ros_lib - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src -) -install(PROGRAMS scripts/make_libraries - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - if(CATKIN_ENABLE_TESTING) find_package(rosserial_msgs REQUIRED) include_directories(src ${rosserial_msgs_INCLUDE_DIRS}) catkin_add_gtest(float64_test test/float64_test.cpp) catkin_add_gtest(subscriber_test test/subscriber_test.cpp) endif() + +install( + DIRECTORY src/ros_lib + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +) + +install( + PROGRAMS scripts/make_libraries + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_library.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/rosserial_client/cmake/rosserial_client-extras.cmake b/rosserial_client/cmake/rosserial_client-extras.cmake index 6b84d5810..0884d5e35 100644 --- a/rosserial_client/cmake/rosserial_client-extras.cmake +++ b/rosserial_client/cmake/rosserial_client-extras.cmake @@ -13,7 +13,7 @@ cmake_minimum_required(VERSION 2.8.3) # @public # function(rosserial_generate_ros_lib) - cmake_parse_arguments(make_libraries "" "PACKAGE;SCRIPT" "" ${ARGN}) + cmake_parse_arguments(make_libraries "" "PACKAGE;SCRIPT" "" ${ARGN}) if(NOT make_libraries_PACKAGE) set(make_libraries_PACKAGE rosserial_client) endif() diff --git a/rosserial_client/package.xml b/rosserial_client/package.xml index 8a00fb55e..9318997b6 100644 --- a/rosserial_client/package.xml +++ b/rosserial_client/package.xml @@ -1,6 +1,6 @@ rosserial_client - 0.7.6 + 0.7.7 Generalized client side source for rosserial. @@ -19,5 +19,6 @@ rospy tf + rosunit rosserial_msgs diff --git a/rosserial_client/src/ros_lib/duration.cpp b/rosserial_client/src/ros_lib/duration.cpp index f471c33d2..d2dfdd6f3 100644 --- a/rosserial_client/src/ros_lib/duration.cpp +++ b/rosserial_client/src/ros_lib/duration.cpp @@ -37,45 +37,47 @@ namespace ros { - void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec) - { - int32_t nsec_part = nsec; - int32_t sec_part = sec; +void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec) +{ + int32_t nsec_part = nsec; + int32_t sec_part = sec; - while (nsec_part > 1000000000L) - { - nsec_part -= 1000000000L; - ++sec_part; - } - while (nsec_part < 0) - { - nsec_part += 1000000000L; - --sec_part; - } - sec = sec_part; - nsec = nsec_part; + while (nsec_part > 1000000000L) + { + nsec_part -= 1000000000L; + ++sec_part; } - - Duration& Duration::operator+=(const Duration &rhs) + while (nsec_part < 0) { - sec += rhs.sec; - nsec += rhs.nsec; - normalizeSecNSecSigned(sec, nsec); - return *this; + nsec_part += 1000000000L; + --sec_part; } + sec = sec_part; + nsec = nsec_part; +} - Duration& Duration::operator-=(const Duration &rhs){ - sec += -rhs.sec; - nsec += -rhs.nsec; - normalizeSecNSecSigned(sec, nsec); - return *this; - } +Duration& Duration::operator+=(const Duration &rhs) +{ + sec += rhs.sec; + nsec += rhs.nsec; + normalizeSecNSecSigned(sec, nsec); + return *this; +} - Duration& Duration::operator*=(double scale){ - sec *= scale; - nsec *= scale; - normalizeSecNSecSigned(sec, nsec); - return *this; - } +Duration& Duration::operator-=(const Duration &rhs) +{ + sec += -rhs.sec; + nsec += -rhs.nsec; + normalizeSecNSecSigned(sec, nsec); + return *this; +} + +Duration& Duration::operator*=(double scale) +{ + sec *= scale; + nsec *= scale; + normalizeSecNSecSigned(sec, nsec); + return *this; +} } diff --git a/rosserial_client/src/ros_lib/ros/duration.h b/rosserial_client/src/ros_lib/ros/duration.h index ab889cdd1..5ec6d9003 100644 --- a/rosserial_client/src/ros_lib/ros/duration.h +++ b/rosserial_client/src/ros_lib/ros/duration.h @@ -38,28 +38,36 @@ #include #include -namespace ros { +namespace ros +{ - void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); +void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); - class Duration - { - public: - int32_t sec, nsec; - - Duration() : sec(0), nsec(0) {} - Duration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) - { - normalizeSecNSecSigned(sec, nsec); - } +class Duration +{ +public: + int32_t sec, nsec; - double toSec() const { return (double)sec + 1e-9*(double)nsec; }; - void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); }; + Duration() : sec(0), nsec(0) {} + Duration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalizeSecNSecSigned(sec, nsec); + } - Duration& operator+=(const Duration &rhs); - Duration& operator-=(const Duration &rhs); - Duration& operator*=(double scale); + double toSec() const + { + return (double)sec + 1e-9 * (double)nsec; }; + void fromSec(double t) + { + sec = (uint32_t) floor(t); + nsec = (uint32_t) round((t - sec) * 1e9); + }; + + Duration& operator+=(const Duration &rhs); + Duration& operator-=(const Duration &rhs); + Duration& operator*=(double scale); +}; } diff --git a/rosserial_client/src/ros_lib/ros/msg.h b/rosserial_client/src/ros_lib/ros/msg.h index b19745020..aea0f6f0b 100644 --- a/rosserial_client/src/ros_lib/ros/msg.h +++ b/rosserial_client/src/ros_lib/ros/msg.h @@ -38,7 +38,8 @@ #include #include -namespace ros { +namespace ros +{ /* Base Message Type */ class Msg @@ -110,7 +111,7 @@ class Msg *val |= ((uint32_t)(*inbuffer) & 0x0f) << 19; // Copy truncated exponent. - uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0)>>4; + uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0) >> 4; exp |= ((uint32_t)(*inbuffer) & 0x7f) << 4; if (exp != 0) { @@ -127,7 +128,7 @@ class Msg template static void varToArr(A arr, const V var) { - for(size_t i = 0; i < sizeof(V); i++) + for (size_t i = 0; i < sizeof(V); i++) arr[i] = (var >> (8 * i)); } @@ -136,7 +137,7 @@ class Msg static void arrToVar(V& var, const A arr) { var = 0; - for(size_t i = 0; i < sizeof(V); i++) + for (size_t i = 0; i < sizeof(V); i++) var |= (arr[i] << (8 * i)); } diff --git a/rosserial_client/src/ros_lib/ros/node_handle.h b/rosserial_client/src/ros_lib/ros/node_handle.h index 4f1459fd1..8fa27b19b 100644 --- a/rosserial_client/src/ros_lib/ros/node_handle.h +++ b/rosserial_client/src/ros_lib/ros/node_handle.h @@ -44,14 +44,16 @@ #include "ros/msg.h" -namespace ros { - - class NodeHandleBase_{ - public: - virtual int publish(int id, const Msg* msg)=0; - virtual int spinOnce()=0; - virtual bool connected()=0; - }; +namespace ros +{ + +class NodeHandleBase_ +{ +public: + virtual int publish(int id, const Msg* msg) = 0; + virtual int spinOnce() = 0; + virtual bool connected() = 0; +}; } #include "ros/publisher.h" @@ -59,492 +61,625 @@ namespace ros { #include "ros/service_server.h" #include "ros/service_client.h" -namespace ros { +namespace ros +{ - const uint8_t SYNC_SECONDS = 5; - const uint8_t MODE_FIRST_FF = 0; - /* - * The second sync byte is a protocol version. It's value is 0xff for the first - * version of the rosserial protocol (used up to hydro), 0xfe for the second version - * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable - * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy - * rosserial_arduino. It must be changed in both this file and in - * rosserial_python/src/rosserial_python/SerialClient.py - */ - const uint8_t MODE_PROTOCOL_VER = 1; - const uint8_t PROTOCOL_VER1 = 0xff; // through groovy - const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro - const uint8_t PROTOCOL_VER = PROTOCOL_VER2; - const uint8_t MODE_SIZE_L = 2; - const uint8_t MODE_SIZE_H = 3; - const uint8_t MODE_SIZE_CHECKSUM = 4; // checksum for msg size received from size L and H - const uint8_t MODE_TOPIC_L = 5; // waiting for topic id - const uint8_t MODE_TOPIC_H = 6; - const uint8_t MODE_MESSAGE = 7; - const uint8_t MODE_MSG_CHECKSUM = 8; // checksum for msg and topic id - - - const uint8_t SERIAL_MSG_TIMEOUT = 20; // 20 milliseconds to recieve all of message data - - using rosserial_msgs::TopicInfo; - - /* Node Handle */ - template - class NodeHandle_ : public NodeHandleBase_ - { - protected: - Hardware hardware_; +const int SPIN_OK = 0; +const int SPIN_ERR = -1; +const int SPIN_TIMEOUT = -2; - /* time used for syncing */ - uint32_t rt_time; - - /* used for computing current time */ - uint32_t sec_offset, nsec_offset; - - uint8_t message_in[INPUT_SIZE]; - uint8_t message_out[OUTPUT_SIZE]; +const uint8_t SYNC_SECONDS = 5; +const uint8_t MODE_FIRST_FF = 0; +/* + * The second sync byte is a protocol version. It's value is 0xff for the first + * version of the rosserial protocol (used up to hydro), 0xfe for the second version + * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable + * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy + * rosserial_arduino. It must be changed in both this file and in + * rosserial_python/src/rosserial_python/SerialClient.py + */ +const uint8_t MODE_PROTOCOL_VER = 1; +const uint8_t PROTOCOL_VER1 = 0xff; // through groovy +const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro +const uint8_t PROTOCOL_VER = PROTOCOL_VER2; +const uint8_t MODE_SIZE_L = 2; +const uint8_t MODE_SIZE_H = 3; +const uint8_t MODE_SIZE_CHECKSUM = 4; // checksum for msg size received from size L and H +const uint8_t MODE_TOPIC_L = 5; // waiting for topic id +const uint8_t MODE_TOPIC_H = 6; +const uint8_t MODE_MESSAGE = 7; +const uint8_t MODE_MSG_CHECKSUM = 8; // checksum for msg and topic id - Publisher * publishers[MAX_PUBLISHERS]; - Subscriber_ * subscribers[MAX_SUBSCRIBERS]; - /* - * Setup Functions - */ - public: - NodeHandle_() : configured_(false) { +const uint8_t SERIAL_MSG_TIMEOUT = 20; // 20 milliseconds to recieve all of message data - for(unsigned int i=0; i< MAX_PUBLISHERS; i++) - publishers[i] = 0; +using rosserial_msgs::TopicInfo; - for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++) - subscribers[i] = 0; +/* Node Handle */ +template +class NodeHandle_ : public NodeHandleBase_ +{ +protected: + Hardware hardware_; - for(unsigned int i=0; i< INPUT_SIZE; i++) - message_in[i] = 0; + /* time used for syncing */ + uint32_t rt_time; - for(unsigned int i=0; i< OUTPUT_SIZE; i++) - message_out[i] = 0; + /* used for computing current time */ + uint32_t sec_offset, nsec_offset; - req_param_resp.ints_length = 0; - req_param_resp.ints = NULL; - req_param_resp.floats_length = 0; - req_param_resp.floats = NULL; - req_param_resp.ints_length = 0; - req_param_resp.ints = NULL; - } + /* Spinonce maximum work timeout */ + uint32_t spin_timeout_; - Hardware* getHardware(){ - return &hardware_; - } + uint8_t message_in[INPUT_SIZE]; + uint8_t message_out[OUTPUT_SIZE]; - /* Start serial, initialize buffers */ - void initNode(){ - hardware_.init(); - mode_ = 0; - bytes_ = 0; - index_ = 0; - topic_ = 0; - }; - - /* Start a named port, which may be network server IP, initialize buffers */ - void initNode(char *portName){ - hardware_.init(portName); - mode_ = 0; - bytes_ = 0; - index_ = 0; - topic_ = 0; - }; + Publisher * publishers[MAX_PUBLISHERS]; + Subscriber_ * subscribers[MAX_SUBSCRIBERS]; - protected: - //State machine variables for spinOnce - int mode_; - int bytes_; - int topic_; - int index_; - int checksum_; - - bool configured_; + /* + * Setup Functions + */ +public: + NodeHandle_() : configured_(false) + { - /* used for syncing the time */ - uint32_t last_sync_time; - uint32_t last_sync_receive_time; - uint32_t last_msg_timeout_time; + for (unsigned int i = 0; i < MAX_PUBLISHERS; i++) + publishers[i] = 0; - public: - /* This function goes in your loop() function, it handles - * serial input and callbacks for subscribers. - */ + for (unsigned int i = 0; i < MAX_SUBSCRIBERS; i++) + subscribers[i] = 0; + for (unsigned int i = 0; i < INPUT_SIZE; i++) + message_in[i] = 0; - virtual int spinOnce(){ + for (unsigned int i = 0; i < OUTPUT_SIZE; i++) + message_out[i] = 0; - /* restart if timed out */ - uint32_t c_time = hardware_.time(); - if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ - configured_ = false; - } + req_param_resp.ints_length = 0; + req_param_resp.ints = NULL; + req_param_resp.floats_length = 0; + req_param_resp.floats = NULL; + req_param_resp.ints_length = 0; + req_param_resp.ints = NULL; - /* reset if message has timed out */ - if ( mode_ != MODE_FIRST_FF){ - if (c_time > last_msg_timeout_time){ - mode_ = MODE_FIRST_FF; - } - } + spin_timeout_ = 0; + } - /* while available buffer, read data */ - while( true ) - { - int data = hardware_.read(); - if( data < 0 ) - break; - checksum_ += data; - if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ - message_in[index_++] = data; - bytes_--; - if(bytes_ == 0) /* is message complete? if so, checksum */ - mode_ = MODE_MSG_CHECKSUM; - }else if( mode_ == MODE_FIRST_FF ){ - if(data == 0xff){ - mode_++; - last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT; - } - else if( hardware_.time() - c_time > (SYNC_SECONDS*1000)){ - /* We have been stuck in spinOnce too long, return error */ - configured_=false; - return -2; - } - }else if( mode_ == MODE_PROTOCOL_VER ){ - if(data == PROTOCOL_VER){ - mode_++; - }else{ - mode_ = MODE_FIRST_FF; - if (configured_ == false) - requestSyncTime(); /* send a msg back showing our protocol version */ - } - }else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */ - bytes_ = data; - index_ = 0; - mode_++; - checksum_ = data; /* first byte for calculating size checksum */ - }else if( mode_ == MODE_SIZE_H ){ /* top half of message size */ - bytes_ += data<<8; - mode_++; - }else if( mode_ == MODE_SIZE_CHECKSUM ){ - if( (checksum_%256) == 255) - mode_++; - else - mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ - }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ - topic_ = data; - mode_++; - checksum_ = data; /* first byte included in checksum */ - }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ - topic_ += data<<8; - mode_ = MODE_MESSAGE; - if(bytes_ == 0) - mode_ = MODE_MSG_CHECKSUM; - }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ - mode_ = MODE_FIRST_FF; - if( (checksum_%256) == 255){ - if(topic_ == TopicInfo::ID_PUBLISHER){ - requestSyncTime(); - negotiateTopics(); - last_sync_time = c_time; - last_sync_receive_time = c_time; - return -1; - }else if(topic_ == TopicInfo::ID_TIME){ - syncTime(message_in); - }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ - req_param_resp.deserialize(message_in); - param_recieved= true; - }else if(topic_ == TopicInfo::ID_TX_STOP){ - configured_ = false; - }else{ - if(subscribers[topic_-100]) - subscribers[topic_-100]->callback( message_in ); - } - } - } - } - - /* occasionally sync time */ - if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ - requestSyncTime(); - last_sync_time = c_time; - } + Hardware* getHardware() + { + return &hardware_; + } - return 0; - } + /* Start serial, initialize buffers */ + void initNode() + { + hardware_.init(); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; + }; + /* Start a named port, which may be network server IP, initialize buffers */ + void initNode(char *portName) + { + hardware_.init(portName); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; + }; - /* Are we connected to the PC? */ - virtual bool connected() { - return configured_; - }; + /** + * @brief Sets the maximum time in millisconds that spinOnce() can work. + * This will not effect the processing of the buffer, as spinOnce processes + * one byte at a time. It simply sets the maximum time that one call can + * process for. You can choose to clear the buffer if that is beneficial if + * SPIN_TIMEOUT is returned from spinOnce(). + * @param timeout The timeout in milliseconds that spinOnce will function. + */ + void setSpinTimeout(const uint32_t& timeout) + { + spin_timeout_ = timeout; + } + +protected: + //State machine variables for spinOnce + int mode_; + int bytes_; + int topic_; + int index_; + int checksum_; + + bool configured_; + + /* used for syncing the time */ + uint32_t last_sync_time; + uint32_t last_sync_receive_time; + uint32_t last_msg_timeout_time; + +public: + /* This function goes in your loop() function, it handles + * serial input and callbacks for subscribers. + */ - /******************************************************************** - * Time functions - */ - void requestSyncTime() + virtual int spinOnce() + { + /* restart if timed out */ + uint32_t c_time = hardware_.time(); + if ((c_time - last_sync_receive_time) > (SYNC_SECONDS * 2200)) + { + configured_ = false; + } + + /* reset if message has timed out */ + if (mode_ != MODE_FIRST_FF) + { + if (c_time > last_msg_timeout_time) { - std_msgs::Time t; - publish(TopicInfo::ID_TIME, &t); - rt_time = hardware_.time(); + mode_ = MODE_FIRST_FF; } + } - void syncTime(uint8_t * data) + /* while available buffer, read data */ + while (true) + { + // If a timeout has been specified, check how long spinOnce has been running. + if (spin_timeout_ > 0) { - std_msgs::Time t; - uint32_t offset = hardware_.time() - rt_time; - - t.deserialize(data); - t.data.sec += offset/1000; - t.data.nsec += (offset%1000)*1000000UL; - - this->setNow(t.data); - last_sync_receive_time = hardware_.time(); + // If the maximum processing timeout has been exceeded, exit with error. + // The next spinOnce can continue where it left off, or optionally + // based on the application in use, the hardware buffer could be flushed + // and start fresh. + if ((hardware_.time() - c_time) > spin_timeout_) + { + // Exit the spin, processing timeout exceeded. + return SPIN_TIMEOUT; + } } - - Time now() + int data = hardware_.read(); + if (data < 0) + break; + checksum_ += data; + if (mode_ == MODE_MESSAGE) /* message data being recieved */ { - uint32_t ms = hardware_.time(); - Time current_time; - current_time.sec = ms/1000 + sec_offset; - current_time.nsec = (ms%1000)*1000000UL + nsec_offset; - normalizeSecNSec(current_time.sec, current_time.nsec); - return current_time; + message_in[index_++] = data; + bytes_--; + if (bytes_ == 0) /* is message complete? if so, checksum */ + mode_ = MODE_MSG_CHECKSUM; } - - void setNow( Time & new_now ) + else if (mode_ == MODE_FIRST_FF) { - uint32_t ms = hardware_.time(); - sec_offset = new_now.sec - ms/1000 - 1; - nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; - normalizeSecNSec(sec_offset, nsec_offset); + if (data == 0xff) + { + mode_++; + last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT; + } + else if (hardware_.time() - c_time > (SYNC_SECONDS * 1000)) + { + /* We have been stuck in spinOnce too long, return error */ + configured_ = false; + return SPIN_TIMEOUT; + } } - - /******************************************************************** - * Topic Management - */ - - /* Register a new publisher */ - bool advertise(Publisher & p) + else if (mode_ == MODE_PROTOCOL_VER) { - for(int i = 0; i < MAX_PUBLISHERS; i++){ - if(publishers[i] == 0){ // empty slot - publishers[i] = &p; - p.id_ = i+100+MAX_SUBSCRIBERS; - p.nh_ = this; - return true; - } + if (data == PROTOCOL_VER) + { + mode_++; } - return false; - } - - /* Register a new subscriber */ - template - bool subscribe(SubscriberT& s){ - for(int i = 0; i < MAX_SUBSCRIBERS; i++){ - if(subscribers[i] == 0){ // empty slot - subscribers[i] = static_cast(&s); - s.id_ = i+100; - return true; - } + else + { + mode_ = MODE_FIRST_FF; + if (configured_ == false) + requestSyncTime(); /* send a msg back showing our protocol version */ } - return false; } - - /* Register a new Service Server */ - template - bool advertiseService(ServiceServer& srv){ - bool v = advertise(srv.pub); - for(int i = 0; i < MAX_SUBSCRIBERS; i++){ - if(subscribers[i] == 0){ // empty slot - subscribers[i] = static_cast(&srv); - srv.id_ = i+100; - return v; - } - } - return false; + else if (mode_ == MODE_SIZE_L) /* bottom half of message size */ + { + bytes_ = data; + index_ = 0; + mode_++; + checksum_ = data; /* first byte for calculating size checksum */ } - - /* Register a new Service Client */ - template - bool serviceClient(ServiceClient& srv){ - bool v = advertise(srv.pub); - for(int i = 0; i < MAX_SUBSCRIBERS; i++){ - if(subscribers[i] == 0){ // empty slot - subscribers[i] = static_cast(&srv); - srv.id_ = i+100; - return v; - } - } - return false; + else if (mode_ == MODE_SIZE_H) /* top half of message size */ + { + bytes_ += data << 8; + mode_++; } - - void negotiateTopics() + else if (mode_ == MODE_SIZE_CHECKSUM) + { + if ((checksum_ % 256) == 255) + mode_++; + else + mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ + } + else if (mode_ == MODE_TOPIC_L) /* bottom half of topic id */ { - rosserial_msgs::TopicInfo ti; - int i; - for(i = 0; i < MAX_PUBLISHERS; i++) + topic_ = data; + mode_++; + checksum_ = data; /* first byte included in checksum */ + } + else if (mode_ == MODE_TOPIC_H) /* top half of topic id */ + { + topic_ += data << 8; + mode_ = MODE_MESSAGE; + if (bytes_ == 0) + mode_ = MODE_MSG_CHECKSUM; + } + else if (mode_ == MODE_MSG_CHECKSUM) /* do checksum */ + { + mode_ = MODE_FIRST_FF; + if ((checksum_ % 256) == 255) { - if(publishers[i] != 0) // non-empty slot + if (topic_ == TopicInfo::ID_PUBLISHER) { - ti.topic_id = publishers[i]->id_; - ti.topic_name = (char *) publishers[i]->topic_; - ti.message_type = (char *) publishers[i]->msg_->getType(); - ti.md5sum = (char *) publishers[i]->msg_->getMD5(); - ti.buffer_size = OUTPUT_SIZE; - publish( publishers[i]->getEndpointType(), &ti ); + requestSyncTime(); + negotiateTopics(); + last_sync_time = c_time; + last_sync_receive_time = c_time; + return SPIN_ERR; } - } - for(i = 0; i < MAX_SUBSCRIBERS; i++) - { - if(subscribers[i] != 0) // non-empty slot + else if (topic_ == TopicInfo::ID_TIME) + { + syncTime(message_in); + } + else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) + { + req_param_resp.deserialize(message_in); + param_recieved = true; + } + else if (topic_ == TopicInfo::ID_TX_STOP) { - ti.topic_id = subscribers[i]->id_; - ti.topic_name = (char *) subscribers[i]->topic_; - ti.message_type = (char *) subscribers[i]->getMsgType(); - ti.md5sum = (char *) subscribers[i]->getMsgMD5(); - ti.buffer_size = INPUT_SIZE; - publish( subscribers[i]->getEndpointType(), &ti ); + configured_ = false; + } + else + { + if (subscribers[topic_ - 100]) + subscribers[topic_ - 100]->callback(message_in); } } - configured_ = true; } + } + + /* occasionally sync time */ + if (configured_ && ((c_time - last_sync_time) > (SYNC_SECONDS * 500))) + { + requestSyncTime(); + last_sync_time = c_time; + } + + return SPIN_OK; + } - virtual int publish(int id, const Msg * msg) + + /* Are we connected to the PC? */ + virtual bool connected() + { + return configured_; + }; + + /******************************************************************** + * Time functions + */ + + void requestSyncTime() + { + std_msgs::Time t; + publish(TopicInfo::ID_TIME, &t); + rt_time = hardware_.time(); + } + + void syncTime(uint8_t * data) + { + std_msgs::Time t; + uint32_t offset = hardware_.time() - rt_time; + + t.deserialize(data); + t.data.sec += offset / 1000; + t.data.nsec += (offset % 1000) * 1000000UL; + + this->setNow(t.data); + last_sync_receive_time = hardware_.time(); + } + + Time now() + { + uint32_t ms = hardware_.time(); + Time current_time; + current_time.sec = ms / 1000 + sec_offset; + current_time.nsec = (ms % 1000) * 1000000UL + nsec_offset; + normalizeSecNSec(current_time.sec, current_time.nsec); + return current_time; + } + + void setNow(Time & new_now) + { + uint32_t ms = hardware_.time(); + sec_offset = new_now.sec - ms / 1000 - 1; + nsec_offset = new_now.nsec - (ms % 1000) * 1000000UL + 1000000000UL; + normalizeSecNSec(sec_offset, nsec_offset); + } + + /******************************************************************** + * Topic Management + */ + + /* Register a new publisher */ + bool advertise(Publisher & p) + { + for (int i = 0; i < MAX_PUBLISHERS; i++) + { + if (publishers[i] == 0) // empty slot { - if(id >= 100 && !configured_) - return 0; - - /* serialize message */ - int l = msg->serialize(message_out+7); - - /* setup the header */ - message_out[0] = 0xff; - message_out[1] = PROTOCOL_VER; - message_out[2] = (uint8_t) ((uint16_t)l&255); - message_out[3] = (uint8_t) ((uint16_t)l>>8); - message_out[4] = 255 - ((message_out[2] + message_out[3])%256); - message_out[5] = (uint8_t) ((int16_t)id&255); - message_out[6] = (uint8_t) ((int16_t)id>>8); - - /* calculate checksum */ - int chk = 0; - for(int i =5; i + bool subscribe(SubscriberT& s) + { + for (int i = 0; i < MAX_SUBSCRIBERS; i++) + { + if (subscribers[i] == 0) // empty slot + { + subscribers[i] = static_cast(&s); + s.id_ = i + 100; + return true; } + } + return false; + } - public: - void logdebug(const char* msg){ - log(rosserial_msgs::Log::ROSDEBUG, msg); + /* Register a new Service Server */ + template + bool advertiseService(ServiceServer& srv) + { + bool v = advertise(srv.pub); + for (int i = 0; i < MAX_SUBSCRIBERS; i++) + { + if (subscribers[i] == 0) // empty slot + { + subscribers[i] = static_cast(&srv); + srv.id_ = i + 100; + return v; } - void loginfo(const char * msg){ - log(rosserial_msgs::Log::INFO, msg); + } + return false; + } + + /* Register a new Service Client */ + template + bool serviceClient(ServiceClient& srv) + { + bool v = advertise(srv.pub); + for (int i = 0; i < MAX_SUBSCRIBERS; i++) + { + if (subscribers[i] == 0) // empty slot + { + subscribers[i] = static_cast(&srv); + srv.id_ = i + 100; + return v; } - void logwarn(const char *msg){ - log(rosserial_msgs::Log::WARN, msg); + } + return false; + } + + void negotiateTopics() + { + rosserial_msgs::TopicInfo ti; + int i; + for (i = 0; i < MAX_PUBLISHERS; i++) + { + if (publishers[i] != 0) // non-empty slot + { + ti.topic_id = publishers[i]->id_; + ti.topic_name = (char *) publishers[i]->topic_; + ti.message_type = (char *) publishers[i]->msg_->getType(); + ti.md5sum = (char *) publishers[i]->msg_->getMD5(); + ti.buffer_size = OUTPUT_SIZE; + publish(publishers[i]->getEndpointType(), &ti); } - void logerror(const char*msg){ - log(rosserial_msgs::Log::ERROR, msg); + } + for (i = 0; i < MAX_SUBSCRIBERS; i++) + { + if (subscribers[i] != 0) // non-empty slot + { + ti.topic_id = subscribers[i]->id_; + ti.topic_name = (char *) subscribers[i]->topic_; + ti.message_type = (char *) subscribers[i]->getMsgType(); + ti.md5sum = (char *) subscribers[i]->getMsgMD5(); + ti.buffer_size = INPUT_SIZE; + publish(subscribers[i]->getEndpointType(), &ti); } - void logfatal(const char*msg){ - log(rosserial_msgs::Log::FATAL, msg); + } + configured_ = true; + } + + virtual int publish(int id, const Msg * msg) + { + if (id >= 100 && !configured_) + return 0; + + /* serialize message */ + int l = msg->serialize(message_out + 7); + + /* setup the header */ + message_out[0] = 0xff; + message_out[1] = PROTOCOL_VER; + message_out[2] = (uint8_t)((uint16_t)l & 255); + message_out[3] = (uint8_t)((uint16_t)l >> 8); + message_out[4] = 255 - ((message_out[2] + message_out[3]) % 256); + message_out[5] = (uint8_t)((int16_t)id & 255); + message_out[6] = (uint8_t)((int16_t)id >> 8); + + /* calculate checksum */ + int chk = 0; + for (int i = 5; i < l + 7; i++) + chk += message_out[i]; + l += 7; + message_out[l++] = 255 - (chk % 256); + + if (l <= OUTPUT_SIZE) + { + hardware_.write(message_out, l); + return l; + } + else + { + logerror("Message from device dropped: message larger than buffer."); + return -1; + } + } + + /******************************************************************** + * Logging + */ + +private: + void log(char byte, const char * msg) + { + rosserial_msgs::Log l; + l.level = byte; + l.msg = (char*)msg; + publish(rosserial_msgs::TopicInfo::ID_LOG, &l); + } + +public: + void logdebug(const char* msg) + { + log(rosserial_msgs::Log::ROSDEBUG, msg); + } + void loginfo(const char * msg) + { + log(rosserial_msgs::Log::INFO, msg); + } + void logwarn(const char *msg) + { + log(rosserial_msgs::Log::WARN, msg); + } + void logerror(const char*msg) + { + log(rosserial_msgs::Log::ERROR, msg); + } + void logfatal(const char*msg) + { + log(rosserial_msgs::Log::FATAL, msg); + } + + /******************************************************************** + * Parameters + */ + +private: + bool param_recieved; + rosserial_msgs::RequestParamResponse req_param_resp; + + bool requestParam(const char * name, int time_out = 1000) + { + param_recieved = false; + rosserial_msgs::RequestParamRequest req; + req.name = (char*)name; + publish(TopicInfo::ID_PARAMETER_REQUEST, &req); + uint32_t end_time = hardware_.time() + time_out; + while (!param_recieved) + { + spinOnce(); + if (hardware_.time() > end_time) + { + logwarn("Failed to get param: timeout expired"); + return false; } + } + return true; + } - /******************************************************************** - * Parameters - */ - - private: - bool param_recieved; - rosserial_msgs::RequestParamResponse req_param_resp; - - bool requestParam(const char * name, int time_out = 1000){ - param_recieved = false; - rosserial_msgs::RequestParamRequest req; - req.name = (char*)name; - publish(TopicInfo::ID_PARAMETER_REQUEST, &req); - uint32_t end_time = hardware_.time() + time_out; - while(!param_recieved ){ - spinOnce(); - if (hardware_.time() > end_time) { - logwarn("Failed to get param: timeout expired"); - return false; - } - } +public: + bool getParam(const char* name, int* param, int length = 1, int timeout = 1000) + { + if (requestParam(name, timeout)) + { + if (length == req_param_resp.ints_length) + { + //copy it over + for (int i = 0; i < length; i++) + param[i] = req_param_resp.ints[i]; return true; } - - public: - bool getParam(const char* name, int* param, int length =1, int timeout = 1000){ - if (requestParam(name, timeout) ){ - if (length == req_param_resp.ints_length){ - //copy it over - for(int i=0; ipublish(id_, msg); }; - int getEndpointType(){ return endpoint_; } + int publish(const Msg * msg) + { + return nh_->publish(id_, msg); + }; + int getEndpointType() + { + return endpoint_; + } - const char * topic_; - Msg *msg_; - // id_ and no_ are set by NodeHandle when we advertise - int id_; - NodeHandleBase_* nh_; + const char * topic_; + Msg *msg_; + // id_ and no_ are set by NodeHandle when we advertise + int id_; + NodeHandleBase_* nh_; - private: - int endpoint_; - }; +private: + int endpoint_; +}; } diff --git a/rosserial_client/src/ros_lib/ros/service_client.h b/rosserial_client/src/ros_lib/ros/service_client.h index 2031fb17e..3494d96da 100644 --- a/rosserial_client/src/ros_lib/ros/service_client.h +++ b/rosserial_client/src/ros_lib/ros/service_client.h @@ -1,4 +1,4 @@ -/* +/* * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. @@ -40,43 +40,55 @@ #include "ros/publisher.h" #include "ros/subscriber.h" -namespace ros { +namespace ros +{ - template - class ServiceClient : public Subscriber_ { - public: - ServiceClient(const char* topic_name) : - pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) - { - this->topic_ = topic_name; - this->waiting = true; - } +template +class ServiceClient : public Subscriber_ +{ +public: + ServiceClient(const char* topic_name) : + pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) + { + this->topic_ = topic_name; + this->waiting = true; + } - virtual void call(const MReq & request, MRes & response) - { - if(!pub.nh_->connected()) return; - ret = &response; - waiting = true; - pub.publish(&request); - while(waiting && pub.nh_->connected()) - if(pub.nh_->spinOnce() < 0) break; - } + virtual void call(const MReq & request, MRes & response) + { + if (!pub.nh_->connected()) return; + ret = &response; + waiting = true; + pub.publish(&request); + while (waiting && pub.nh_->connected()) + if (pub.nh_->spinOnce() < 0) break; + } - // these refer to the subscriber - virtual void callback(unsigned char *data){ - ret->deserialize(data); - waiting = false; - } - virtual const char * getMsgType(){ return this->resp.getType(); } - virtual const char * getMsgMD5(){ return this->resp.getMD5(); } - virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + // these refer to the subscriber + virtual void callback(unsigned char *data) + { + ret->deserialize(data); + waiting = false; + } + virtual const char * getMsgType() + { + return this->resp.getType(); + } + virtual const char * getMsgMD5() + { + return this->resp.getMD5(); + } + virtual int getEndpointType() + { + return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; + } - MReq req; - MRes resp; - MRes * ret; - bool waiting; - Publisher pub; - }; + MReq req; + MRes resp; + MRes * ret; + bool waiting; + Publisher pub; +}; } diff --git a/rosserial_client/src/ros_lib/ros/service_server.h b/rosserial_client/src/ros_lib/ros/service_server.h index 4903cb2e5..459d66ebf 100644 --- a/rosserial_client/src/ros_lib/ros/service_server.h +++ b/rosserial_client/src/ros_lib/ros/service_server.h @@ -1,4 +1,4 @@ -/* +/* * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. @@ -40,67 +40,90 @@ #include "ros/publisher.h" #include "ros/subscriber.h" -namespace ros { +namespace ros +{ - template - class ServiceServer : public Subscriber_ { - public: - typedef void(ObjT::*CallbackT)(const MReq&, MRes&); +template +class ServiceServer : public Subscriber_ +{ +public: + typedef void(ObjT::*CallbackT)(const MReq&, MRes&); - ServiceServer(const char* topic_name, CallbackT cb, ObjT* obj) : - pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER), - obj_(obj) - { - this->topic_ = topic_name; - this->cb_ = cb; - } + ServiceServer(const char* topic_name, CallbackT cb, ObjT* obj) : + pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER), + obj_(obj) + { + this->topic_ = topic_name; + this->cb_ = cb; + } - // these refer to the subscriber - virtual void callback(unsigned char *data){ - req.deserialize(data); - (obj_->*cb_)(req,resp); - pub.publish(&resp); - } - virtual const char * getMsgType(){ return this->req.getType(); } - virtual const char * getMsgMD5(){ return this->req.getMD5(); } - virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + // these refer to the subscriber + virtual void callback(unsigned char *data) + { + req.deserialize(data); + (obj_->*cb_)(req, resp); + pub.publish(&resp); + } + virtual const char * getMsgType() + { + return this->req.getType(); + } + virtual const char * getMsgMD5() + { + return this->req.getMD5(); + } + virtual int getEndpointType() + { + return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; + } - MReq req; - MRes resp; - Publisher pub; - private: - CallbackT cb_; - ObjT* obj_; - }; + MReq req; + MRes resp; + Publisher pub; +private: + CallbackT cb_; + ObjT* obj_; +}; - template - class ServiceServer : public Subscriber_ { - public: - typedef void(*CallbackT)(const MReq&, MRes&); +template +class ServiceServer : public Subscriber_ +{ +public: + typedef void(*CallbackT)(const MReq&, MRes&); - ServiceServer(const char* topic_name, CallbackT cb) : - pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER) - { - this->topic_ = topic_name; - this->cb_ = cb; - } + ServiceServer(const char* topic_name, CallbackT cb) : + pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER) + { + this->topic_ = topic_name; + this->cb_ = cb; + } - // these refer to the subscriber - virtual void callback(unsigned char *data){ - req.deserialize(data); - cb_(req,resp); - pub.publish(&resp); - } - virtual const char * getMsgType(){ return this->req.getType(); } - virtual const char * getMsgMD5(){ return this->req.getMD5(); } - virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + // these refer to the subscriber + virtual void callback(unsigned char *data) + { + req.deserialize(data); + cb_(req, resp); + pub.publish(&resp); + } + virtual const char * getMsgType() + { + return this->req.getType(); + } + virtual const char * getMsgMD5() + { + return this->req.getMD5(); + } + virtual int getEndpointType() + { + return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; + } - MReq req; - MRes resp; - Publisher pub; - private: - CallbackT cb_; - }; + MReq req; + MRes resp; + Publisher pub; +private: + CallbackT cb_; +}; } diff --git a/rosserial_client/src/ros_lib/ros/subscriber.h b/rosserial_client/src/ros_lib/ros/subscriber.h index 7b6db4c47..d420bba76 100644 --- a/rosserial_client/src/ros_lib/ros/subscriber.h +++ b/rosserial_client/src/ros_lib/ros/subscriber.h @@ -37,84 +37,103 @@ #include "rosserial_msgs/TopicInfo.h" -namespace ros { - - /* Base class for objects subscribers. */ - class Subscriber_ +namespace ros +{ + +/* Base class for objects subscribers. */ +class Subscriber_ +{ +public: + virtual void callback(unsigned char *data) = 0; + virtual int getEndpointType() = 0; + + // id_ is set by NodeHandle when we advertise + int id_; + + virtual const char * getMsgType() = 0; + virtual const char * getMsgMD5() = 0; + const char * topic_; +}; + +/* Bound function subscriber. */ +template +class Subscriber: public Subscriber_ +{ +public: + typedef void(ObjT::*CallbackT)(const MsgT&); + MsgT msg; + + Subscriber(const char * topic_name, CallbackT cb, ObjT* obj, int endpoint = rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : + cb_(cb), + obj_(obj), + endpoint_(endpoint) { - public: - virtual void callback(unsigned char *data)=0; - virtual int getEndpointType()=0; + topic_ = topic_name; + }; - // id_ is set by NodeHandle when we advertise - int id_; + virtual void callback(unsigned char* data) + { + msg.deserialize(data); + (obj_->*cb_)(msg); + } - virtual const char * getMsgType()=0; - virtual const char * getMsgMD5()=0; - const char * topic_; + virtual const char * getMsgType() + { + return this->msg.getType(); + } + virtual const char * getMsgMD5() + { + return this->msg.getMD5(); + } + virtual int getEndpointType() + { + return endpoint_; + } + +private: + CallbackT cb_; + ObjT* obj_; + int endpoint_; +}; + +/* Standalone function subscriber. */ +template +class Subscriber: public Subscriber_ +{ +public: + typedef void(*CallbackT)(const MsgT&); + MsgT msg; + + Subscriber(const char * topic_name, CallbackT cb, int endpoint = rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : + cb_(cb), + endpoint_(endpoint) + { + topic_ = topic_name; }; - /* Bound function subscriber. */ - template - class Subscriber: public Subscriber_ + virtual void callback(unsigned char* data) { - public: - typedef void(ObjT::*CallbackT)(const MsgT&); - MsgT msg; - - Subscriber(const char * topic_name, CallbackT cb, ObjT* obj, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : - cb_(cb), - obj_(obj), - endpoint_(endpoint) - { - topic_ = topic_name; - }; - - virtual void callback(unsigned char* data) - { - msg.deserialize(data); - (obj_->*cb_)(msg); - } - - virtual const char * getMsgType() { return this->msg.getType(); } - virtual const char * getMsgMD5() { return this->msg.getMD5(); } - virtual int getEndpointType() { return endpoint_; } - - private: - CallbackT cb_; - ObjT* obj_; - int endpoint_; - }; + msg.deserialize(data); + this->cb_(msg); + } - /* Standalone function subscriber. */ - template - class Subscriber: public Subscriber_ + virtual const char * getMsgType() { - public: - typedef void(*CallbackT)(const MsgT&); - MsgT msg; - - Subscriber(const char * topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : - cb_(cb), - endpoint_(endpoint) - { - topic_ = topic_name; - }; - - virtual void callback(unsigned char* data) - { - msg.deserialize(data); - this->cb_(msg); - } - - virtual const char * getMsgType() { return this->msg.getType(); } - virtual const char * getMsgMD5() { return this->msg.getMD5(); } - virtual int getEndpointType() { return endpoint_; } - - private: - CallbackT cb_; - int endpoint_; - }; + return this->msg.getType(); + } + virtual const char * getMsgMD5() + { + return this->msg.getMD5(); + } + virtual int getEndpointType() + { + return endpoint_; + } + +private: + CallbackT cb_; + int endpoint_; +}; } diff --git a/rosserial_client/src/ros_lib/ros/time.h b/rosserial_client/src/ros_lib/ros/time.h index 24958132f..441d95225 100644 --- a/rosserial_client/src/ros_lib/ros/time.h +++ b/rosserial_client/src/ros_lib/ros/time.h @@ -41,31 +41,41 @@ namespace ros { - void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); +void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); - class Time - { - public: - uint32_t sec, nsec; +class Time +{ +public: + uint32_t sec, nsec; - Time() : sec(0), nsec(0) {} - Time(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) - { - normalizeSecNSec(sec, nsec); - } + Time() : sec(0), nsec(0) {} + Time(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalizeSecNSec(sec, nsec); + } - double toSec() const { return (double)sec + 1e-9*(double)nsec; }; - void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); }; + double toSec() const + { + return (double)sec + 1e-9 * (double)nsec; + }; + void fromSec(double t) + { + sec = (uint32_t) floor(t); + nsec = (uint32_t) round((t - sec) * 1e9); + }; - uint32_t toNsec() { return (uint32_t)sec*1000000000ull + (uint32_t)nsec; }; - Time& fromNSec(int32_t t); + uint32_t toNsec() + { + return (uint32_t)sec * 1000000000ull + (uint32_t)nsec; + }; + Time& fromNSec(int32_t t); - Time& operator +=(const Duration &rhs); - Time& operator -=(const Duration &rhs); + Time& operator +=(const Duration &rhs); + Time& operator -=(const Duration &rhs); - static Time now(); - static void setNow( Time & new_now); - }; + static Time now(); + static void setNow(Time & new_now); +}; } diff --git a/rosserial_client/src/ros_lib/tf/tf.h b/rosserial_client/src/ros_lib/tf/tf.h index a2888e3d9..97858fe2e 100644 --- a/rosserial_client/src/ros_lib/tf/tf.h +++ b/rosserial_client/src/ros_lib/tf/tf.h @@ -1,4 +1,4 @@ -/* +/* * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. @@ -39,16 +39,16 @@ namespace tf { - - static inline geometry_msgs::Quaternion createQuaternionFromYaw(double yaw) - { - geometry_msgs::Quaternion q; - q.x = 0; - q.y = 0; - q.z = sin(yaw * 0.5); - q.w = cos(yaw * 0.5); - return q; - } + +static inline geometry_msgs::Quaternion createQuaternionFromYaw(double yaw) +{ + geometry_msgs::Quaternion q; + q.x = 0; + q.y = 0; + q.z = sin(yaw * 0.5); + q.w = cos(yaw * 0.5); + return q; +} } diff --git a/rosserial_client/src/ros_lib/tf/transform_broadcaster.h b/rosserial_client/src/ros_lib/tf/transform_broadcaster.h index caaba817b..6c4e5fee7 100644 --- a/rosserial_client/src/ros_lib/tf/transform_broadcaster.h +++ b/rosserial_client/src/ros_lib/tf/transform_broadcaster.h @@ -1,4 +1,4 @@ -/* +/* * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. @@ -41,27 +41,27 @@ namespace tf { - class TransformBroadcaster - { - public: - TransformBroadcaster() : publisher_("/tf", &internal_msg) {} +class TransformBroadcaster +{ +public: + TransformBroadcaster() : publisher_("/tf", &internal_msg) {} - void init(ros::NodeHandle &nh) - { - nh.advertise(publisher_); - } + void init(ros::NodeHandle &nh) + { + nh.advertise(publisher_); + } - void sendTransform(geometry_msgs::TransformStamped &transform) - { - internal_msg.transforms_length = 1; - internal_msg.transforms = &transform; - publisher_.publish(&internal_msg); - } + void sendTransform(geometry_msgs::TransformStamped &transform) + { + internal_msg.transforms_length = 1; + internal_msg.transforms = &transform; + publisher_.publish(&internal_msg); + } - private: - tf::tfMessage internal_msg; - ros::Publisher publisher_; - }; +private: + tf::tfMessage internal_msg; + ros::Publisher publisher_; +}; } diff --git a/rosserial_client/src/ros_lib/time.cpp b/rosserial_client/src/ros_lib/time.cpp index 934119648..86221f946 100644 --- a/rosserial_client/src/ros_lib/time.cpp +++ b/rosserial_client/src/ros_lib/time.cpp @@ -36,33 +36,35 @@ namespace ros { - void normalizeSecNSec(uint32_t& sec, uint32_t& nsec){ - uint32_t nsec_part= nsec % 1000000000UL; - uint32_t sec_part = nsec / 1000000000UL; - sec += sec_part; - nsec = nsec_part; - } +void normalizeSecNSec(uint32_t& sec, uint32_t& nsec) +{ + uint32_t nsec_part = nsec % 1000000000UL; + uint32_t sec_part = nsec / 1000000000UL; + sec += sec_part; + nsec = nsec_part; +} - Time& Time::fromNSec(int32_t t) - { - sec = t / 1000000000; - nsec = t % 1000000000; - normalizeSecNSec(sec, nsec); - return *this; - } +Time& Time::fromNSec(int32_t t) +{ + sec = t / 1000000000; + nsec = t % 1000000000; + normalizeSecNSec(sec, nsec); + return *this; +} - Time& Time::operator +=(const Duration &rhs) - { - sec += rhs.sec; - nsec += rhs.nsec; - normalizeSecNSec(sec, nsec); - return *this; - } +Time& Time::operator +=(const Duration &rhs) +{ + sec += rhs.sec; + nsec += rhs.nsec; + normalizeSecNSec(sec, nsec); + return *this; +} - Time& Time::operator -=(const Duration &rhs){ - sec += -rhs.sec; - nsec += -rhs.nsec; - normalizeSecNSec(sec, nsec); - return *this; - } +Time& Time::operator -=(const Duration &rhs) +{ + sec += -rhs.sec; + nsec += -rhs.nsec; + normalizeSecNSec(sec, nsec); + return *this; +} } diff --git a/rosserial_client/test/float64_test.cpp b/rosserial_client/test/float64_test.cpp index 84f18cf35..43297c020 100644 --- a/rosserial_client/test/float64_test.cpp +++ b/rosserial_client/test/float64_test.cpp @@ -10,12 +10,13 @@ class TestFloat64 : public ::testing::Test double val; unsigned char buffer[8]; }; - + static const double cases[]; static const int num_cases; }; - -const double TestFloat64::cases[] = { + +const double TestFloat64::cases[] = +{ 0.0, 10.0, 15642.1, -50.2, 0.0001, -0.321, 123456.789, -987.654321, 3.4e38, -3.4e38, }; @@ -29,7 +30,7 @@ TEST_F(TestFloat64, testRoundTrip) memset(buffer, 0, sizeof(buffer)); ros::Msg::serializeAvrFloat64(buffer, cases[i]); EXPECT_FLOAT_EQ(cases[i], val); - + float ret = 0; ros::Msg::deserializeAvrFloat64(buffer, &ret); EXPECT_FLOAT_EQ(cases[i], ret); @@ -37,7 +38,8 @@ TEST_F(TestFloat64, testRoundTrip) } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/rosserial_client/test/subscriber_test.cpp b/rosserial_client/test/subscriber_test.cpp index 1a763a5d9..60159a53c 100644 --- a/rosserial_client/test/subscriber_test.cpp +++ b/rosserial_client/test/subscriber_test.cpp @@ -8,10 +8,22 @@ unsigned char buffer[1]; class DummyMsg { public: - int serialize(unsigned char *outbuffer) const { return 0; } - int deserialize(unsigned char *inbuffer) { return 0; } - const char * getType() { return ""; } - const char * getMD5() { return ""; } + int serialize(unsigned char *outbuffer) const + { + return 0; + } + int deserialize(unsigned char *inbuffer) + { + return 0; + } + const char * getType() + { + return ""; + } + const char * getMD5() + { + return ""; + } }; class DummyClass diff --git a/rosserial_embeddedlinux/CHANGELOG.rst b/rosserial_embeddedlinux/CHANGELOG.rst index 5656de1e5..df641ab86 100644 --- a/rosserial_embeddedlinux/CHANGELOG.rst +++ b/rosserial_embeddedlinux/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rosserial_embeddedlinux ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Copy src/examples to install dir so make_libraries.py doesn't fail (`#336 `_) +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu, Jonny Dark + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_embeddedlinux/CMakeLists.txt b/rosserial_embeddedlinux/CMakeLists.txt index 7cf902aa8..64d31bd33 100644 --- a/rosserial_embeddedlinux/CMakeLists.txt +++ b/rosserial_embeddedlinux/CMakeLists.txt @@ -1,17 +1,19 @@ cmake_minimum_required(VERSION 2.8.3) project(rosserial_embeddedlinux) -find_package(catkin REQUIRED) -catkin_package(CATKIN_DEPENDS) - -install(DIRECTORY src/ros_lib - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +find_package(catkin REQUIRED COMPONENTS + rosserial_client ) -install(DIRECTORY src/examples +catkin_package(CATKIN_DEPENDS) + +install( + DIRECTORY src/ros_lib + src/examples DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src ) -install(PROGRAMS src/rosserial_embeddedlinux/make_libraries.py - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_libraries.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/rosserial_embeddedlinux/package.xml b/rosserial_embeddedlinux/package.xml index 420d5f192..212ded3af 100644 --- a/rosserial_embeddedlinux/package.xml +++ b/rosserial_embeddedlinux/package.xml @@ -1,8 +1,8 @@ rosserial_embeddedlinux - 0.7.6 + 0.7.7 - Libraries and examples for ROSserial usage on Embedded Linux Enviroments + rosserial for embedded Linux enviroments Paul Bouchier Paul Bouchier @@ -12,10 +12,6 @@ catkin - std_msgs - sensor_msgs - geometry_msgs - nav_msgs rosserial_client rospy diff --git a/rosserial_embeddedlinux/src/examples/ExampleService/ExampleService.cpp b/rosserial_embeddedlinux/src/examples/ExampleService/ExampleService.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/ExampleServiceClient/ExampleServiceClient.cpp b/rosserial_embeddedlinux/src/examples/ExampleServiceClient/ExampleServiceClient.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/ExampleServiceClient/exampleService.py b/rosserial_embeddedlinux/src/examples/ExampleServiceClient/exampleService.py old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/ExampleSubscriber/ExampleSubscriber.cpp b/rosserial_embeddedlinux/src/examples/ExampleSubscriber/ExampleSubscriber.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/HelloRos/HelloROS.cpp b/rosserial_embeddedlinux/src/examples/HelloRos/HelloROS.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/VEXProMotor13Subscribe/VEXProMotor13Subscribe.cpp b/rosserial_embeddedlinux/src/examples/VEXProMotor13Subscribe/VEXProMotor13Subscribe.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/VEXProRangeMotorLoop/VEXProRangeMotorLoop.cpp b/rosserial_embeddedlinux/src/examples/VEXProRangeMotorLoop/VEXProRangeMotorLoop.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/VEXProRangePublish/VEXProRangePublish.cpp b/rosserial_embeddedlinux/src/examples/VEXProRangePublish/VEXProRangePublish.cpp old mode 100755 new mode 100644 diff --git a/rosserial_embeddedlinux/src/examples/VEXProServoSubscribe/VEXProServoSubscribe.cpp b/rosserial_embeddedlinux/src/examples/VEXProServoSubscribe/VEXProServoSubscribe.cpp old mode 100755 new mode 100644 diff --git a/rosserial_mbed/CHANGELOG.rst b/rosserial_mbed/CHANGELOG.rst index 2b03787f3..d9cb07960 100644 --- a/rosserial_mbed/CHANGELOG.rst +++ b/rosserial_mbed/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rosserial_mbed ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_mbed/CMakeLists.txt b/rosserial_mbed/CMakeLists.txt index 1ac076a49..da7a21caa 100644 --- a/rosserial_mbed/CMakeLists.txt +++ b/rosserial_mbed/CMakeLists.txt @@ -1,34 +1,30 @@ cmake_minimum_required(VERSION 2.8.3) project(rosserial_mbed) -find_package(catkin REQUIRED COMPONENTS message_generation) +find_package(catkin REQUIRED COMPONENTS + message_generation +) add_message_files(FILES - Adc.msg - ) + Adc.msg +) add_service_files(FILES - Test.srv - ) + Test.srv +) generate_messages() -catkin_package( - CATKIN_DEPENDS message_runtime -# CFG_EXTRAS rosserial_mbed-extras.cmake +catkin_package(CATKIN_DEPENDS + message_runtime ) -install(DIRECTORY src/ros_lib +install( + DIRECTORY src/ros_lib DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src ) -#install(DIRECTORY mbed-cmake -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) - -#install(PROGRAMS src/rosserial_mbed/make_libraries.py -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) - - - +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_libraries.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/rosserial_mbed/package.xml b/rosserial_mbed/package.xml index 99b6a8da3..3024f3d05 100644 --- a/rosserial_mbed/package.xml +++ b/rosserial_mbed/package.xml @@ -1,8 +1,8 @@ rosserial_mbed - 0.7.6 + 0.7.7 - Libraries and examples for ROSserial usage on Mbed Platforms. + rosserial for mbed platforms. Gary Servin Gary Servin diff --git a/rosserial_mbed/src/examples/ADC/makefile b/rosserial_mbed/src/examples/ADC/makefile index 2bd85d575..9ea8ac91c 100644 --- a/rosserial_mbed/src/examples/ADC/makefile +++ b/rosserial_mbed/src/examples/ADC/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_ADC DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/Blink/makefile b/rosserial_mbed/src/examples/Blink/makefile index 35bb79a62..156ba0004 100644 --- a/rosserial_mbed/src/examples/Blink/makefile +++ b/rosserial_mbed/src/examples/Blink/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_Blink DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/Clapper/makefile b/rosserial_mbed/src/examples/Clapper/makefile index 9e1a30e46..90b164d36 100644 --- a/rosserial_mbed/src/examples/Clapper/makefile +++ b/rosserial_mbed/src/examples/Clapper/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_Clapper DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/GroveBuzzer/makefile b/rosserial_mbed/src/examples/GroveBuzzer/makefile index b896e5766..c5ab4e38c 100644 --- a/rosserial_mbed/src/examples/GroveBuzzer/makefile +++ b/rosserial_mbed/src/examples/GroveBuzzer/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_grove_buzzer DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/GroveCollision/makefile b/rosserial_mbed/src/examples/GroveCollision/makefile index f359ab81d..cc26a978f 100644 --- a/rosserial_mbed/src/examples/GroveCollision/makefile +++ b/rosserial_mbed/src/examples/GroveCollision/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_grove_collision DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/GrovePIRMotionSensor/makefile b/rosserial_mbed/src/examples/GrovePIRMotionSensor/makefile index 2e4a78d60..aef7150e2 100644 --- a/rosserial_mbed/src/examples/GrovePIRMotionSensor/makefile +++ b/rosserial_mbed/src/examples/GrovePIRMotionSensor/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_pir_motion_sensor DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/GroveTemperatureHumidity/makefile b/rosserial_mbed/src/examples/GroveTemperatureHumidity/makefile index fd036cd03..2d65da4c6 100644 --- a/rosserial_mbed/src/examples/GroveTemperatureHumidity/makefile +++ b/rosserial_mbed/src/examples/GroveTemperatureHumidity/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_grove_temperature_humidity DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/HelloWorld/makefile b/rosserial_mbed/src/examples/HelloWorld/makefile index f70cc9889..db5e0e4ca 100644 --- a/rosserial_mbed/src/examples/HelloWorld/makefile +++ b/rosserial_mbed/src/examples/HelloWorld/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_HelloWorld DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/IrRanger/makefile b/rosserial_mbed/src/examples/IrRanger/makefile index 32610d8e6..b0f39d891 100644 --- a/rosserial_mbed/src/examples/IrRanger/makefile +++ b/rosserial_mbed/src/examples/IrRanger/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_IrRanger DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/Logging/makefile b/rosserial_mbed/src/examples/Logging/makefile index 38ca105d7..312f51fb0 100644 --- a/rosserial_mbed/src/examples/Logging/makefile +++ b/rosserial_mbed/src/examples/Logging/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_Logging DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/MotorShield/makefile b/rosserial_mbed/src/examples/MotorShield/makefile index c376f4c5c..1548d6fef 100644 --- a/rosserial_mbed/src/examples/MotorShield/makefile +++ b/rosserial_mbed/src/examples/MotorShield/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_motor_shield DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/Odom/makefile b/rosserial_mbed/src/examples/Odom/makefile index ab0d1e6c6..53f3c4a0f 100644 --- a/rosserial_mbed/src/examples/Odom/makefile +++ b/rosserial_mbed/src/examples/Odom/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_Odom DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/ServiceClient/client.py b/rosserial_mbed/src/examples/ServiceClient/client.py old mode 100755 new mode 100644 diff --git a/rosserial_mbed/src/examples/ServiceClient/makefile b/rosserial_mbed/src/examples/ServiceClient/makefile index 1ffe3fb9c..756e4bad4 100644 --- a/rosserial_mbed/src/examples/ServiceClient/makefile +++ b/rosserial_mbed/src/examples/ServiceClient/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_ServiceClient DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/ServiceServer/makefile b/rosserial_mbed/src/examples/ServiceServer/makefile index f4966c5a1..fc017180b 100644 --- a/rosserial_mbed/src/examples/ServiceServer/makefile +++ b/rosserial_mbed/src/examples/ServiceServer/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_ServiceServer DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/ServoControl/makefile b/rosserial_mbed/src/examples/ServoControl/makefile index 982513258..55751c21b 100644 --- a/rosserial_mbed/src/examples/ServoControl/makefile +++ b/rosserial_mbed/src/examples/ServoControl/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_ServoControl DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/Temperature/makefile b/rosserial_mbed/src/examples/Temperature/makefile index e27a0747d..1cf7960c9 100644 --- a/rosserial_mbed/src/examples/Temperature/makefile +++ b/rosserial_mbed/src/examples/Temperature/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_Temperature DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/TimeTF/makefile b/rosserial_mbed/src/examples/TimeTF/makefile index 0412490e0..82220f74c 100644 --- a/rosserial_mbed/src/examples/TimeTF/makefile +++ b/rosserial_mbed/src/examples/TimeTF/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_TimeTF DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/Ultrasound/makefile b/rosserial_mbed/src/examples/Ultrasound/makefile index 55a75d5d6..e59cf215c 100644 --- a/rosserial_mbed/src/examples/Ultrasound/makefile +++ b/rosserial_mbed/src/examples/Ultrasound/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_Ultrasound DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/button_example/makefile b/rosserial_mbed/src/examples/button_example/makefile index 168603c9b..1fe2404d0 100644 --- a/rosserial_mbed/src/examples/button_example/makefile +++ b/rosserial_mbed/src/examples/button_example/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_button_example DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_mbed/src/examples/pubsub/makefile b/rosserial_mbed/src/examples/pubsub/makefile index b5d498633..002716542 100644 --- a/rosserial_mbed/src/examples/pubsub/makefile +++ b/rosserial_mbed/src/examples/pubsub/makefile @@ -1,7 +1,7 @@ PROJECT := rosserial_mbed_pubsub DEVICES := LPC1768 KL25Z NUCLEO_F401RE GCC4MBED_DIR := $(GCC4MBED_DIR) -USER_LIBS := $(ROS_LIB_DIR) +USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 1 diff --git a/rosserial_msgs/CHANGELOG.rst b/rosserial_msgs/CHANGELOG.rst index 85431f597..59b714735 100644 --- a/rosserial_msgs/CHANGELOG.rst +++ b/rosserial_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rosserial_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_msgs/CMakeLists.txt b/rosserial_msgs/CMakeLists.txt index 45ccbc5a9..d2b8cac9f 100644 --- a/rosserial_msgs/CMakeLists.txt +++ b/rosserial_msgs/CMakeLists.txt @@ -1,21 +1,26 @@ cmake_minimum_required(VERSION 2.8.3) project(rosserial_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation) +find_package(catkin REQUIRED COMPONENTS + message_generation +) add_message_files(FILES - Log.msg - RosTopicAndType.msg - ServiceCallResult.msg - TopicInfo.msg - ) + Log.msg + RosTopicAndType.msg + ServiceCallResult.msg + TopicInfo.msg +) add_service_files(FILES - RequestParam.srv - RequestMessageInfo.srv - RequestServiceInfo.srv - RequestTopicsList.srv - ) + RequestParam.srv + RequestMessageInfo.srv + RequestServiceInfo.srv + RequestTopicsList.srv +) generate_messages() -catkin_package(CATKIN_DEPENDS message_runtime) + +catkin_package(CATKIN_DEPENDS + message_runtime +) diff --git a/rosserial_msgs/package.xml b/rosserial_msgs/package.xml index 1b72686bf..2eb25ae3a 100644 --- a/rosserial_msgs/package.xml +++ b/rosserial_msgs/package.xml @@ -1,6 +1,6 @@ rosserial_msgs - 0.7.6 + 0.7.7 Messages for automatic topic configuration using rosserial. diff --git a/rosserial_python/CHANGELOG.rst b/rosserial_python/CHANGELOG.rst index 89e909da7..7a0de9fc5 100644 --- a/rosserial_python/CHANGELOG.rst +++ b/rosserial_python/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rosserial_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* pyserial bug workaround to fix rosserial_test test against SerialClient.py (`#313 `_) +* Add requestTopics to correct publish before setup (`#308 `_) +* Contributors: Bei Chen Liu, Tom O'Connell + 0.7.6 (2017-03-01) ------------------ * Fix typo in serial error message (`#253 `_) diff --git a/rosserial_python/CMakeLists.txt b/rosserial_python/CMakeLists.txt index badc0f209..17787ccfe 100644 --- a/rosserial_python/CMakeLists.txt +++ b/rosserial_python/CMakeLists.txt @@ -6,6 +6,7 @@ catkin_package() catkin_python_setup() -install(PROGRAMS nodes/serial_node.py nodes/message_info_service.py +catkin_install_python( + PROGRAMS nodes/message_info_service.py nodes/serial_node.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/rosserial_python/package.xml b/rosserial_python/package.xml index 27cd1fdaf..22dc97405 100644 --- a/rosserial_python/package.xml +++ b/rosserial_python/package.xml @@ -1,8 +1,8 @@ rosserial_python - 0.7.6 + 0.7.7 - A Python-based implementation of the ROS serial protocol. + A Python-based implementation of the rosserial protocol. Michael Ferguson Paul Bouchier diff --git a/rosserial_python/src/rosserial_python/SerialClient.py b/rosserial_python/src/rosserial_python/SerialClient.py index 513326f2e..180dceff1 100644 --- a/rosserial_python/src/rosserial_python/SerialClient.py +++ b/rosserial_python/src/rosserial_python/SerialClient.py @@ -35,38 +35,42 @@ __author__ = "mferguson@willowgarage.com (Michael Ferguson)" -import roslib -import rospy import imp - -import thread +import threading +import sys import multiprocessing -from serial import * import StringIO - -from std_msgs.msg import Time -from rosserial_msgs.msg import * -from rosserial_msgs.srv import * - -import diagnostic_msgs.msg - import errno import signal import socket import struct import time +from Queue import Queue + +from serial import Serial, SerialException, SerialTimeoutException + +import roslib +import rospy +from std_msgs.msg import Time +from rosserial_msgs.msg import TopicInfo, Log +from rosserial_msgs.srv import RequestParamRequest, RequestParamResponse + +import diagnostic_msgs.msg +ERROR_MISMATCHED_PROTOCOL = "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client" +ERROR_NO_SYNC = "no sync with device" +ERROR_PACKET_FAILED = "Packet Failed : Failed to read msg data" def load_pkg_module(package, directory): #check if its in the python path path = sys.path try: imp.find_module(package) - except: + except ImportError: roslib.load_manifest(package) try: m = __import__( package + '.' + directory ) - except: + except ImportError: rospy.logerr( "Cannot import package : %s"% package ) rospy.logerr( "sys.path was " + str(path) ) return None @@ -126,10 +130,6 @@ def __init__(self, topic_info, parent): else: raise Exception('Checksum does not match: ' + self.message._md5sum + ',' + topic_info.md5sum) - def unregister(self): - rospy.loginfo("Removing subscriber: %s", self.topic) - self.subscriber.unregister() - def callback(self, msg): """ Forward message to serial device. """ data_buffer = StringIO.StringIO() @@ -137,6 +137,7 @@ def callback(self, msg): self.parent.send(self.id, data_buffer.getvalue()) def unregister(self): + rospy.loginfo("Removing subscriber: %s", self.topic) self.subscriber.unregister() class ServiceServer: @@ -170,7 +171,7 @@ def callback(self, req): req.serialize(data_buffer) self.response = None if self.parent.send(self.id, data_buffer.getvalue()) >= 0: - while self.response == None: + while self.response is None: pass return self.response @@ -241,7 +242,7 @@ def listen(self): self.socket = clientsocket self.isConnected = True - if (self.fork_server == True): # if configured to launch server in a separate process + if self.fork_server: # if configured to launch server in a separate process rospy.loginfo("Forking a socket server process") process = multiprocessing.Process(target=self.startSocketServer, args=(address)) process.daemon = True @@ -278,10 +279,10 @@ def startSocketServer(self, port, address): self.startSerialClient() def flushInput(self): - pass + pass def write(self, data): - if (self.isConnected == False): + if not self.isConnected: return length = len(data) totalsent = 0 @@ -294,7 +295,7 @@ def write(self, data): def read(self, rqsted_length): self.msg = '' - if (self.isConnected == False): + if not self.isConnected: return self.msg while len(self.msg) < rqsted_length: @@ -315,25 +316,32 @@ def inWaiting(self): return 0 raise - -class SerialClient: +class SerialClient(object): """ ServiceServer responds to requests from the serial device. """ def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=False): """ Initialize node, connect to bus, attempt to negotiate topics. """ - self.mutex = thread.allocate_lock() + + self.read_lock = threading.RLock() + + self.write_lock = threading.RLock() + self.write_queue = Queue() + self.write_thread = None self.lastsync = rospy.Time(0) self.lastsync_lost = rospy.Time(0) + self.lastsync_success = rospy.Time(0) + self.last_read = rospy.Time(0) + self.last_write = rospy.Time(0) self.timeout = timeout self.synced = False self.fix_pyserial_for_test = fix_pyserial_for_test self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=10) - if port== None: + if port is None: # no port specified, listen for any new port? pass elif hasattr(port, 'read'): @@ -341,19 +349,20 @@ def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=Fal self.port=port else: # open a specific port - try: - if self.fix_pyserial_for_test: - # see https://github.com/pyserial/pyserial/issues/59 - self.port = Serial(port, baud, timeout=self.timeout*0.5, rtscts=True, dsrdtr=True) - else: - self.port = Serial(port, baud, timeout=self.timeout*0.5) - - except SerialException as e: - rospy.logerr("Error opening serial: %s", e) - rospy.signal_shutdown("Error opening serial: %s" % e) - raise SystemExit + while not rospy.is_shutdown(): + try: + if self.fix_pyserial_for_test: + # see https://github.com/pyserial/pyserial/issues/59 + self.port = Serial(port, baud, timeout=self.timeout, write_timeout=10, rtscts=True, dsrdtr=True) + else: + self.port = Serial(port, baud, timeout=self.timeout, write_timeout=10) + break + except SerialException as e: + rospy.logerr("Error opening serial: %s", e) + time.sleep(3) - self.port.timeout = 0.01 # Edit the port timeout + if rospy.is_shutdown(): + return time.sleep(0.1) # Wait for ready (patch for Uno) @@ -384,7 +393,7 @@ def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=Fal self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest - rospy.sleep(2.0) # TODO + rospy.sleep(2.0) self.requestTopics() self.lastsync = rospy.Time.now() @@ -392,19 +401,24 @@ def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=Fal def requestTopics(self): """ Determine topics to subscribe/publish. """ + rospy.loginfo('Requesting topics...') + # TODO remove if possible if not self.fix_pyserial_for_test: - self.port.flushInput() + with self.read_lock: + self.port.flushInput() # request topic sync - self.port.write("\xff" + self.protocol_ver + "\x00\x00\xff\x00\x00\xff") + self.write_queue.put("\xff" + self.protocol_ver + "\x00\x00\xff\x00\x00\xff") def txStopRequest(self, signal, frame): """ send stop tx request to arduino when receive SIGINT(Ctrl-c)""" if not self.fix_pyserial_for_test: - self.port.flushInput() + with self.read_lock: + self.port.flushInput() + + self.write_queue.put("\xff" + self.protocol_ver + "\x00\x00\xff\x0b\x00\xf4") - self.port.write("\xff" + self.protocol_ver + "\x00\x00\xff\x0b\x00\xf4") # tx_stop_request is x0b rospy.loginfo("Send tx stop request") sys.exit(0) @@ -412,37 +426,43 @@ def txStopRequest(self, signal, frame): def tryRead(self, length): try: read_start = time.time() - read_current = read_start bytes_remaining = length result = bytearray() - while bytes_remaining != 0 and read_current - read_start < self.timeout: - received = self.port.read(bytes_remaining) + while bytes_remaining != 0 and time.time() - read_start < self.timeout: + with self.read_lock: + received = self.port.read(bytes_remaining) if len(received) != 0: + self.last_read = rospy.Time.now() result.extend(received) bytes_remaining -= len(received) - read_current = time.time() if bytes_remaining != 0: - rospy.logwarn("Serial Port read returned short (expected %d bytes, received %d instead)." - % (length, length - bytes_remaining)) - raise IOError() + raise IOError("Returned short (expected %d bytes, received %d instead)." % (length, length - bytes_remaining)) return bytes(result) except Exception as e: - rospy.logwarn("Serial Port read failure: %s", e) - raise IOError() + raise IOError("Serial Port read failure: %s" % e) def run(self): """ Forward recieved messages to appropriate publisher. """ + + # Launch write thread. + if self.write_thread is None: + self.write_thread = threading.Thread(target=self.processWriteQueue) + self.write_thread.daemon = True + self.write_thread.start() + + # Handle reading. data = '' + read_step = None while not rospy.is_shutdown(): if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3): - if (self.synced == True): + if self.synced: rospy.logerr("Lost sync with device, restarting...") else: rospy.logerr("Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino") self.lastsync_lost = rospy.Time.now() - self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "no sync with device") + self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, ERROR_NO_SYNC) self.requestTopics() self.lastsync = rospy.Time.now() @@ -450,56 +470,72 @@ def run(self): # an IOError if there's a serial problem or timeout. In that scenario, a single handler at the # bottom attempts to reconfigure the topics. try: - if self.port.inWaiting() < 1: - time.sleep(0.001) - continue - - flag = [0,0] + with self.read_lock: + if self.port.inWaiting() < 1: + time.sleep(0.001) + continue + + # Find sync flag. + flag = [0, 0] + read_step = 'syncflag' flag[0] = self.tryRead(1) if (flag[0] != '\xff'): continue + # Find protocol version. + read_step = 'protocol' flag[1] = self.tryRead(1) - if ( flag[1] != self.protocol_ver): - self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client") - rospy.logerr("Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client") + if flag[1] != self.protocol_ver: + self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, ERROR_MISMATCHED_PROTOCOL) + rospy.logerr("Mismatched protocol version in packet (%s): lost sync or rosserial_python is from different ros release than the rosserial client" % repr(flag[1])) protocol_ver_msgs = {'\xff': 'Rev 0 (rosserial 0.4 and earlier)', '\xfe': 'Rev 1 (rosserial 0.5+)', '\xfd': 'Some future rosserial version'} - if (flag[1] in protocol_ver_msgs): + if flag[1] in protocol_ver_msgs: found_ver_msg = 'Protocol version of client is ' + protocol_ver_msgs[flag[1]] else: found_ver_msg = "Protocol version of client is unrecognized" rospy.loginfo("%s, expected %s" % (found_ver_msg, protocol_ver_msgs[self.protocol_ver])) continue + # Read message length. + read_step = 'message length' msg_len_bytes = self.tryRead(2) msg_length, = struct.unpack(" 0 and length > self.buffer_in: - rospy.logerr("Message from ROS network dropped: message larger than buffer.") - print msg - return -1 + """ + Queues data to be written to the serial port. + """ + self.write_queue.put((topic, msg)) + + def _write(self, data): + """ + Writes raw data over the serial port. Assumes the data is formatting as a packet. http://wiki.ros.org/rosserial/Overview/Protocol + """ + with self.write_lock: + self.port.write(data) + self.last_write = rospy.Time.now() + + def _send(self, topic, msg): + """ + Send a message on a particular topic to the device. + """ + length = len(msg) + if self.buffer_in > 0 and length > self.buffer_in: + rospy.logerr("Message from ROS network dropped: message larger than buffer.\n%s" % msg) + return -1 + else: + #modified frame : header(2 bytes) + msg_len(2 bytes) + msg_len_chk(1 byte) + topic_id(2 bytes) + msg(x bytes) + msg_topic_id_chk(1 byte) + # second byte of header is protocol version + msg_len_checksum = 255 - ( ((length&255) + (length>>8))%256 ) + msg_checksum = 255 - ( ((topic&255) + (topic>>8) + sum([ord(x) for x in msg]))%256 ) + data = "\xff" + self.protocol_ver + chr(length&255) + chr(length>>8) + chr(msg_len_checksum) + chr(topic&255) + chr(topic>>8) + data = data + msg + chr(msg_checksum) + self._write(data) + return length + + def processWriteQueue(self): + """ + Main loop for the thread that processes outgoing data to write to the serial port. + """ + while not rospy.is_shutdown(): + if self.write_queue.empty(): + time.sleep(0.01) else: - #modified frame : header(2 bytes) + msg_len(2 bytes) + msg_len_chk(1 byte) + topic_id(2 bytes) + msg(x bytes) + msg_topic_id_chk(1 byte) - # second byte of header is protocol version - msg_len_checksum = 255 - ( ((length&255) + (length>>8))%256 ) - msg_checksum = 255 - ( ((topic&255) + (topic>>8) + sum([ord(x) for x in msg]))%256 ) - data = "\xff" + self.protocol_ver + chr(length&255) + chr(length>>8) + chr(msg_len_checksum) + chr(topic&255) + chr(topic>>8) - data = data + msg + chr(msg_checksum) - self.port.write(data) - return length + data = self.write_queue.get() + while True: + try: + if isinstance(data, tuple): + topic, msg = data + self._send(topic, msg) + elif isinstance(data, basestring): + self._write(data) + else: + rospy.logerr("Trying to write invalid data type: %s" % type(data)) + break + except SerialTimeoutException as exc: + rospy.logerr('Write timeout: %s' % exc) + time.sleep(1) def sendDiagnostics(self, level, msg_text): msg = diagnostic_msgs.msg.DiagnosticArray() diff --git a/rosserial_server/CHANGELOG.rst b/rosserial_server/CHANGELOG.rst index 3cff42679..67f10bacb 100644 --- a/rosserial_server/CHANGELOG.rst +++ b/rosserial_server/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rosserial_server ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_server/CMakeLists.txt b/rosserial_server/CMakeLists.txt index 716dd69c3..2cbb90d94 100644 --- a/rosserial_server/CMakeLists.txt +++ b/rosserial_server/CMakeLists.txt @@ -3,18 +3,30 @@ project(rosserial_server) add_definitions(-std=c++11) -find_package(catkin REQUIRED COMPONENTS roscpp rosserial_msgs topic_tools) -find_package(Boost REQUIRED COMPONENTS system thread) +find_package(catkin REQUIRED COMPONENTS + roscpp + rosserial_msgs + topic_tools +) + +find_package(Boost REQUIRED COMPONENTS + system + thread +) catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rosserial_msgs topic_tools + CATKIN_DEPENDS + roscpp + rosserial_msgs + std_msgs + topic_tools ) include_directories( include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) add_executable(${PROJECT_NAME}_serial_node src/serial_node.cpp) @@ -37,7 +49,11 @@ target_link_libraries(topics_service_server ${catkin_LIBRARIES}) install( - TARGETS ${PROJECT_NAME}_serial_node ${PROJECT_NAME}_socket_node ${PROJECT_NAME}_udp_socket_node topics_service_server + TARGETS + ${PROJECT_NAME}_serial_node + ${PROJECT_NAME}_socket_node + ${PROJECT_NAME}_udp_socket_node + topics_service_server RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/rosserial_server/include/rosserial_server/udp_stream.h b/rosserial_server/include/rosserial_server/udp_stream.h index 8e1e7b593..8e4bb0b7e 100644 --- a/rosserial_server/include/rosserial_server/udp_stream.h +++ b/rosserial_server/include/rosserial_server/udp_stream.h @@ -79,10 +79,21 @@ class UdpStream : public udp::socket // If you get an error on the following line it means that your handler does // not meet the documented type requirements for a WriteHandler. BOOST_ASIO_WRITE_HANDLER_CHECK(WriteHandler, handler) type_check; +#if (BOOST_VERSION >= 106600) + // See: http://www.boost.org/doc/libs/1_66_0/doc/html/boost_asio/net_ts.html + boost::asio::async_completion init(handler); + this->get_service().async_send_to( + this->get_implementation(), buffers, client_endpoint_, 0, + init.completion_handler); + + return init.result.get(); +#else return this->get_service().async_send_to( this->get_implementation(), buffers, client_endpoint_, 0, BOOST_ASIO_MOVE_CAST(WriteHandler)(handler)); +#endif } template @@ -94,10 +105,20 @@ class UdpStream : public udp::socket // If you get an error on the following line it means that your handler does // not meet the documented type requirements for a ReadHandler. BOOST_ASIO_READ_HANDLER_CHECK(ReadHandler, handler) type_check; +#if (BOOST_VERSION >= 106600) + // See: http://www.boost.org/doc/libs/1_66_0/doc/html/boost_asio/net_ts.html + boost::asio::async_completion init(handler); + + this->get_service().async_receive(this->get_implementation(), + buffers, 0, init.completion_handler); + return init.result.get(); +#else return this->get_service().async_receive_from( this->get_implementation(), buffers, client_endpoint_, 0, BOOST_ASIO_MOVE_CAST(ReadHandler)(handler)); +#endif } private: diff --git a/rosserial_server/package.xml b/rosserial_server/package.xml index 0c87978ab..baf856950 100644 --- a/rosserial_server/package.xml +++ b/rosserial_server/package.xml @@ -1,10 +1,10 @@ rosserial_server - 0.7.6 + 0.7.7 - The rosserial_server package provides a C++ implementation of the rosserial server side, - serving as a more performance- and stability-oriented alternative to rosserial_python. + A more performance- and stability-oriented server alternative implemented + in C++ to rosserial_python. Mike Purvis diff --git a/rosserial_test/CHANGELOG.rst b/rosserial_test/CHANGELOG.rst index bb733d321..d12e61ede 100644 --- a/rosserial_test/CHANGELOG.rst +++ b/rosserial_test/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rosserial_test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* pyserial bug workaround to fix rosserial_test test against SerialClient.py (`#313 `_) +* Contributors: Bei Chen Liu, Tom O'Connell + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_test/CMakeLists.txt b/rosserial_test/CMakeLists.txt index 48de41d50..2e1752e85 100644 --- a/rosserial_test/CMakeLists.txt +++ b/rosserial_test/CMakeLists.txt @@ -1,15 +1,27 @@ cmake_minimum_required(VERSION 2.8.3) project(rosserial_test) -find_package(catkin REQUIRED COMPONENTS roscpp rosserial_client rosserial_msgs rosserial_python rosserial_server rostest std_msgs) +find_package(catkin REQUIRED COMPONENTS + roscpp + rosserial_client + rosserial_msgs + rosserial_python + rosserial_server + rostest + std_msgs +) -catkin_package() +catkin_package( + CATKIN_DEPENDS + rosserial_msgs + std_msgs +) if(CATKIN_ENABLE_TESTING) # Generate a client library for the test harnesses to use. add_custom_command( OUTPUT ${PROJECT_BINARY_DIR}/include/rosserial - COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh rosrun rosserial_test generate_client_ros_lib ${PROJECT_BINARY_DIR}/include + COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh rosrun ${PROJECT_NAME} generate_client_ros_lib ${PROJECT_BINARY_DIR}/include ) add_custom_target(${PROJECT_NAME}_rosserial_lib DEPENDS ${PROJECT_BINARY_DIR}/include/rosserial) add_dependencies(${PROJECT_NAME}_rosserial_lib ${catkin_EXPORTED_TARGETS}) @@ -19,7 +31,7 @@ if(CATKIN_ENABLE_TESTING) ) # Helper for building and linking test executables. - function(add_rosserial_test_executable test_source) + function(add_${PROJECT_NAME}_executable test_source) set(target ${PROJECT_NAME}_${test_source}) add_executable(${target} EXCLUDE_FROM_ALL src/${test_source}) add_dependencies(${target} ${PROJECT_NAME}_rosserial_lib) @@ -35,3 +47,7 @@ if(CATKIN_ENABLE_TESTING) # Disabled due to reconnect logic in rosserial_python not being robust enough. # add_rostest(test/rosserial_python_serial.test) endif() + +catkin_install_python(PROGRAMS scripts/generate_client_ros_lib + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/rosserial_test/package.xml b/rosserial_test/package.xml index c6142eebb..4618ae5ab 100644 --- a/rosserial_test/package.xml +++ b/rosserial_test/package.xml @@ -1,7 +1,7 @@ rosserial_test - 0.7.6 + 0.7.7 A specialized harness which allows end-to-end integration testing of the rosserial client and server components. diff --git a/rosserial_tivac/CHANGELOG.rst b/rosserial_tivac/CHANGELOG.rst index 9cf7cd306..1f6464614 100644 --- a/rosserial_tivac/CHANGELOG.rst +++ b/rosserial_tivac/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rosserial_tivac ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu + 0.7.6 (2017-03-01) ------------------ * Allows to specify chip's silicon revision for TivaWare. (`#268 `_) diff --git a/rosserial_tivac/CMakeLists.txt b/rosserial_tivac/CMakeLists.txt index 44fe9dd43..399e28672 100644 --- a/rosserial_tivac/CMakeLists.txt +++ b/rosserial_tivac/CMakeLists.txt @@ -4,7 +4,7 @@ project(rosserial_tivac) find_package(catkin REQUIRED) catkin_package( - CFG_EXTRAS rosserial_tivac-extras.cmake + CFG_EXTRAS ${PROJECT_NAME}-extras.cmake ) install( @@ -20,8 +20,8 @@ install( ) install( - PROGRAMS - src/rosserial_tivac/make_libraries_tiva - src/rosserial_tivac/make_libraries_energia + PROGRAMS + src/${PROJECT_NAME}/make_libraries_energia + src/${PROJECT_NAME}/make_libraries_tiva DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/rosserial_tivac/package.xml b/rosserial_tivac/package.xml index 901690cf8..4832a235b 100644 --- a/rosserial_tivac/package.xml +++ b/rosserial_tivac/package.xml @@ -1,9 +1,8 @@ rosserial_tivac - 0.7.6 + 0.7.7 - rosserial_tivac package provides the required hardware definitions for compiling rosserial_client targets for TivaC Launchpad - evaluation boards. + rosserial for TivaC Launchpad evaluation boards. Vitor Matos BSD @@ -13,7 +12,7 @@ Vitor Matos catkin - + rosserial_msgs rosserial_client diff --git a/rosserial_vex_cortex/CHANGELOG.rst b/rosserial_vex_cortex/CHANGELOG.rst new file mode 100644 index 000000000..59c471d38 --- /dev/null +++ b/rosserial_vex_cortex/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rosserial_vex_cortex +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 +----------------------------- +- README fixes +- started tagging versions +- initial was wrong version number, fixed. diff --git a/rosserial_vex_cortex/CMakeLists.txt b/rosserial_vex_cortex/CMakeLists.txt new file mode 100644 index 000000000..309f29a25 --- /dev/null +++ b/rosserial_vex_cortex/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rosserial_vex_cortex) + +find_package(catkin REQUIRED COMPONENTS) + +catkin_python_setup() + +catkin_package(CATKIN_DEPENDS) + +install( + DIRECTORY src/ros_lib + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +) + +install( + PROGRAMS scripts/genproject.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_libraries.py + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/rosserial_vex_cortex/README.md b/rosserial_vex_cortex/README.md new file mode 100644 index 000000000..47e23f1ac --- /dev/null +++ b/rosserial_vex_cortex/README.md @@ -0,0 +1,140 @@ +# rosserial for VEX Cortex + +This package contains everything needed to run rosserial on the [VEX Cortex](https://www.vexrobotics.com/276-2194.html), on the [PROS Kernel](https://pros.cs.purdue.edu/cortex/index.html). + +# Requirements +Software: +1. Linux (Only tested on Ubuntu 18.04LTS) (possible, but not tested, on windows + virtual machines with USB support) +2. ROS installed on Linux (Only tested on ROS Melodic) - [installation guide](http://wiki.ros.org/melodic/Installation/Source). +3. PROS installed on Linux - [installation guide](https://pros.cs.purdue.edu/cortex/getting-started/index.html) +Hardware: +1. VEX essentials: VEX Cortex, Joystick, VexNet keys, battery +2. VEX Programming Cable +3. (recommended, for debugging) a [USB-serial adapter](https://www.adafruit.com/product/954) +4. Three Male-Male jumper wires for USB-serial adapter + +# Setup +Setup a ROS workspace and build rosserial packages (including rosserial_vex_cortex) from source: +```bash +source /opt/ros/melodic/setup.bash # or replace melodic with your corresponding ROS version name +mkdir -p /src +cd /src +git clone https://github.com/ros-drivers/rosserial.git +cd .. +catkin_make +catkin_make install # this will generate folders in the workspace that contain executable scripts. +``` + +# Usage Example (Linux) +This will show you how to run the "hello world" example PROS project. Plug in the VEX Programming cable to the computer and the joystick, plug the VexNet keys into the Cortex and the joystick. Power cycle the Joystick and Cortex between cortex downloads for optimal usage. + +### Step 1 : Run the ROS instance +open a terminal window and run: +```bash +source /opt/ros/melodic/setup.bash # or replace melodic with your corresponding ROS version name +roscore +``` +this will start ROS on the host linux machine. + +### Step 2: get the VEX Cortex to talk +This step generates a PROS project with the necessary includes for ROS to talk to the VEX Cortex, and then downloads the "Hello World" program onto the Cortex. +Open another terminal window and run: +```bash +source /install/setup.bash +cd /anywhere/on/your/computer +# below runs the generate command to build the PROS project with the rosserial libraries installed. +rosrun rosserial_vex_cortex genproject.sh +cd +pros make upload # uploads the hello world demo onto the cortex. +``` +The Cortex is now trying to send messages on the "chatter" topic, as long as it is plugged into the VEX Programming cable. see `src/opcontrol.cpp` and look around so see how the PROS side works. + +### Step 3: Begin the serial node! +This is how rosserial clients are able to communicate with the ROS system. +```bash +source /install/setup.bash +rosrun rosserial_arduino serial_node.py _port:=/dev/ttyACM0 _baud:=115200 +``` +see the `Serial Connections` section below, for more information about how the serial ports are used. + +### Step 4: Display a topic in a terminal (technically optional) +open another terminal window and run: +```bash +source /opt/ros/melodic/setup.bash # or replace melodic with your corresponding ROS version name +rostopic echo chatter +``` +this will display any messages that come through on the "chatter" topic. + +If everyting is working properly, then the terminal from step 2 should show outputs of `"Hello World!"` This means the bridge between the cortex and ROS is established. +You are now able to use ROS with the VEX Cortex! + + +# Physical Serial Connections +The optimal setup for this project is with two physical serial connections, one for rosserial to function, and one for debugging. The default connection for rosserial is the VEX Programming Cable, and the default debugging serial connection is UART2. + +Since the The VEX programming cable provides the default rosserial connection, the baud rate of the ROS serial connecting node must be 115200 Hz, which is why the command reads: + +```bash +rosserial_arduino serial_node _port:=/dev/ttyACM0 _baud:=115200 +``` +This overrides the default (57600) Hz.To switch the rosserial/debug serial connections, see `logger.h` in your generated PROS project. If you do end up using UART1 or UART2 for rosserial instead of debugging, update the USB device argument: `_port:=/dev/ttyUSB0` instead, and update the baud rate argument, or simply remove it for the default 57600 Hz. + +Also, the USB device path for the VEX Programming cable on linux may either be `/dev/ttyACM0` or `/dev/ttyACM1`. To figure out which to use as an argument, use `pros lsusb`, or unplug/replug the cable from/into the computer and run `dmesg` and look at the last lines. + +Viewing the UART debug stream requires a [USB-serial adapter](https://www.adafruit.com/product/954) for your computer, and it needs to be plugged in correctly. +To set up the wires with the cable linked above, use this layout: +[layout](./uartdiagram.png) +You need male-male jumper wires to plug in this adapter. +Run `dmesg` right after plugging in the adapter to identify the USB device path - it is most likely `/dev/ttyUSB0`. + +To view serial output from UART2, (instead of running `pros terminal`, which only works for the VEX Programming cable), use screen: +```bash +# to install screen: sudo apt-get install screen +screen /dev/ttyUSB0 57600 +``` +Again, you may need to change the above command to use the correct USB device path and baud rate, if you change the default configuration. + +PROS does not provide a debugger, but it does provide printing. +use vexroslog(char* out, ...) just like you would use fprintf from the PROS API: + +```bash +vexroslog("hello, my favorite number is %d", 3); +``` +remember to include this header in your code for logging! +```cpp +#include "logger.h" +``` + +# Generating Custom Messages +To design you own ROS messages, it is necessary to add the `msgs` directory to this project, +add the `message_generation` dependency, and make several modifications to this project's `CMakeLists.txt`. +This infrastructure is removed from this project by default, +because there are many useful built-in message types that are used commonly across ROS packages anyway. + +See documentation and source of sister packages, such as `rosserial_arduino`, for more information about generating custom messages. + +# Limitations + +### Global Scope +Global scope variables causes segmentation faults. Unlike in the other rosserial examples, avoid putting objects/structs in the global scope. Read the comments inside the templates for clarification on where `global scope` is referring to. + +To workaround not having global variables: +1. If possible, keep variables inside functions. +2. If an object/struct must be accessed across concurrently running tasks, use a global shared pointer or semaphore (see API.h for semaphore functions). + +The second strategy should be used as a last-resort, because the better option is simply to use locally-scoped variables inside functions (e.g. declare variables in the body of `void opcontrol()`, and pass them to functions that need to use the variables). + +This issue is most likely a side-effect of mixing C++ source, compiled with g++, and C source, compiled with C99. the initialization procedure for C++ static constructors is skipped for some reason (see [this issue](https://github.com/purduesigbots/pros/issues/48), although the fix provided does not work here). + +### Platforms +This has been developed and tested on ROS melodic, but it should work on many earier/later versions as well. + +# Speed +Over the VEX Programming cable/VexNet wireless connection, the simplest messages can stream at upwards of 200Hz. More complex messages, such as sensor_msgs::JointState, can be published at 50hz. The baud rate degrades as the distance from the cortex and the Joystick increases, however. + +A wired UART1 or UART2 connection should have higher stability and frequency with arbitrarily-sized messages, with no degradation in baud rate as distance increases. + +# Troubleshooting +If your program crashes, it may be difficult to debug effectively without a second USB-serial connection for debugging messages, since the crash message will only print to stdout. run `pros terminal` to view whether or not the program is crashing (this only works if you can switch rosserial to use a UART connection - see Physical Serial Connections). Make sure to test the individual pieces of your program seperately to ensure they work properly, before integrating them with the program as a whole. Make sure to power cycle the Cortex if it crashes. + + diff --git a/rosserial_vex_cortex/package.xml b/rosserial_vex_cortex/package.xml new file mode 100644 index 000000000..7fe2555fb --- /dev/null +++ b/rosserial_vex_cortex/package.xml @@ -0,0 +1,17 @@ + + rosserial_vex_cortex + 0.7.7 + + rosserial for Cortex/AVR platforms. + + Cannon + Canyon Turtle + BSD + + + catkin + + rospy + rosserial_client + rosserial_python + diff --git a/rosserial_vex_cortex/scripts/genproject.sh b/rosserial_vex_cortex/scripts/genproject.sh new file mode 100755 index 000000000..c942f5867 --- /dev/null +++ b/rosserial_vex_cortex/scripts/genproject.sh @@ -0,0 +1,21 @@ +# generate new pros project with name as first argument +pros conduct new $1 + +# go into incude directory and generate ros message library. +cd $1/include +rosrun rosserial_vex_cortex make_libraries.py . + +# custom include for files containing strlen. +# grep -lnrw '.' -e 'strlen' +grep -lrnw '.' -e 'strlen' | xargs sed -i '1 s@^@#include "vexstrlen.h"\n@g; s@strlen(@vexstrlen(@g' + +# move the cpp files into the src directory. +mv ros_lib/*.cpp ../src +mv ros_lib/examples/*.cpp ../src +cd .. + +# delete the incorrect files in src +rm src/*.c + +# replace the common.mk with the correct one. +sed -i '/^INCLUDE=/ s@$@ -I$(ROOT)/include/ros_lib@' common.mk diff --git a/rosserial_vex_cortex/setup.py b/rosserial_vex_cortex/setup.py new file mode 100644 index 000000000..9598d7c7a --- /dev/null +++ b/rosserial_vex_cortex/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['rosserial_vex_cortex'], + package_dir={'': 'src'}, + ) + +setup(**d) diff --git a/rosserial_vex_cortex/src/ros_lib/CortexHardware.h b/rosserial_vex_cortex/src/ros_lib/CortexHardware.h new file mode 100644 index 000000000..ef57d5679 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/CortexHardware.h @@ -0,0 +1,74 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROSSERIAL_VEX_CORTEX_CORTEX_HARDWARE_H_ +#define _ROSSERIAL_VEX_CORTEX_CORTEX_HARDWARE_H_ + +#include "main.h" +#include "logger.h" + +#define SERIAL_CLASS int + +class CortexHardware { + public: + CortexHardware() {} + + // any initialization code necessary to use the serial port + // note: the serial port initialization for rosserial for VEX Cortex must be implemented in `src/init.cpp` + // see that file for more information. + void init() {} + + // read a byte from the serial port. -1 = failure + int read() { + if(fcount(stdin) != 0) { + int c = vexrosreadchar(); + return c; + } + return -1; + } + + // write data to the connection to ROS + void write(uint8_t* data, int length) { + for(int i = 0; i < length; i++) { + vexroswritechar(data[i]); + } + } + + // returns milliseconds since start of program + unsigned long time() { + return millis(); + } +}; + +#endif diff --git a/rosserial_vex_cortex/src/ros_lib/auto.cpp b/rosserial_vex_cortex/src/ros_lib/auto.cpp new file mode 100644 index 000000000..3a8a49831 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/auto.cpp @@ -0,0 +1,43 @@ +/** @file auto.cpp + * @brief File for autonomous code + * + * This file should contain the user autonomous() function and any functions related to it. + * + * Any copyright is dedicated to the Public Domain. + * http://creativecommons.org/publicdomain/zero/1.0/ + * + * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be + * obtained from http://sourceforge.net/projects/freertos/files/ or on request. + */ + +#include "main.h" + +/* + * + * + * NO GLOBAL OBJECT DEFINITIONS! + * + * DO NOT globally define either structs or objects. + * See issue: https://github.com/purduesigbots/pros/issues/48 + * + * Instead, put all objects/structs inside functions. + * + * + */ + +/* + * Runs the user autonomous code. This function will be started in its own task with the default + * priority and stack size whenever the robot is enabled via the Field Management System or the + * VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is + * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart + * the task, not re-start it from where it left off. + * + * Code running in the autonomous task cannot access information from the VEX Joystick. However, + * the autonomous function can be invoked from another task if a VEX Competition Switch is not + * available, and it can access joystick information if called in this way. + * + * The autonomous task may exit, unlike operatorControl() which should never exit. If it does + * so, the robot will await a switch to another mode or disable/enable cycle. + */ +void autonomous() { +} diff --git a/rosserial_vex_cortex/src/ros_lib/examples/helloworld.cpp b/rosserial_vex_cortex/src/ros_lib/examples/helloworld.cpp new file mode 100644 index 000000000..e7ab981c3 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/examples/helloworld.cpp @@ -0,0 +1,46 @@ +/* + * rosserial Publisher Example for VEX Cortex + * Prints "hello world!" + * + * + * + * Note: defining rosserial objects on the global scope will cause segmentation faults. + * put variables in functons. + */ + +#include +#include + +// this loop is run in setup function, which publishes at 50hz. +inline void loop(ros::NodeHandle & nh, ros::Publisher & p, std_msgs::String & str_msg, char* msgdata) +{ + str_msg.data = msgdata; + p.publish( &str_msg ); + nh.spinOnce(); + delay(20); +} + +// The setup function will start a publisher on the topic "chatter" and begin publishing there. +inline void setup() +{ + // debug logging + vexroslog("\n\n\n\n\r\t\tROSserial for VEX Cortex V2 - June 2018 - START\n\n\n\n\n\r"); + + // make a node handle object, string message, and publisher for that message. + ros::NodeHandle nh; + std_msgs::String str_msg; + ros::Publisher chatter("chatter\0", &str_msg); + + // set up rosserial, and prepare to publish the chatter message + nh.initNode(); + nh.advertise(chatter); + + // message data variable. + char* msg = (char*) malloc(20 * sizeof(char)); + while (1) { + + // send a message about the time! + sprintf(msg, "[%d] Hello there!!", (int) millis()); + loop(nh, chatter, str_msg, msg); + } +} diff --git a/rosserial_vex_cortex/src/ros_lib/examples/joydrive.cpp b/rosserial_vex_cortex/src/ros_lib/examples/joydrive.cpp new file mode 100644 index 000000000..32d2db637 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/examples/joydrive.cpp @@ -0,0 +1,63 @@ +/* + * Rosserial Vex Cortex joystick control demo + * + * This lets you control a VEX Clawbot (or any other robot, with some modification) + * using a Logitech Wireless gamepad F710. + * + * install the joystick_drivers ROS system dependency: http://wiki.ros.org/joystick_drivers?distro=melodic + * then run `rosrun joy joy_node _autorepeat_rate:=20` + * + * This also can work with a PS3 controller / wiimote (see the ROS joystick_drivers metapackage). + * + * Or any other ROS node that publishes joystick events! + */ + +#include "ros.h" +#include "main.h" +#include "sensor_msgs/Joy.h" + +#define MAX_SPEED 80 + +// fired every joystick message, uses the message to set motor powers on robot +inline void moveRobot( const sensor_msgs::Joy &joy_msg) { + motorSet(6, joy_msg.buttons[7] * MAX_SPEED - joy_msg.buttons[5] * MAX_SPEED); + motorSet(7, joy_msg.buttons[6] * MAX_SPEED - joy_msg.buttons[4] * MAX_SPEED); + + int lp = 0; + int rp = 0; + if(joy_msg.axes[0] < 0.0) { lp = -MAX_SPEED; rp = MAX_SPEED; } + else if(joy_msg.axes[0] > 0.0) { lp = MAX_SPEED; rp = -MAX_SPEED; } + else if(joy_msg.axes[1] < 0.0) { lp = -MAX_SPEED; rp = -MAX_SPEED; } + else if(joy_msg.axes[1] > 0.0) { lp = MAX_SPEED; rp = MAX_SPEED; } + else { + lp = MAX_SPEED * (joy_msg.axes[3] + joy_msg.axes[2]); + rp = MAX_SPEED * (joy_msg.axes[3] - joy_msg.axes[2]); + } + + lp = (abs(lp) > 10) ? lp : 0; + rp = (abs(rp) > 10) ? rp : 0; + + motorSet(1, -lp); + motorSet(10, rp); +} + +inline void begin(void*){ + // set up nodehandle instance and a subscriber for Joystick events + ros::NodeHandle nh; + ros::Subscriber sub("joy", &moveRobot); + + nh.initNode(); + nh.subscribe(sub); + + // loop for subscriber does not need delay - this ensures the node handle is as reliable as possible. + while (1) { + nh.spinOnce(); + } +} + +// is a setup function. +inline void setup() +{ + taskCreate(&begin, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_HIGHEST); +} + diff --git a/rosserial_vex_cortex/src/ros_lib/examples/twistdrive.cpp b/rosserial_vex_cortex/src/ros_lib/examples/twistdrive.cpp new file mode 100644 index 000000000..fd365ba8b --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/examples/twistdrive.cpp @@ -0,0 +1,48 @@ +/* + * Rosserial Vex Cortex keyboard/phone driving demo + * + * For keyboard + * --------- + * install the keyboard twist publisher: http://wiki.ros.org/teleop_twist_keyboard + * + * run the command described in the link, and key control will work on the robot! + * press center key to re-center. + * + * For phone + * -------- + * Install ROS Nav Map from the google play store + * follow setup instructions, use the joypad! + */ + +#include +#include "geometry_msgs/Twist.h" + +inline void handleControl(const geometry_msgs::Twist& t){ + // power motors using the message event! + // (default configuration is for a clawbot set up in the standard fashion). + motorSet(1, -(int) (100 * t.linear.x + 50 * t.angular.z)); + motorSet(10, (int) (100 * t.linear.x - 50 * t.angular.z)); +} + +// called from opcontrol.cpp +inline void setup() +{ + // debug logging + vexroslog("\n\n\n\n\r\t\tROSserial for VEX Cortex V2 - June 2018 - TWIST\n\n\n\n\n\r"); + + // make a node handle object and a subscriber for the Twist message. + ros::NodeHandle nh; + ros::Subscriber sub("cmd_vel\0", &handleControl); + + // set up rosserial, and prepare to publish the chatter message + nh.initNode(); + nh.subscribe(sub); + + // message data variable. + while (1) { + + // send a message about the time! + nh.spinOnce(); + delay(20); + } +} diff --git a/rosserial_vex_cortex/src/ros_lib/init.cpp b/rosserial_vex_cortex/src/ros_lib/init.cpp new file mode 100644 index 000000000..fa3ea726f --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/init.cpp @@ -0,0 +1,55 @@ +/** @file init.cpp + * @brief File for initialization code + * + * This file should contain the user initialize() function and any functions related to it. + * + * Any copyright is dedicated to the Public Domain. + * http://creativecommons.org/publicdomain/zero/1.0/ + * + * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be + * obtained from http://sourceforge.net/projects/freertos/files/ or on request. + */ + +#include "main.h" + +/* + * + * + * NO GLOBAL OBJECT DEFINITIONS! + * + * DO NOT globally define either structs or objects. + * See issue: https://github.com/purduesigbots/pros/issues/48 + * + * Instead, put all objects/structs inside functions. + * + * + */ + +/* + * Runs pre-initialization code. This function will be started in kernel mode one time while the + * VEX Cortex is starting up. As the scheduler is still paused, most API functions will fail. + * + * The purpose of this function is solely to set the default pin modes (pinMode()) and port + * states (digitalWrite()) of limit switches, push buttons, and solenoids. It can also safely + * configure a UART port (usartOpen()) but cannot set up an LCD (lcdInit()). + */ +void initializeIO() { + // this is needed to configure the debug serial connection for rosserial. + usartInit(uart2, 57600, SERIAL_8N1); +} + +/* + * Runs user initialization code. This function will be started in its own task with the default + * priority and stack size once when the robot is starting up. It is possible that the VEXnet + * communication link may not be fully established at this time, so reading from the VEX + * Joystick may fail. + * + * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global + * variables, and IMEs. + * + * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks + * will not start. An autonomous mode selection menu like the pre_auton() in other environments + * can be implemented in this task if desired. + */ +void initialize() { +} diff --git a/rosserial_vex_cortex/src/ros_lib/logger.h b/rosserial_vex_cortex/src/ros_lib/logger.h new file mode 100644 index 000000000..3626570b7 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/logger.h @@ -0,0 +1,39 @@ +#ifndef _ROSSERIAL_VEX_CORTEX_LOGGER_H_ +#define _ROSSERIAL_VEX_CORTEX_LOGGER_H_ + +#include "API.h" + +// VEXROS defines are used only locally. + +// debugging works over an alternative serial connection. +// 0 for no +// 1 for yes +#define VEXROS_DEBUG_MODE 0 + +// which serial connection to output debug messages on. +// To switch the serial connections, switch these values. +// note that stdin/stdout being seperate is unique - stdin/stdout can both be replaced with either uart1 or uart2, +// because UART1 and UART2 work as both input and output serial connections. +#define VEXROS_DEBUG_OUTPUT_SERIAL uart2 +#define VEXROS_ROSSERIAL_OUTPUT_SERIAL stdout +#define VEXROS_ROSSERIAL_INPUT_SERIAL stdin + +// logging function for debugging via print statements in the PROS environment. +// this macro simply adds formatting ontop of the regular pros frintf function. +// usage: vexroslog("hello, my favorite number is %d", 3); +// remember to include this header for logging in user code! +#define vexroslog(fmtstr, ...) fprintf((VEXROS_DEBUG_OUTPUT_SERIAL ), "[%d]: " " " fmtstr " " "\r\n", millis() ,##__VA_ARGS__) + +// will print to stdout, used by the serial client.; +#define vexroswrite(...) fprintf((VEXROS_ROSSERIAL_OUTPUT_SERIAL), __VA_ARGS__) +#define vexroswritechar(ch) fputc(ch, VEXROS_ROSSERIAL_OUTPUT_SERIAL) +#define vexrosreadchar() fgetc(VEXROS_ROSSERIAL_INPUT_SERIAL) + +// will only print if debug mode is on (see top of this file). +#define vexroslogdebug(fmtstr, ...) { \ + if(DEBUG_MODE) { \ + vexroslog(fmtstr, ##__VA_ARGS__); \ + } \ +} + +#endif diff --git a/rosserial_vex_cortex/src/ros_lib/opcontrol.cpp b/rosserial_vex_cortex/src/ros_lib/opcontrol.cpp new file mode 100644 index 000000000..08ca8e1a3 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/opcontrol.cpp @@ -0,0 +1,36 @@ +/** @file opcontrol.cpp + * @brief File for operator control code + * + * This file should contain the user operatorControl() function and any functions related to it. + * + * Any copyright is dedicated to the Public Domain. + * http://creativecommons.org/publicdomain/zero/1.0/ + * + * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be + * obtained from http://sourceforge.net/projects/freertos/files/ or on request. + */ + +#include "main.h" +#include "helloworld.cpp" + +/* + * + * + * NO GLOBAL OBJECT DEFINITIONS! + * + * DO NOT globally define either structs or objects. + * See issue: https://github.com/purduesigbots/pros/issues/48 + * + * Instead, put all objects/structs inside functions. + * + * + */ + +// entry point of user control period. +void operatorControl() { + + //running the hello world example + setup(); + + while(1) delay(20); +} diff --git a/rosserial_vex_cortex/src/ros_lib/ros.h b/rosserial_vex_cortex/src/ros_lib/ros.h new file mode 100644 index 000000000..38af2d76b --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/ros.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROSSERIAL_VEX_CORTEX_ROS_H_ +#define _ROSSERIAL_VEX_CORTEX_ROS_H_ + +#include "ros/node_handle.h" + +#include "CortexHardware.h" + +namespace ros { + typedef NodeHandle_ NodeHandle; +} + +#endif diff --git a/rosserial_vex_cortex/src/ros_lib/vexstrlen.h b/rosserial_vex_cortex/src/ros_lib/vexstrlen.h new file mode 100644 index 000000000..4393aa9e4 --- /dev/null +++ b/rosserial_vex_cortex/src/ros_lib/vexstrlen.h @@ -0,0 +1,16 @@ +#ifndef _ROSSERIAL_VEX_CORTEX_VEXSTRLEN_H_ +#define _ROSSERIAL_VEX_CORTEX_VEXSTRLEN_H_ + +#define MAX_VEXSTRLEN 2048 + +/* substitute string length function. + * if the length is too long or if there is no null terminator, behavior is undefined. + */ +inline uint32_t vexstrlen(const char* st) { + for(int i = 0; i < MAX_VEXSTRLEN; i++) { + if(st[i] == '\0') return i; + } + return MAX_VEXSTRLEN; +} + +#endif diff --git a/rosserial_vex_cortex/src/rosserial_vex_cortex/make_libraries.py b/rosserial_vex_cortex/src/rosserial_vex_cortex/make_libraries.py new file mode 100755 index 000000000..4c832ee2e --- /dev/null +++ b/rosserial_vex_cortex/src/rosserial_vex_cortex/make_libraries.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python + +##################################################################### +# Software License Agreement (BSD License) +# +# Copyright (c) 2013, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +THIS_PACKAGE = "rosserial_vex_cortex" + +__usage__ = """ +make_libraries.py generates the VEX Cortex rosserial library files. It requires the location of the include directory of a PROS kernel. + +rosrun rosserial_cortex make_libraries.py +""" + +import rospkg +import rosserial_client +from rosserial_client.make_library import * + +# for copying files +import shutil + +ROS_TO_EMBEDDED_TYPES = { + 'bool' : ('bool', 1, PrimitiveDataType, []), + 'byte' : ('int8_t', 1, PrimitiveDataType, []), + 'int8' : ('int8_t', 1, PrimitiveDataType, []), + 'char' : ('uint8_t', 1, PrimitiveDataType, []), + 'uint8' : ('uint8_t', 1, PrimitiveDataType, []), + 'int16' : ('int16_t', 2, PrimitiveDataType, []), + 'uint16' : ('uint16_t', 2, PrimitiveDataType, []), + 'int32' : ('int32_t', 4, PrimitiveDataType, []), + 'uint32' : ('uint32_t', 4, PrimitiveDataType, []), + 'int64' : ('int64_t', 8, PrimitiveDataType, []), + 'uint64' : ('uint64_t', 8, PrimitiveDataType, []), + 'float32' : ('float', 4, PrimitiveDataType, []), + 'float64' : ('float', 4, AVR_Float64DataType, []), + 'time' : ('ros::Time', 8, TimeDataType, ['ros/time']), + 'duration': ('ros::Duration', 8, TimeDataType, ['ros/duration']), + 'string' : ('char*', 0, StringDataType, []), + 'Header' : ('std_msgs::Header', 0, MessageDataType, ['std_msgs/Header']) +} + +# need correct inputs +if (len(sys.argv) < 2): + print __usage__ + exit() + +# get output path +path = sys.argv[1] +if path[-1] == "/": + path = path[0:-1] +print "\nExporting to %s" % path + +rospack = rospkg.RosPack() + +# copy ros_lib stuff in +rosserial_cortex_dir = rospack.get_path(THIS_PACKAGE) +shutil.copytree(rosserial_cortex_dir+"/src/ros_lib", path+"/ros_lib") +rosserial_client_copy_files(rospack, path+"/ros_lib/") + +# generate messages +rosserial_generate(rospack, path+"/ros_lib", ROS_TO_EMBEDDED_TYPES) + diff --git a/rosserial_vex_cortex/uartdiagram.png b/rosserial_vex_cortex/uartdiagram.png new file mode 100644 index 000000000..e7a5356b9 Binary files /dev/null and b/rosserial_vex_cortex/uartdiagram.png differ diff --git a/rosserial_windows/CHANGELOG.rst b/rosserial_windows/CHANGELOG.rst index ab5565a1a..7d0dbb5fe 100644 --- a/rosserial_windows/CHANGELOG.rst +++ b/rosserial_windows/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rosserial_windows ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_windows/CMakeLists.txt b/rosserial_windows/CMakeLists.txt index 876db61c1..f55657d29 100644 --- a/rosserial_windows/CMakeLists.txt +++ b/rosserial_windows/CMakeLists.txt @@ -1,17 +1,21 @@ cmake_minimum_required(VERSION 2.8.3) project(rosserial_windows) -find_package(catkin REQUIRED) -catkin_package(CATKIN_DEPENDS) - -install(DIRECTORY src/ros_lib - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + rosserial_client + sensor_msgs + std_msgs ) +catkin_package(CATKIN_DEPENDS) -install(DIRECTORY src/examples +install( + DIRECTORY src/ros_lib DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src ) -install(PROGRAMS src/rosserial_windows/make_libraries.py - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_libraries.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/rosserial_windows/package.xml b/rosserial_windows/package.xml index 2fb28b40c..621ce725f 100644 --- a/rosserial_windows/package.xml +++ b/rosserial_windows/package.xml @@ -1,8 +1,8 @@ rosserial_windows - 0.7.6 + 0.7.7 - Libraries and examples for ROSserial usage on Windows Platforms. + rosserial for Windows platforms. Kareem Shehata Kareem Shehata @@ -16,7 +16,6 @@ geometry_msgs nav_msgs rosserial_client - message_generation rospy rosserial_msgs diff --git a/rosserial_xbee/CHANGELOG.rst b/rosserial_xbee/CHANGELOG.rst index c1c745108..706f5f37a 100644 --- a/rosserial_xbee/CHANGELOG.rst +++ b/rosserial_xbee/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rosserial_xbee ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.7 (2017-11-29) +------------------ +* Fix catkin lint errors (`#296 `_) +* Contributors: Bei Chen Liu + 0.7.6 (2017-03-01) ------------------ diff --git a/rosserial_xbee/CMakeLists.txt b/rosserial_xbee/CMakeLists.txt index 1058e8f8e..6eeb24d88 100644 --- a/rosserial_xbee/CMakeLists.txt +++ b/rosserial_xbee/CMakeLists.txt @@ -6,10 +6,12 @@ catkin_package() catkin_python_setup() -install(PROGRAMS scripts/setup_xbee.py +install( + PROGRAMS scripts/setup_xbee.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(PROGRAMS scripts/xbee_network.py +install( + PROGRAMS scripts/xbee_network.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/rosserial_xbee/package.xml b/rosserial_xbee/package.xml index d1566062c..0f2a33833 100644 --- a/rosserial_xbee/package.xml +++ b/rosserial_xbee/package.xml @@ -1,16 +1,15 @@ rosserial_xbee - 0.7.6 + 0.7.7 + Allows multipoint communication between rosserial + nodes connected to an xbee. All nodes communicate back + to a master xbee connected to a computer running ROS. + + This software currently only works with Series 1 Xbees. - rosserial_xbee provides tools to do point to multipoint communication - between rosserial nodes connected to an xbee. All of the nodes - communicate back to a master xbee connected to a computer running - ROS. This software currently only works with Series 1 Xbees. - This pkg includes python code from the python-xbee project: http://code.google.com/p/python-xbee/ - Adam Stambler Paul Bouchier