Add rp2040 client

rosserial_rp2040/CHANGELOG.rst

@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rosserial_rp2040
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.1 (2022-07-12)
+------------------
+* initial version

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}
+)

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 <custom location>'
+
+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.
+

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

rosserial_rp2040/package.xml

@@ -0,0 +1,22 @@
+<package>
+  <name>rosserial_rp2040</name>
+  <version>0.9.2</version>
+  <description>
+    rosserial for rp2040 based microcontrollers.
+  </description>
+  <author>Michael Ferguson</author>
+  <author>Paul Tervoort</author>
+  <maintainer email="[email protected]">Paul Tervoort</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>rosserial_msgs</run_depend>
+  <run_depend>rosserial_client</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>rosserial_python</run_depend>
+</package>
+

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)

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)

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

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 <stdio.h>
+
+#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

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_<RP2040_Hardware_USB> NodeHandle;
+  #else
+  typedef ros::NodeHandle_<RP2040_Hardware> NodeHandle;
+  #endif
+}
+
+#endif