diff --git a/rosserial_rp2040/CHANGELOG.rst b/rosserial_rp2040/CHANGELOG.rst new file mode 100644 index 000000000..05878c1fc --- /dev/null +++ b/rosserial_rp2040/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rosserial_rp2040 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.1 (2022-07-12) +------------------ +* initial version diff --git a/rosserial_rp2040/CMakeLists.txt b/rosserial_rp2040/CMakeLists.txt new file mode 100644 index 000000000..1e64089b5 --- /dev/null +++ b/rosserial_rp2040/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.7.2) +project(rosserial_rp2040) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS message_runtime +) + +install( + DIRECTORY src/ros_lib + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +) + +catkin_install_python( + PROGRAMS src/${PROJECT_NAME}/make_libraries.py nodes/serial_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/rosserial_rp2040/Readme.txt b/rosserial_rp2040/Readme.txt new file mode 100644 index 000000000..e0a1fbaf3 --- /dev/null +++ b/rosserial_rp2040/Readme.txt @@ -0,0 +1,16 @@ +Rosserial client for rp2040 usage + +- Install the catkin package for rosserial +- Install the rp2040 toolchain +- Create a rp2040 project +- Add the following lines to CMakeLists.txt (above target_link_libraries): + add_subdirectory($ENV{PICO_SDK_PATH}/lib/ros_lib build) + target_include_directories(${PROJECT_NAME} PRIVATE $ENV{PICO_SDK_PATH}/lib/ros_lib) +- Add 'ros_lib' to the target_link_libraries in CMakeLists.txt (without quotes) +- Execute the command 'rosrun rosserial_rp2040 make_libraries.py' to generate the ros_lib library files in the pico-sdk library folder +- To generate the ros_lib library files in a custom location use 'rosrun rosserial_rp2040 make_libraries.py ' + +To use the rp2040 USB serial connection for ros, use NodeHandleUSB instead of NodeHandle. + +The rosserial functionality can now be accessed from the project as described in the rosserial tutorials. + diff --git a/rosserial_rp2040/nodes/serial_node.py b/rosserial_rp2040/nodes/serial_node.py new file mode 100644 index 000000000..05c9cec84 --- /dev/null +++ b/rosserial_rp2040/nodes/serial_node.py @@ -0,0 +1,72 @@ +#!/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_rp2040 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/ttyACM0') + baud = int(rospy.get_param('~baud','57600')) + + # 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) + client.run() + except KeyboardInterrupt: + break + except SerialException: + sleep(1.0) + continue + except OSError: + sleep(1.0) + continue diff --git a/rosserial_rp2040/package.xml b/rosserial_rp2040/package.xml new file mode 100644 index 000000000..2566545f0 --- /dev/null +++ b/rosserial_rp2040/package.xml @@ -0,0 +1,22 @@ + + rosserial_rp2040 + 0.9.2 + + rosserial for rp2040 based microcontrollers. + + Michael Ferguson + Paul Tervoort + Paul Tervoort + BSD + + catkin + + message_generation + + rospy + rosserial_msgs + rosserial_client + message_runtime + rosserial_python + + diff --git a/rosserial_rp2040/setup.py b/rosserial_rp2040/setup.py new file mode 100644 index 000000000..12bc95ce3 --- /dev/null +++ b/rosserial_rp2040/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_rp2040'], + package_dir={'': 'src'}, + ) + +setup(**d) diff --git a/rosserial_rp2040/src/ros_lib/CMakeLists.txt b/rosserial_rp2040/src/ros_lib/CMakeLists.txt new file mode 100644 index 000000000..e00d2c2ee --- /dev/null +++ b/rosserial_rp2040/src/ros_lib/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.13) + +project(ros_lib) + +add_library(ros_lib STATIC + duration.cpp + time.cpp +) + +target_include_directories(ros_lib PRIVATE .) + +install(TARGETS ros_lib DESTINATION lib) +install(FILES ros.h DESTINATION include) diff --git a/rosserial_rp2040/src/ros_lib/RP2040_Hardware.h b/rosserial_rp2040/src/ros_lib/RP2040_Hardware.h new file mode 100644 index 000000000..b5cb62966 --- /dev/null +++ b/rosserial_rp2040/src/ros_lib/RP2040_Hardware.h @@ -0,0 +1,101 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Paul Tervoort + * 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. + */ + +#include "pico/stdlib.h" +#include "hardware/uart.h" + +#ifndef ROS_RP2040_Hardware_H_ +#define ROS_RP2040_Hardware_H_ + +class RP2040_Hardware +{ +public: + RP2040_Hardware(uart_inst_t* uart, int tx, int rx, long baud) + { + uart_ = uart; + tx_ = tx; + rx_ = rx; + baud_ = baud; + } + RP2040_Hardware(long baud) : RP2040_Hardware(uart0, 0, 1, baud) {} + RP2040_Hardware() : RP2040_Hardware(uart0, 0, 1, 57600) {} + + void custom_config(uart_inst_t* uart, int tx, int rx, long baud) + { + uart_ = uart; + tx_ = tx; + rx_ = rx; + baud_ = baud; + } + + // any initialization code necessary to use the serial port + void init() + { + uart_init(uart_, baud_); + + gpio_set_function(tx_, GPIO_FUNC_UART); + gpio_set_function(rx_, GPIO_FUNC_UART); + } + + // read a byte from the serial port. -1 = failure + int read() + { + if (uart_is_readable(uart_)) + { + return uart_getc(uart_); + } + + return -1; + } + + // write data to the connection to ROS + void write(uint8_t* data, int length) + { + uart_write_blocking(uart_, data, length); + } + + // returns milliseconds since start of program + unsigned long time() + { + return us_to_ms(time_us_32()); + } + +protected: + uart_inst_t* uart_; + int rx_; + int tx_; + long baud_; +}; + +#endif diff --git a/rosserial_rp2040/src/ros_lib/RP2040_Hardware_USB.h b/rosserial_rp2040/src/ros_lib/RP2040_Hardware_USB.h new file mode 100644 index 000000000..35ef8d0b3 --- /dev/null +++ b/rosserial_rp2040/src/ros_lib/RP2040_Hardware_USB.h @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Paul Tervoort + * 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. + */ + +#include "pico/stdlib.h" + +#include + +#ifndef ROS_RP2040_Hardware_USB_H_ +#define ROS_RP2040_Hardware_USB_H_ + +class RP2040_Hardware_USB +{ +public: + RP2040_Hardware_USB() {} + + // any initialization code necessary to use the serial port + void init() + { + stdio_usb_init(); + } + + // read a byte from the serial port. -1 = failure + int read() + { + return getchar_timeout_us(0); + } + + // write data to the connection to ROS + void write(uint8_t* data, int length) + { + for(int i = 0; i < length; i++) + { + putchar_raw(data[i]); + } + //printf("%.*s", length, data); + } + + // returns milliseconds since start of program + unsigned long time() + { + return us_to_ms(time_us_32()); + } +}; + +#endif diff --git a/rosserial_rp2040/src/ros_lib/ros.h b/rosserial_rp2040/src/ros_lib/ros.h new file mode 100644 index 000000000..92c12f59b --- /dev/null +++ b/rosserial_rp2040/src/ros_lib/ros.h @@ -0,0 +1,25 @@ +/* + * ros.h + */ + +#ifndef _ROS_H_ +#define _ROS_H_ + +#include "ros/node_handle.h" + +#ifdef USE_USBCON +#include "RP2040_Hardware_USB.h" +#else +#include "RP2040_Hardware.h" +#endif + +namespace ros +{ + #ifdef USE_USBCON + typedef ros::NodeHandle_ NodeHandle; + #else + typedef ros::NodeHandle_ NodeHandle; + #endif +} + +#endif diff --git a/rosserial_rp2040/src/rosserial_rp2040/SerialClient.py b/rosserial_rp2040/src/rosserial_rp2040/SerialClient.py new file mode 100644 index 000000000..c5800b607 --- /dev/null +++ b/rosserial_rp2040/src/rosserial_rp2040/SerialClient.py @@ -0,0 +1,54 @@ +#!/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): + super(SerialClient, self).__init__(*args, **kwargs) + + def sendDiagnostics(self, level, msg_text): + super(SerialClient, self).sendDiagnostics(level, msg_text) + diff --git a/rosserial_rp2040/src/rosserial_rp2040/__init__.py b/rosserial_rp2040/src/rosserial_rp2040/__init__.py new file mode 100644 index 000000000..c3666c67a --- /dev/null +++ b/rosserial_rp2040/src/rosserial_rp2040/__init__.py @@ -0,0 +1 @@ +from .SerialClient import * diff --git a/rosserial_rp2040/src/rosserial_rp2040/make_libraries.py b/rosserial_rp2040/src/rosserial_rp2040/make_libraries.py new file mode 100644 index 000000000..33e3c48e8 --- /dev/null +++ b/rosserial_rp2040/src/rosserial_rp2040/make_libraries.py @@ -0,0 +1,106 @@ +#!/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_rp2040" + +__usage__ = """ +make_libraries.py generates the rosserial library files. It +requires the location of your libraries folder. + +rosrun rosserial_rp2040 make_libraries.py + +If PICO_SDK_PATH is set properly then can be +omitted to install ros_lib in the pico-sdk library folder. + +rosrun rosserial_rp2040 make_libraries.py +""" + +import rospkg +import rosserial_client +from rosserial_client.make_library import * + +# for copying files +import shutil +import subprocess +import sys +import os +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' : ('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']) +} + +path = "" + +# get output path +if (len(sys.argv) > 1): # use argument path + path = sys.argv[1] +elif (os.environ.get('PICO_SDK_PATH')): # use pico sdk lib path + path = os.path.join(os.environ.get('PICO_SDK_PATH'), "lib") + print("\nNo export path specified. Using default pico lib path which is %s" % path) + if (not os.access(path, os.W_OK)): + subprocess.call(['sudo', 'chmod', '-R', '777', path]) +else: # no path + print(__usage__) + exit() + +output_path = os.path.join(path, "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)