Updates to arduino-cmake to support Teensy boards and small changes to rosserial_client and rosserial_python

rosserial_arduino/arduino-cmake/cmake/Teensy2Toolchain.cmake

@@ -0,0 +1,23 @@
+# Add current directory to CMake Module path automatically
+if(EXISTS  ${CMAKE_CURRENT_LIST_DIR}/Platform/Arduino.cmake)
+    set(CMAKE_MODULE_PATH  ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR})
+endif()
+
+include(ArduinoToolchain)
+
+set(TEENSY "" CACHE STRING "Using one of the Teensy boards")
+
+set(CMAKE_C_COMPILER "avr-gcc")
+set(CMAKE_CXX_COMPILER "avr-g++")
+set(CMAKE_AR "avr-ar")
+set(CMAKE_LINKER "avr-ld")
+set(CMAKE_NM "avr-nm")
+set(CMAKE_OBJCOPY "avr-objcopy")
+set(CMAKE_OBJDUMP "avr-objdump")
+set(CMAKE_STRIP "avr-strip")
+set(CMAKE_RANLIB "avr-ranlib")
+
+set(CMAKE_EXE_LINKER_FLAGS "--specs=nosys.specs" CACHE INTERNAL "")
+
+set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
+set(ARDUINO_CXX_FLAGS "-fno-exceptions -fpermissive -felide-constructors -std=gnu++11")

rosserial_arduino/arduino-cmake/cmake/TeensyToolchain.cmake

@@ -0,0 +1,28 @@
+# Add current directory to CMake Module path automatically
+if(EXISTS  ${CMAKE_CURRENT_LIST_DIR}/Platform/Arduino.cmake)
+    set(CMAKE_MODULE_PATH  ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR})
+endif()
+
+include(ArduinoToolchain)
+
+list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/arm)
+list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/arm/arm-none-eabi)
+
+set(TEENSY "" CACHE STRING "Using one of the Teensy boards")
+set(CMAKE_SYSTEM_PROCESSOR arm)
+set(CMAKE_CROSSCOMPILING 1)
+
+set(CMAKE_C_COMPILER "arm-none-eabi-gcc")
+set(CMAKE_CXX_COMPILER "arm-none-eabi-g++")
+set(CMAKE_AR "arm-none-eabi-ar")
+set(CMAKE_LINKER "arm-none-eabi-ld")
+set(CMAKE_NM "arm-none-eabi-nm")
+set(CMAKE_OBJCOPY "arm-none-eabi-objcopy")
+set(CMAKE_OBJDUMP "arm-none-eabi-objdump")
+set(CMAKE_STRIP "arm-none-eabi-strip")
+set(CMAKE_RANLIB "arm-none-eabi-ranlib")
+
+set(CMAKE_EXE_LINKER_FLAGS "--specs=nosys.specs" CACHE INTERNAL "")
+
+set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
+set(ARDUINO_CXX_FLAGS "-std=gnu++14 -fno-exceptions -fpermissive -fno-rtti -fno-threadsafe-statics -felide-constructors -Wno-error=narrowing")

rosserial_arduino/cmake/rosserial_arduino-extras.cmake.em

@@ -2,7 +2,11 @@ cmake_minimum_required(VERSION 3.7.2)
 
 @[if DEVELSPACE]@
 set(ROSSERIAL_ARDUINO_TOOLCHAIN "@(CMAKE_CURRENT_SOURCE_DIR)/arduino-cmake/cmake/ArduinoToolchain.cmake")
+set(ROSSERIAL_TEENSY_TOOLCHAIN "@(CMAKE_CURRENT_SOURCE_DIR)/arduino-cmake/cmake/TeensyToolchain.cmake")
+set(ROSSERIAL_TEENSY2_TOOLCHAIN "@(CMAKE_CURRENT_SOURCE_DIR)/arduino-cmake/cmake/Teensy2Toolchain.cmake")
 @[else]@
 set(ROSSERIAL_ARDUINO_TOOLCHAIN "${rosserial_arduino_DIR}/../arduino-cmake/cmake/ArduinoToolchain.cmake")
+set(ROSSERIAL_TEENSY_TOOLCHAIN "${rosserial_arduino_DIR}/../arduino-cmake/cmake/TeensyToolchain.cmake")
+set(ROSSERIAL_TEENSY2_TOOLCHAIN "${rosserial_arduino_DIR}/../arduino-cmake/cmake/Teensy2Toolchain.cmake")
 @[end if]@
 

rosserial_arduino/nodes/serial_node.py

@@ -56,6 +56,16 @@
     # TIOCM_DTR_str) line, which causes an IOError, when using simulated port
     fix_pyserial_for_test = rospy.get_param('~fix_pyserial_for_test', False)
 
+    # for cases where waiting for data and/or callbacks is problematic
+    # e.g. when using high speed USB with specific timing constraints
+    fast_read = rospy.get_param('~fast_read', False)
+    if fast_read:
+        read_wait = False
+        callback_wait = False
+    else:
+        read_wait = rospy.get_param('~read_wait', True)
+        callback_wait = rospy.get_param('~callback_wait', True)
+
     # 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 :
@@ -65,7 +75,7 @@
         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()
+            client.run(read_wait, callback_wait)
         except KeyboardInterrupt:
             break
         except SerialException:

rosserial_arduino/src/rosserial_arduino/make_libraries_hardfloat64.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_arduino"
+
+__usage__ = """
+make_libraries.py generates the Arduino rosserial library files.  It
+requires the location of your Arduino sketchbook/libraries folder.
+
+rosrun rosserial_arduino make_libraries_hardfloat64.py <output_path>
+"""
+
+import rospkg
+import rosserial_client
+from rosserial_client.make_library import *
+
+# for copying files
+import shutil
+import os.path
+
+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' :   ('double',            8, PrimitiveDataType, []),
+    '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]
+output_path = os.path.join(sys.argv[1], "ros_lib")
+print("\nExporting to %s" % output_path)
+
+rospack = rospkg.RosPack()
+
+# copy ros_lib stuff in
+shutil.rmtree(output_path, ignore_errors=True)
+shutil.copytree(os.path.join(rospack.get_path(THIS_PACKAGE), "src", "ros_lib"), output_path)
+rosserial_client_copy_files(rospack, output_path)
+
+# generate messages
+rosserial_generate(rospack, output_path, ROS_TO_EMBEDDED_TYPES)

rosserial_client/src/ros_lib/ros/node_handle.h

@@ -371,14 +371,34 @@ class NodeHandle_ : public NodeHandleBase_
     last_sync_receive_time = hardware_.time();
   }
 
+  Time then(Time time)
+  {
+    time.sec += sec_offset;
+    time.nsec += nsec_offset;
+    normalizeSecNSec(time.sec, time.nsec);
+    return time;
+  }
+
+  Time then(uint32_t sec, uint32_t nsec)
+  {
+    return then(Time(sec, nsec));
+  }
+
+  Time then(uint32_t ms)
+  {
+    return then(ms / 1000, (ms % 1000) * 1000000UL);
+  }
+
+  Time then(double sec)
+  {
+    Time time;
+    time.fromSec(sec);
+    return then(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;
+    return then(hardware_.time());
   }
 
   void setNow(const Time & new_now)

rosserial_client/src/ros_lib/tf/transform_broadcaster.h

@@ -51,11 +51,11 @@ class TransformBroadcaster
     nh.advertise(publisher_);
   }
 
-  void sendTransform(geometry_msgs::TransformStamped &transform)
+  int sendTransform(geometry_msgs::TransformStamped &transform)
   {
     internal_msg.transforms_length = 1;
     internal_msg.transforms = &transform;
-    publisher_.publish(&internal_msg);
+    return publisher_.publish(&internal_msg);
   }
 
 private:

rosserial_embeddedlinux/src/rosserial_embeddedlinux/make_libraries.py

@@ -61,7 +61,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' :   ('double',            8, PrimitiveDataType, []),
     'time'    :   ('ros::Time',         8, TimeDataType, ['ros/time']),

rosserial_python/nodes/serial_node.py

@@ -54,6 +54,16 @@
     # TIOCM_DTR_str) line, which causes an IOError, when using simulated port
     fix_pyserial_for_test = rospy.get_param('~fix_pyserial_for_test', False)
 
+    # for cases where waiting for data and/or callbacks is problematic
+    # e.g. when using high speed USB with specific timing constraints
+    fast_read = rospy.get_param('~fast_read', False)
+    if fast_read:
+        read_wait = False
+        callback_wait = False
+    else:
+        read_wait = rospy.get_param('~read_wait', True)
+        callback_wait = rospy.get_param('~callback_wait', True)
+
     # Allows for assigning local parameters for tcp_port and fork_server with
     # global parameters as fallback to prevent breaking changes 
     if(rospy.has_param('~tcp_port')):
@@ -93,7 +103,7 @@
             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)
-                client.run()
+                client.run(read_wait, callback_wait)
             except KeyboardInterrupt:
                 break
             except SerialException:

rosserial_python/src/rosserial_python/SerialClient.py

@@ -444,7 +444,7 @@ def tryRead(self, length):
         except Exception as e:
             raise IOError("Serial Port read failure: %s" % e)
 
-    def run(self):
+    def run(self, read_wait=True, callback_wait=True):
         """ Forward recieved messages to appropriate publisher. """
 
         # Launch write thread.
@@ -472,7 +472,7 @@ def run(self):
             # bottom attempts to reconfigure the topics.
             try:
                 with self.read_lock:
-                    if self.port.inWaiting() < 1:
+                    if read_wait and self.port.inWaiting() < 1:
                         time.sleep(0.001)
                         continue
 
@@ -540,7 +540,8 @@ def run(self):
                     except KeyError:
                         rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
                         self.requestTopics()
-                    time.sleep(0.001)
+                    if callback_wait:
+                        time.sleep(0.001)
                 else:
                     rospy.loginfo("wrong checksum for topic id and msg")