├── .gitignore ├── .travis.yml ├── README.md ├── rosserial ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── rosserial_arduino ├── CHANGELOG.rst ├── CMakeLists.txt ├── arduino-cmake │ └── cmake │ │ ├── ArduinoToolchain.cmake │ │ └── Platform │ │ └── Arduino.cmake ├── cmake │ └── rosserial_arduino-extras.cmake.em ├── msg │ └── Adc.msg ├── nodes │ └── serial_node.py ├── package.xml ├── setup.py ├── src │ ├── ros_lib │ │ ├── ArduinoHardware.h │ │ ├── ArduinoTcpHardware.h │ │ ├── examples │ │ │ ├── ADC │ │ │ │ └── ADC.pde │ │ │ ├── Blink │ │ │ │ └── Blink.pde │ │ │ ├── BlinkM │ │ │ │ ├── BlinkM.pde │ │ │ │ └── BlinkM_funcs.h │ │ │ ├── BlinkerWithClass │ │ │ │ └── BlinkerWithClass.ino │ │ │ ├── Clapper │ │ │ │ └── Clapper.pde │ │ │ ├── Esp8266HelloWorld │ │ │ │ └── Esp8266HelloWorld.ino │ │ │ ├── HelloWorld │ │ │ │ └── HelloWorld.pde │ │ │ ├── IrRanger │ │ │ │ └── IrRanger.pde │ │ │ ├── Logging │ │ │ │ └── Logging.pde │ │ │ ├── Odom │ │ │ │ └── Odom.pde │ │ │ ├── ServiceClient │ │ │ │ ├── ServiceClient.pde │ │ │ │ └── client.py │ │ │ ├── ServiceServer │ │ │ │ └── ServiceServer.pde │ │ │ ├── ServoControl │ │ │ │ └── ServoControl.pde │ │ │ ├── TcpBlink │ │ │ │ └── TcpBlink.ino │ │ │ ├── TcpHelloWorld │ │ │ │ └── TcpHelloWorld.ino │ │ │ ├── Temperature │ │ │ │ └── Temperature.pde │ │ │ ├── TimeTF │ │ │ │ └── TimeTF.pde │ │ │ ├── Ultrasound │ │ │ │ └── Ultrasound.pde │ │ │ ├── button_example │ │ │ │ └── button_example.pde │ │ │ └── pubsub │ │ │ │ └── pubsub.pde │ │ ├── ros.h │ │ └── tests │ │ │ ├── array_test │ │ │ └── array_test.pde │ │ │ ├── float64_test │ │ │ └── float64_test.pde │ │ │ └── time_test │ │ │ └── time_test.pde │ └── rosserial_arduino │ │ ├── SerialClient.py │ │ ├── __init__.py │ │ └── make_libraries.py └── srv │ └── Test.srv ├── rosserial_chibios ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ ├── ros_lib │ ├── ChibiOSHardware.h │ ├── ros.h │ └── ros_lib.mk │ └── rosserial_chibios │ └── make_libraries.py ├── rosserial_client ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake │ └── rosserial_client-extras.cmake ├── package.xml ├── scripts │ └── make_libraries ├── setup.py ├── src │ ├── ros_lib │ │ ├── duration.cpp │ │ ├── ros │ │ │ ├── duration.h │ │ │ ├── msg.h │ │ │ ├── node_handle.h │ │ │ ├── publisher.h │ │ │ ├── service_client.h │ │ │ ├── service_server.h │ │ │ ├── subscriber.h │ │ │ └── time.h │ │ ├── tf │ │ │ ├── tf.h │ │ │ └── transform_broadcaster.h │ │ └── time.cpp │ └── rosserial_client │ │ ├── __init__.py │ │ └── make_library.py └── test │ ├── float64_test.cpp │ ├── subscriber_test.cpp │ └── time_test.cpp ├── rosserial_embeddedlinux ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ ├── examples │ ├── ExampleService │ │ └── ExampleService.cpp │ ├── ExampleServiceClient │ │ ├── ExampleServiceClient.cpp │ │ └── exampleService.py │ ├── ExampleSubscriber │ │ └── ExampleSubscriber.cpp │ ├── HelloRos │ │ └── HelloROS.cpp │ ├── VEXProMotor13Subscribe │ │ └── VEXProMotor13Subscribe.cpp │ ├── VEXProRangeMotorLoop │ │ └── VEXProRangeMotorLoop.cpp │ ├── VEXProRangePublish │ │ └── VEXProRangePublish.cpp │ └── VEXProServoSubscribe │ │ └── VEXProServoSubscribe.cpp │ ├── ros_lib │ ├── embedded_linux_comms.c │ ├── embedded_linux_hardware.h │ └── ros.h │ └── rosserial_embeddedlinux │ └── make_libraries.py ├── rosserial_mbed ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── msg │ └── Adc.msg ├── package.xml ├── src │ ├── examples │ │ ├── ADC │ │ │ ├── ADC.cpp │ │ │ └── makefile │ │ ├── Blink │ │ │ ├── Blink.cpp │ │ │ └── makefile │ │ ├── Clapper │ │ │ ├── Clapper.cpp │ │ │ └── makefile │ │ ├── GroveBuzzer │ │ │ ├── GroveBuzzer.cpp │ │ │ └── makefile │ │ ├── GroveCollision │ │ │ ├── GroveCollision.cpp │ │ │ └── makefile │ │ ├── GrovePIRMotionSensor │ │ │ ├── GrovePIRMotionSensor.cpp │ │ │ └── makefile │ │ ├── GroveTemperatureHumidity │ │ │ ├── DHT │ │ │ │ ├── DHT.cpp │ │ │ │ └── DHT.h │ │ │ ├── GroveTemperatureHumidity.cpp │ │ │ └── makefile │ │ ├── HelloWorld │ │ │ ├── HelloWorld.cpp │ │ │ └── makefile │ │ ├── IrRanger │ │ │ ├── IrRanger.cpp │ │ │ └── makefile │ │ ├── Logging │ │ │ ├── Logging.cpp │ │ │ └── makefile │ │ ├── MotorShield │ │ │ ├── MotorDriver │ │ │ │ ├── MotorDriver.cpp │ │ │ │ └── MotorDriver.h │ │ │ ├── MotorShield.cpp │ │ │ ├── SoftwarePWM │ │ │ │ ├── SoftwarePWM.cpp │ │ │ │ └── SoftwarePWM.h │ │ │ └── makefile │ │ ├── Odom │ │ │ ├── Odom.cpp │ │ │ └── makefile │ │ ├── ServiceClient │ │ │ ├── ServiceClient.cpp │ │ │ ├── client.py │ │ │ └── makefile │ │ ├── ServiceServer │ │ │ ├── ServiceServer.cpp │ │ │ └── makefile │ │ ├── ServoControl │ │ │ ├── Servo │ │ │ │ └── Servo.h │ │ │ ├── ServoControl.cpp │ │ │ └── makefile │ │ ├── Temperature │ │ │ ├── Temperature.cpp │ │ │ └── makefile │ │ ├── TimeTF │ │ │ ├── TimeTF.cpp │ │ │ └── makefile │ │ ├── Ultrasound │ │ │ ├── Ultrasound.cpp │ │ │ └── makefile │ │ ├── button_example │ │ │ ├── button_example.cpp │ │ │ └── makefile │ │ ├── makefile │ │ └── pubsub │ │ │ ├── makefile │ │ │ └── pubsub.cpp │ ├── ros_lib │ │ ├── BufferedSerial.lib │ │ ├── BufferedSerial │ │ │ ├── Buffer.lib │ │ │ ├── Buffer │ │ │ │ ├── Buffer.cpp │ │ │ │ └── Buffer.h │ │ │ ├── BufferedSerial.cpp │ │ │ └── BufferedSerial.h │ │ ├── MbedHardware.h │ │ └── ros.h │ ├── rosserial_mbed │ │ └── make_libraries.py │ └── tests │ │ ├── array_test │ │ ├── array_test.cpp │ │ └── makefile │ │ ├── float64_test │ │ ├── float64_test.cpp │ │ └── makefile │ │ └── time_test │ │ ├── makefile │ │ └── time_test.cpp └── srv │ └── Test.srv ├── rosserial_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ ├── Log.msg │ └── TopicInfo.msg ├── package.xml └── srv │ └── RequestParam.srv ├── rosserial_python ├── CHANGELOG.rst ├── CMakeLists.txt ├── nodes │ ├── message_info_service.py │ └── serial_node.py ├── package.xml ├── setup.py └── src │ └── rosserial_python │ ├── SerialClient.py │ └── __init__.py ├── rosserial_server ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rosserial_server │ │ ├── async_read_buffer.h │ │ ├── msg_lookup.h │ │ ├── serial_session.h │ │ ├── session.h │ │ ├── tcp_server.h │ │ ├── topic_handlers.h │ │ ├── udp_socket_session.h │ │ └── udp_stream.h ├── launch │ ├── serial.launch │ ├── socket.launch │ └── udp_socket.launch ├── package.xml └── src │ ├── msg_lookup.cpp │ ├── serial_node.cpp │ ├── socket_node.cpp │ └── udp_socket_node.cpp ├── rosserial_test ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rosserial_test │ │ ├── fixture.h │ │ ├── helpers.h │ │ └── ros.h ├── package.xml ├── scripts │ └── generate_client_ros_lib.py ├── src │ └── publish_subscribe.cpp └── test │ ├── rosserial_python_serial.test │ ├── rosserial_python_socket.test │ ├── rosserial_server_serial.test │ └── rosserial_server_socket.test ├── rosserial_tivac ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake │ └── rosserial_tivac-extras.cmake.em ├── package.xml ├── src │ ├── ros_lib │ │ ├── ros.h │ │ ├── startup_gcc.c │ │ ├── tivac_hardware.cpp │ │ ├── tivac_hardware.h │ │ ├── tivac_hardware_usb.h │ │ ├── usb_serial_structs.c │ │ └── usb_serial_structs.h │ ├── ros_lib_energia │ │ ├── TivaCHardware.h │ │ ├── examples │ │ │ ├── chatter │ │ │ │ └── chatter.ino │ │ │ └── rgb │ │ │ │ └── rgb.ino │ │ ├── ros.h │ │ └── tests │ │ │ ├── array_test │ │ │ ├── array_test.ino │ │ │ └── test_data │ │ │ ├── float64_test │ │ │ └── float64_test.ino │ │ │ └── time_test │ │ │ └── time_test.ino │ └── rosserial_tivac │ │ ├── make_libraries_energia │ │ └── make_libraries_tiva └── tivac-cmake │ ├── cmake │ └── TivaCToolchain.cmake │ ├── tiva.specs │ ├── tm4c123g.ld │ └── tm4c1294.ld ├── rosserial_vex_cortex ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── launch │ ├── hello_world.launch │ ├── joystick.launch │ └── minimal_robot.launch ├── package.xml ├── scripts │ └── genproject.sh ├── setup.py ├── src │ ├── ros_lib │ │ ├── CortexHardware.h │ │ ├── auto.cpp │ │ ├── examples │ │ │ ├── helloworld.cpp │ │ │ ├── joydrive.cpp │ │ │ └── twistdrive.cpp │ │ ├── init.cpp │ │ ├── logger.h │ │ ├── opcontrol.cpp │ │ ├── ros.h │ │ └── vexstrlen.h │ └── rosserial_vex_cortex │ │ └── make_libraries.py └── uartdiagram.png ├── rosserial_vex_v5 ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── launch │ ├── hello_world.launch │ ├── joystick.launch │ └── minimal_robot.launch ├── package.xml ├── scripts │ └── genproject.sh ├── setup.py ├── src │ ├── ros_lib │ │ ├── V5Hardware.h │ │ ├── autonomous.cpp │ │ ├── initialize.cpp │ │ ├── opcontrol.cpp │ │ ├── ros.h │ │ └── rosserial_vex_v5 │ │ │ ├── examples │ │ │ ├── helloworld.cpp │ │ │ └── helloworld.h │ │ │ └── utils │ │ │ ├── RingBuf.h │ │ │ └── RingBufHelpers.h │ └── rosserial_vex_v5 │ │ └── make_libraries.py └── uartdiagram.png ├── rosserial_windows ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ ├── examples │ ├── MessageReceive │ │ └── messagereceive.cpp │ └── TestDrive │ │ ├── readme.md │ │ └── testdrive.cpp │ ├── ros_lib │ ├── WindowsSocket.cpp │ ├── WindowsSocket.h │ └── ros.h │ └── rosserial_windows │ └── make_libraries.py └── rosserial_xbee ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── scripts ├── setup_xbee.py └── xbee_network.py ├── setup.py └── src ├── rosserial_xbee └── __init__.py └── xbee ├── LICENSE.txt ├── README ├── __init__.py ├── base.py ├── frame.py ├── helpers ├── __init__.py └── dispatch │ ├── __init__.py │ ├── dispatch.py │ └── tests │ ├── __init__.py │ ├── fake.py │ └── test_dispatch.py ├── ieee.py ├── tests ├── Fake.py ├── __init__.py ├── test_base.py ├── test_fake.py ├── test_frame.py ├── test_ieee.py └── test_zigbee.py └── zigbee.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.swp 3 | rosserial_server/doc/ 4 | *.out 5 | *.geany 6 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | language: generic 5 | 6 | services: 7 | - docker 8 | 9 | cache: 10 | directories: 11 | - $HOME/.ccache 12 | 13 | git: 14 | quiet: true 15 | 16 | env: 17 | global: 18 | - ROS_REPO=ros 19 | - CCACHE_DIR=$HOME/.ccache 20 | matrix: 21 | - ROS_DISTRO="noetic" ROS_REPO=ros-shadow-fixed 22 | 23 | install: 24 | - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master 25 | script: 26 | - .industrial_ci/travis.sh 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rosserial 2 | 3 | [![Build Status](https://travis-ci.org/ros-drivers/rosserial.svg?branch=melodic-devel)](https://travis-ci.org/ros-drivers/rosserial) 4 | 5 | Please see [rosserial on the ROS wiki](http://wiki.ros.org/rosserial) to get started. 6 | -------------------------------------------------------------------------------- /rosserial/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Fix Travis for Noetic + Python 3 14 | * Bump minimum CMake version to 3.7.2 (Melodic). 15 | * Contributors: Mike Purvis 16 | 17 | 0.8.0 (2018-10-11) 18 | ------------------ 19 | 20 | 0.7.7 (2017-11-29) 21 | ------------------ 22 | 23 | 0.7.6 (2017-03-01) 24 | ------------------ 25 | 26 | 0.7.5 (2016-11-22) 27 | ------------------ 28 | 29 | 0.7.4 (2016-09-21) 30 | ------------------ 31 | 32 | 0.7.3 (2016-08-05) 33 | ------------------ 34 | 35 | 0.7.2 (2016-07-15) 36 | ------------------ 37 | 38 | 0.7.1 (2015-07-06) 39 | ------------------ 40 | 41 | 0.7.0 (2015-04-23) 42 | ------------------ 43 | 44 | 0.6.3 (2014-11-05) 45 | ------------------ 46 | 47 | 0.6.2 (2014-09-10) 48 | ------------------ 49 | 50 | 0.6.1 (2014-06-30) 51 | ------------------ 52 | 53 | 0.6.0 (2014-06-11) 54 | ------------------ 55 | 56 | 0.5.6 (2014-06-11) 57 | ------------------ 58 | 59 | 0.5.5 (2014-01-14) 60 | ------------------ 61 | 62 | 0.5.4 (2013-10-17) 63 | ------------------ 64 | 65 | 0.5.3 (2013-09-21) 66 | ------------------ 67 | 68 | 0.5.2 (2013-07-17) 69 | ------------------ 70 | 71 | * Fix release version 72 | 73 | 0.5.1 (2013-07-15) 74 | ------------------ 75 | 76 | 0.4.5 (2013-07-02) 77 | ------------------ 78 | * fixes catkin warnings (`#40 `_) 79 | 80 | 0.4.4 (2013-03-20) 81 | ------------------ 82 | 83 | 0.4.3 (2013-03-13 14:08) 84 | ------------------------ 85 | 86 | 0.4.2 (2013-03-13 01:15) 87 | ------------------------ 88 | 89 | 0.4.1 (2013-03-09) 90 | ------------------ 91 | 92 | 0.4.0 (2013-03-08) 93 | ------------------ 94 | * make new metapackage 95 | -------------------------------------------------------------------------------- /rosserial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /rosserial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial 3 | 0.9.2 4 | 5 | Metapackage for core of rosserial. 6 | 7 | Michael Ferguson 8 | Paul Bouchier 9 | Mike Purvis 10 | BSD 11 | http://ros.org/wiki/rosserial 12 | 13 | catkin 14 | 15 | rosserial_msgs 16 | rosserial_client 17 | rosserial_python 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /rosserial_arduino/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_arduino) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | ) 7 | 8 | add_message_files(FILES 9 | Adc.msg 10 | ) 11 | 12 | add_service_files(FILES 13 | Test.srv 14 | ) 15 | 16 | catkin_python_setup() 17 | 18 | generate_messages() 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS message_runtime 22 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake 23 | ) 24 | 25 | install( 26 | DIRECTORY src/ros_lib 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 28 | ) 29 | 30 | install( 31 | DIRECTORY arduino-cmake 32 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 33 | ) 34 | 35 | catkin_install_python( 36 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py nodes/serial_node.py 37 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | -------------------------------------------------------------------------------- /rosserial_arduino/cmake/rosserial_arduino-extras.cmake.em: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | 3 | @[if DEVELSPACE]@ 4 | set(ROSSERIAL_ARDUINO_TOOLCHAIN "@(CMAKE_CURRENT_SOURCE_DIR)/arduino-cmake/cmake/ArduinoToolchain.cmake") 5 | @[else]@ 6 | set(ROSSERIAL_ARDUINO_TOOLCHAIN "${rosserial_arduino_DIR}/../arduino-cmake/cmake/ArduinoToolchain.cmake") 7 | @[end if]@ 8 | 9 | -------------------------------------------------------------------------------- /rosserial_arduino/msg/Adc.msg: -------------------------------------------------------------------------------- 1 | uint16 adc0 2 | uint16 adc1 3 | uint16 adc2 4 | uint16 adc3 5 | uint16 adc4 6 | uint16 adc5 7 | -------------------------------------------------------------------------------- /rosserial_arduino/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_arduino 3 | 0.9.2 4 | 5 | rosserial for Arduino/AVR platforms. 6 | 7 | Michael Ferguson 8 | Adam Stambler 9 | Paul Bouchier 10 | Mike Purvis 11 | BSD 12 | http://ros.org/wiki/rosserial_arduino 13 | 14 | catkin 15 | 16 | message_generation 17 | 18 | arduino-core 19 | rospy 20 | rosserial_msgs 21 | rosserial_client 22 | message_runtime 23 | rosserial_python 24 | 25 | -------------------------------------------------------------------------------- /rosserial_arduino/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosserial_arduino'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/ADC/ADC.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial ADC Example 3 | * 4 | * This is a poor man's Oscilloscope. It does not have the sampling 5 | * rate or accuracy of a commerical scope, but it is great to get 6 | * an analog value into ROS in a pinch. 7 | */ 8 | 9 | #if (ARDUINO >= 100) 10 | #include 11 | #else 12 | #include 13 | #endif 14 | #include 15 | #include 16 | 17 | ros::NodeHandle nh; 18 | 19 | rosserial_arduino::Adc adc_msg; 20 | ros::Publisher p("adc", &adc_msg); 21 | 22 | void setup() 23 | { 24 | pinMode(13, OUTPUT); 25 | nh.initNode(); 26 | 27 | nh.advertise(p); 28 | } 29 | 30 | //We average the analog reading to elminate some of the noise 31 | int averageAnalog(int pin){ 32 | int v=0; 33 | for(int i=0; i<4; i++) v+= analogRead(pin); 34 | return v/4; 35 | } 36 | 37 | long adc_timer; 38 | 39 | void loop() 40 | { 41 | adc_msg.adc0 = averageAnalog(0); 42 | adc_msg.adc1 = averageAnalog(1); 43 | adc_msg.adc2 = averageAnalog(2); 44 | adc_msg.adc3 = averageAnalog(3); 45 | adc_msg.adc4 = averageAnalog(4); 46 | adc_msg.adc5 = averageAnalog(5); 47 | 48 | p.publish(&adc_msg); 49 | 50 | nh.spinOnce(); 51 | } 52 | 53 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Blink/Blink.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Subscriber Example 3 | * Blinks an LED on callback 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | 11 | void messageCb( const std_msgs::Empty& toggle_msg){ 12 | digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN)); // blink the led 13 | } 14 | 15 | ros::Subscriber sub("toggle_led", &messageCb ); 16 | 17 | void setup() 18 | { 19 | pinMode(LED_BUILTIN, OUTPUT); 20 | nh.initNode(); 21 | nh.subscribe(sub); 22 | } 23 | 24 | void loop() 25 | { 26 | nh.spinOnce(); 27 | delay(1); 28 | } 29 | 30 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/BlinkerWithClass/BlinkerWithClass.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | ros::NodeHandle nh; 6 | 7 | class Blinker 8 | { 9 | public: 10 | Blinker(byte pin, uint16_t period) 11 | : pin_(pin), period_(period), 12 | subscriber_("set_blink_period", &Blinker::set_period_callback, this), 13 | service_server_("activate_blinker", &Blinker::service_callback, this) 14 | {} 15 | 16 | void init(ros::NodeHandle& nh) 17 | { 18 | pinMode(pin_, OUTPUT); 19 | nh.subscribe(subscriber_); 20 | nh.advertiseService(service_server_); 21 | } 22 | 23 | void run() 24 | { 25 | if(active_ && ((millis() - last_time_) >= period_)) 26 | { 27 | last_time_ = millis(); 28 | digitalWrite(pin_, !digitalRead(pin_)); 29 | } 30 | } 31 | 32 | void set_period_callback(const std_msgs::UInt16& msg) 33 | { 34 | period_ = msg.data; 35 | } 36 | 37 | void service_callback(const std_srvs::SetBool::Request& req, 38 | std_srvs::SetBool::Response& res) 39 | { 40 | active_ = req.data; 41 | res.success = true; 42 | if(req.data) 43 | res.message = "Blinker ON"; 44 | else 45 | res.message = "Blinker OFF"; 46 | } 47 | 48 | private: 49 | const byte pin_; 50 | bool active_ = true; 51 | uint16_t period_; 52 | uint32_t last_time_; 53 | ros::Subscriber subscriber_; 54 | ros::ServiceServer service_server_; 55 | }; 56 | 57 | Blinker blinker(LED_BUILTIN, 500); 58 | 59 | void setup() 60 | { 61 | nh.initNode(); 62 | blinker.init(nh); 63 | } 64 | 65 | void loop() 66 | { 67 | blinker.run(); 68 | nh.spinOnce(); 69 | delay(1); 70 | } 71 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Clapper/Clapper.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Clapper Example 3 | * 4 | * This code is a very simple example of the kinds of 5 | * custom sensors that you can easily set up with rosserial 6 | * and Arduino. This code uses a microphone attached to 7 | * analog pin 5 detect two claps (2 loud sounds). 8 | * You can use this clapper, for example, to command a robot 9 | * in the area to come do your bidding. 10 | */ 11 | 12 | #if (ARDUINO >= 100) 13 | #include 14 | #else 15 | #include 16 | #endif 17 | #include 18 | #include 19 | 20 | ros::NodeHandle nh; 21 | 22 | std_msgs::Empty clap_msg; 23 | ros::Publisher p("clap", &clap_msg); 24 | 25 | enum clapper_state { clap1, clap_one_waiting, pause, clap2}; 26 | clapper_state clap; 27 | 28 | int volume_thresh = 200; //a clap sound needs to be: 29 | //abs(clap_volume) > average noise + volume_thresh 30 | int mic_pin = 5; 31 | int adc_ave=0; 32 | 33 | void setup() 34 | { 35 | pinMode(13, OUTPUT); 36 | nh.initNode(); 37 | 38 | nh.advertise(p); 39 | 40 | //measure the average volume of the noise in the area 41 | for (int i =0; i<10;i++) adc_ave += analogRead(mic_pin); 42 | adc_ave /= 10; 43 | } 44 | 45 | long event_timer; 46 | 47 | void loop() 48 | { 49 | int mic_val = 0; 50 | for(int i=0; i<4; i++) mic_val += analogRead(mic_pin); 51 | 52 | mic_val = mic_val/4-adc_ave; 53 | 54 | switch(clap){ 55 | case clap1: 56 | if (abs(mic_val) > volume_thresh){ 57 | clap = clap_one_waiting; 58 | event_timer = millis(); 59 | } 60 | break; 61 | case clap_one_waiting: 62 | if ( (abs(mic_val) < 30) && ( (millis() - event_timer) > 20 ) ) 63 | { 64 | clap= pause; 65 | event_timer = millis(); 66 | 67 | } 68 | break; 69 | case pause: // make sure there is a pause between 70 | // the loud sounds 71 | if ( mic_val > volume_thresh) 72 | { 73 | clap = clap1; 74 | 75 | } 76 | else if ( (millis()-event_timer)> 60) { 77 | clap = clap2; 78 | event_timer = millis(); 79 | 80 | } 81 | break; 82 | case clap2: 83 | if (abs(mic_val) > volume_thresh){ //we have got a double clap! 84 | clap = clap1; 85 | p.publish(&clap_msg); 86 | } 87 | else if ( (millis()-event_timer)> 200) { 88 | clap= clap1; // no clap detected, reset state machine 89 | } 90 | 91 | break; 92 | } 93 | nh.spinOnce(); 94 | } 95 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Esp8266HelloWorld/Esp8266HelloWorld.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | * This intend to connect to a Wifi Access Point 5 | * and a rosserial socket server. 6 | * You can launch the rosserial socket server with 7 | * roslaunch rosserial_server socket.launch 8 | * The default port is 11411 9 | * 10 | */ 11 | #include 12 | 13 | #define ROSSERIAL_ARDUINO_TCP 14 | 15 | #include 16 | #include 17 | 18 | const char* ssid = "your-ssid"; 19 | const char* password = "your-password"; 20 | // Set the rosserial socket server IP address 21 | IPAddress server(192,168,1,1); 22 | // Set the rosserial socket server port 23 | const uint16_t serverPort = 11411; 24 | 25 | ros::NodeHandle nh; 26 | // Make a chatter publisher 27 | std_msgs::String str_msg; 28 | ros::Publisher chatter("chatter", &str_msg); 29 | 30 | // Be polite and say hello 31 | char hello[13] = "hello world!"; 32 | 33 | void setup() 34 | { 35 | // Use ESP8266 serial to monitor the process 36 | Serial.begin(115200); 37 | Serial.println(); 38 | Serial.print("Connecting to "); 39 | Serial.println(ssid); 40 | 41 | // Connect the ESP8266 the the wifi AP 42 | WiFi.begin(ssid, password); 43 | while (WiFi.status() != WL_CONNECTED) { 44 | delay(500); 45 | Serial.print("."); 46 | } 47 | Serial.println(""); 48 | Serial.println("WiFi connected"); 49 | Serial.println("IP address: "); 50 | Serial.println(WiFi.localIP()); 51 | 52 | // Set the connection to rosserial socket server 53 | nh.getHardware()->setConnection(server, serverPort); 54 | nh.initNode(); 55 | 56 | // Another way to get IP 57 | Serial.print("IP = "); 58 | Serial.println(nh.getHardware()->getLocalIP()); 59 | 60 | // Start to be polite 61 | nh.advertise(chatter); 62 | } 63 | 64 | void loop() 65 | { 66 | 67 | if (nh.connected()) { 68 | Serial.println("Connected"); 69 | // Say hello 70 | str_msg.data = hello; 71 | chatter.publish( &str_msg ); 72 | } else { 73 | Serial.println("Not Connected"); 74 | } 75 | nh.spinOnce(); 76 | // Loop exproximativly at 1Hz 77 | delay(1000); 78 | } 79 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/HelloWorld/HelloWorld.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | 11 | std_msgs::String str_msg; 12 | ros::Publisher chatter("chatter", &str_msg); 13 | 14 | char hello[13] = "hello world!"; 15 | 16 | void setup() 17 | { 18 | nh.initNode(); 19 | nh.advertise(chatter); 20 | } 21 | 22 | void loop() 23 | { 24 | str_msg.data = hello; 25 | chatter.publish( &str_msg ); 26 | nh.spinOnce(); 27 | delay(1000); 28 | } 29 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/IrRanger/IrRanger.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial IR Ranger Example 3 | * 4 | * This example is calibrated for the Sharp GP2D120XJ00F. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | sensor_msgs::Range range_msg; 15 | ros::Publisher pub_range( "range_data", &range_msg); 16 | 17 | const int analog_pin = 0; 18 | unsigned long range_timer; 19 | 20 | /* 21 | * getRange() - samples the analog input from the ranger 22 | * and converts it into meters. 23 | */ 24 | float getRange(int pin_num){ 25 | int sample; 26 | // Get data 27 | sample = analogRead(pin_num)/4; 28 | // if the ADC reading is too low, 29 | // then we are really far away from anything 30 | if(sample < 10) 31 | return 254; // max range 32 | // Magic numbers to get cm 33 | sample= 1309/(sample-3); 34 | return (sample - 1)/100; //convert to meters 35 | } 36 | 37 | char frameid[] = "/ir_ranger"; 38 | 39 | void setup() 40 | { 41 | nh.initNode(); 42 | nh.advertise(pub_range); 43 | 44 | range_msg.radiation_type = sensor_msgs::Range::INFRARED; 45 | range_msg.header.frame_id = frameid; 46 | range_msg.field_of_view = 0.01; 47 | range_msg.min_range = 0.03; 48 | range_msg.max_range = 0.4; 49 | 50 | } 51 | 52 | void loop() 53 | { 54 | // publish the range value every 50 milliseconds 55 | // since it takes that long for the sensor to stabilize 56 | if ( (millis()-range_timer) > 50){ 57 | range_msg.range = getRange(analog_pin); 58 | range_msg.header.stamp = nh.now(); 59 | pub_range.publish(&range_msg); 60 | range_timer = millis() + 50; 61 | } 62 | nh.spinOnce(); 63 | } 64 | 65 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Logging/Logging.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | 13 | std_msgs::String str_msg; 14 | ros::Publisher chatter("chatter", &str_msg); 15 | 16 | char hello[13] = "hello world!"; 17 | 18 | 19 | char debug[]= "debug statements"; 20 | char info[] = "infos"; 21 | char warn[] = "warnings"; 22 | char error[] = "errors"; 23 | char fatal[] = "fatalities"; 24 | 25 | void setup() 26 | { 27 | pinMode(13, OUTPUT); 28 | nh.initNode(); 29 | nh.advertise(chatter); 30 | } 31 | 32 | void loop() 33 | { 34 | str_msg.data = hello; 35 | chatter.publish( &str_msg ); 36 | 37 | nh.logdebug(debug); 38 | nh.loginfo(info); 39 | nh.logwarn(warn); 40 | nh.logerror(error); 41 | nh.logfatal(fatal); 42 | 43 | nh.spinOnce(); 44 | delay(500); 45 | } 46 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Odom/Odom.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Planar Odometry Example 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | double x = 1.0; 16 | double y = 0.0; 17 | double theta = 1.57; 18 | 19 | char base_link[] = "/base_link"; 20 | char odom[] = "/odom"; 21 | 22 | void setup() 23 | { 24 | nh.initNode(); 25 | broadcaster.init(nh); 26 | } 27 | 28 | void loop() 29 | { 30 | // drive in a circle 31 | double dx = 0.2; 32 | double dtheta = 0.18; 33 | x += cos(theta)*dx*0.1; 34 | y += sin(theta)*dx*0.1; 35 | theta += dtheta*0.1; 36 | if(theta > 3.14) 37 | theta=-3.14; 38 | 39 | // tf odom->base_link 40 | t.header.frame_id = odom; 41 | t.child_frame_id = base_link; 42 | 43 | t.transform.translation.x = x; 44 | t.transform.translation.y = y; 45 | 46 | t.transform.rotation = tf::createQuaternionFromYaw(theta); 47 | t.header.stamp = nh.now(); 48 | 49 | broadcaster.sendTransform(t); 50 | nh.spinOnce(); 51 | 52 | delay(10); 53 | } 54 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/ServiceClient/ServiceClient.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Client 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_arduino::Test; 11 | 12 | ros::ServiceClient client("test_srv"); 13 | 14 | std_msgs::String str_msg; 15 | ros::Publisher chatter("chatter", &str_msg); 16 | 17 | char hello[13] = "hello world!"; 18 | 19 | void setup() 20 | { 21 | nh.initNode(); 22 | nh.serviceClient(client); 23 | nh.advertise(chatter); 24 | while(!nh.connected()) nh.spinOnce(); 25 | nh.loginfo("Startup complete"); 26 | } 27 | 28 | void loop() 29 | { 30 | Test::Request req; 31 | Test::Response res; 32 | req.input = hello; 33 | client.call(req, res); 34 | str_msg.data = res.output; 35 | chatter.publish( &str_msg ); 36 | nh.spinOnce(); 37 | delay(100); 38 | } 39 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/ServiceClient/client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Sample code to use with ServiceClient.pde 5 | """ 6 | 7 | import roslib; roslib.load_manifest("rosserial_arduino") 8 | import rospy 9 | 10 | from rosserial_arduino.srv import * 11 | 12 | def callback(req): 13 | print("The arduino is calling! Please send it a message:") 14 | t = TestResponse() 15 | t.output = raw_input() 16 | return t 17 | 18 | rospy.init_node("service_client_test") 19 | rospy.Service("test_srv", Test, callback) 20 | rospy.spin() 21 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/ServiceServer/ServiceServer.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Server 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_arduino::Test; 11 | 12 | int i; 13 | void callback(const Test::Request & req, Test::Response & res){ 14 | if((i++)%2) 15 | res.output = "hello"; 16 | else 17 | res.output = "world"; 18 | } 19 | 20 | ros::ServiceServer server("test_srv",&callback); 21 | 22 | std_msgs::String str_msg; 23 | ros::Publisher chatter("chatter", &str_msg); 24 | 25 | char hello[13] = "hello world!"; 26 | 27 | void setup() 28 | { 29 | nh.initNode(); 30 | nh.advertiseService(server); 31 | nh.advertise(chatter); 32 | } 33 | 34 | void loop() 35 | { 36 | str_msg.data = hello; 37 | chatter.publish( &str_msg ); 38 | nh.spinOnce(); 39 | delay(10); 40 | } 41 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/ServoControl/ServoControl.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Servo Control Example 3 | * 4 | * This sketch demonstrates the control of hobby R/C servos 5 | * using ROS and the arduiono 6 | * 7 | * For the full tutorial write up, visit 8 | * www.ros.org/wiki/rosserial_arduino_demos 9 | * 10 | * For more information on the Arduino Servo Library 11 | * Checkout : 12 | * http://www.arduino.cc/en/Reference/Servo 13 | */ 14 | 15 | #if (ARDUINO >= 100) 16 | #include 17 | #else 18 | #include 19 | #endif 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | ros::NodeHandle nh; 26 | 27 | Servo servo; 28 | 29 | void servo_cb( const std_msgs::UInt16& cmd_msg){ 30 | servo.write(cmd_msg.data); //set servo angle, should be from 0-180 31 | digitalWrite(13, HIGH-digitalRead(13)); //toggle led 32 | } 33 | 34 | 35 | ros::Subscriber sub("servo", servo_cb); 36 | 37 | void setup(){ 38 | pinMode(13, OUTPUT); 39 | 40 | nh.initNode(); 41 | nh.subscribe(sub); 42 | 43 | servo.attach(9); //attach it to pin 9 44 | } 45 | 46 | void loop(){ 47 | nh.spinOnce(); 48 | delay(1); 49 | } 50 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/TcpBlink/TcpBlink.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Subscriber Example using TCP on Arduino Shield (Wiznet W5100 based) 3 | * Blinks an LED on callback 4 | */ 5 | #include 6 | #include 7 | 8 | #define ROSSERIAL_ARDUINO_TCP 9 | 10 | #include 11 | #include 12 | 13 | ros::NodeHandle nh; 14 | 15 | // Shield settings 16 | byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; 17 | IPAddress ip(192, 168, 0, 177); 18 | 19 | // Server settings 20 | IPAddress server(192, 168, 0, 11); 21 | uint16_t serverPort = 11411; 22 | 23 | const uint8_t ledPin = 6; // 13 already used for SPI connection with the shield 24 | 25 | void messageCb( const std_msgs::Empty&){ 26 | digitalWrite(ledPin, HIGH-digitalRead(ledPin)); // blink the led 27 | } 28 | 29 | ros::Subscriber sub("toggle_led", &messageCb ); 30 | 31 | void setup() 32 | { 33 | Ethernet.begin(mac, ip); 34 | // give the Ethernet shield a second to initialize: 35 | delay(1000); 36 | pinMode(ledPin, OUTPUT); 37 | nh.getHardware()->setConnection(server, serverPort); 38 | nh.initNode(); 39 | nh.subscribe(sub); 40 | } 41 | 42 | void loop() 43 | { 44 | nh.spinOnce(); 45 | delay(1); 46 | } 47 | 48 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/TcpHelloWorld/TcpHelloWorld.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | * This intend to connect to an Arduino Ethernet Shield 5 | * and a rosserial socket server. 6 | * You can launch the rosserial socket server with 7 | * roslaunch rosserial_server socket.launch 8 | * The default port is 11411 9 | * 10 | */ 11 | #include 12 | #include 13 | 14 | // To use the TCP version of rosserial_arduino 15 | #define ROSSERIAL_ARDUINO_TCP 16 | 17 | #include 18 | #include 19 | 20 | // Set the shield settings 21 | byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; 22 | IPAddress ip(192, 168, 0, 177); 23 | 24 | // Set the rosserial socket server IP address 25 | IPAddress server(192,168,0,11); 26 | // Set the rosserial socket server port 27 | const uint16_t serverPort = 11411; 28 | 29 | ros::NodeHandle nh; 30 | // Make a chatter publisher 31 | std_msgs::String str_msg; 32 | ros::Publisher chatter("chatter", &str_msg); 33 | 34 | // Be polite and say hello 35 | char hello[13] = "hello world!"; 36 | uint16_t period = 1000; 37 | uint32_t last_time = 0; 38 | 39 | void setup() 40 | { 41 | // Use serial to monitor the process 42 | Serial.begin(115200); 43 | 44 | // Connect the Ethernet 45 | Ethernet.begin(mac, ip); 46 | 47 | // Let some time for the Ethernet Shield to be initialized 48 | delay(1000); 49 | 50 | Serial.println(""); 51 | Serial.println("Ethernet connected"); 52 | Serial.println("IP address: "); 53 | Serial.println(Ethernet.localIP()); 54 | 55 | // Set the connection to rosserial socket server 56 | nh.getHardware()->setConnection(server, serverPort); 57 | nh.initNode(); 58 | 59 | // Another way to get IP 60 | Serial.print("IP = "); 61 | Serial.println(nh.getHardware()->getLocalIP()); 62 | 63 | // Start to be polite 64 | nh.advertise(chatter); 65 | } 66 | 67 | void loop() 68 | { 69 | if(millis() - last_time >= period) 70 | { 71 | last_time = millis(); 72 | if (nh.connected()) 73 | { 74 | Serial.println("Connected"); 75 | // Say hello 76 | str_msg.data = hello; 77 | chatter.publish( &str_msg ); 78 | } else { 79 | Serial.println("Not Connected"); 80 | } 81 | } 82 | nh.spinOnce(); 83 | delay(1); 84 | } 85 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Temperature/Temperature.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Temperature Sensor Example 3 | * 4 | * This tutorial demonstrates the usage of the 5 | * Sparkfun TMP102 Digital Temperature Breakout board 6 | * http://www.sparkfun.com/products/9418 7 | * 8 | * Source Code Based off of: 9 | * http://wiring.org.co/learning/libraries/tmp102sparkfun.html 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | ros::NodeHandle nh; 17 | 18 | 19 | std_msgs::Float32 temp_msg; 20 | ros::Publisher pub_temp("temperature", &temp_msg); 21 | 22 | 23 | // From the datasheet the BMP module address LSB distinguishes 24 | // between read (1) and write (0) operations, corresponding to 25 | // address 0x91 (read) and 0x90 (write). 26 | // shift the address 1 bit right (0x91 or 0x90), the Wire library only needs the 7 27 | // most significant bits for the address 0x91 >> 1 = 0x48 28 | // 0x90 >> 1 = 0x48 (72) 29 | 30 | int sensorAddress = 0x91 >> 1; // From datasheet sensor address is 0x91 31 | // shift the address 1 bit right, the Wire library only needs the 7 32 | // most significant bits for the address 33 | 34 | 35 | void setup() 36 | { 37 | Wire.begin(); // join i2c bus (address optional for master) 38 | 39 | nh.initNode(); 40 | nh.advertise(pub_temp); 41 | 42 | } 43 | 44 | long publisher_timer; 45 | 46 | void loop() 47 | { 48 | 49 | if (millis() > publisher_timer) { 50 | // step 1: request reading from sensor 51 | Wire.requestFrom(sensorAddress,2); 52 | delay(10); 53 | if (2 <= Wire.available()) // if two bytes were received 54 | { 55 | byte msb; 56 | byte lsb; 57 | int temperature; 58 | 59 | msb = Wire.read(); // receive high byte (full degrees) 60 | lsb = Wire.read(); // receive low byte (fraction degrees) 61 | temperature = ((msb) << 4); // MSB 62 | temperature |= (lsb >> 4); // LSB 63 | 64 | temp_msg.data = temperature*0.0625; 65 | pub_temp.publish(&temp_msg); 66 | } 67 | 68 | publisher_timer = millis() + 1000; 69 | } 70 | 71 | nh.spinOnce(); 72 | } 73 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/TimeTF/TimeTF.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Time and TF Example 3 | * Publishes a transform at current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | char base_link[] = "/base_link"; 16 | char odom[] = "/odom"; 17 | 18 | void setup() 19 | { 20 | nh.initNode(); 21 | broadcaster.init(nh); 22 | } 23 | 24 | void loop() 25 | { 26 | t.header.frame_id = odom; 27 | t.child_frame_id = base_link; 28 | t.transform.translation.x = 1.0; 29 | t.transform.rotation.x = 0.0; 30 | t.transform.rotation.y = 0.0; 31 | t.transform.rotation.z = 0.0; 32 | t.transform.rotation.w = 1.0; 33 | t.header.stamp = nh.now(); 34 | broadcaster.sendTransform(t); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/Ultrasound/Ultrasound.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Ultrasound Example 3 | * 4 | * This example is for the Maxbotix Ultrasound rangers. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | sensor_msgs::Range range_msg; 14 | ros::Publisher pub_range( "/ultrasound", &range_msg); 15 | 16 | const int adc_pin = 0; 17 | 18 | char frameid[] = "/ultrasound"; 19 | 20 | float getRange_Ultrasound(int pin_num){ 21 | int val = 0; 22 | for(int i=0; i<4; i++) val += analogRead(pin_num); 23 | float range = val; 24 | return range /322.519685; // (0.0124023437 /4) ; //cvt to meters 25 | } 26 | 27 | void setup() 28 | { 29 | nh.initNode(); 30 | nh.advertise(pub_range); 31 | 32 | 33 | range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; 34 | range_msg.header.frame_id = frameid; 35 | range_msg.field_of_view = 0.1; // fake 36 | range_msg.min_range = 0.0; 37 | range_msg.max_range = 6.47; 38 | 39 | pinMode(8,OUTPUT); 40 | digitalWrite(8, LOW); 41 | } 42 | 43 | 44 | long range_time; 45 | 46 | void loop() 47 | { 48 | 49 | //publish the adc value every 50 milliseconds 50 | //since it takes that long for the sensor to stablize 51 | if ( millis() >= range_time ){ 52 | int r =0; 53 | 54 | range_msg.range = getRange_Ultrasound(5); 55 | range_msg.header.stamp = nh.now(); 56 | pub_range.publish(&range_msg); 57 | range_time = millis() + 50; 58 | } 59 | 60 | nh.spinOnce(); 61 | } 62 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/button_example/button_example.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * Button Example for Rosserial 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | 9 | ros::NodeHandle nh; 10 | 11 | std_msgs::Bool pushed_msg; 12 | ros::Publisher pub_button("pushed", &pushed_msg); 13 | 14 | const int button_pin = 7; 15 | const int led_pin = 13; 16 | 17 | bool last_reading; 18 | long last_debounce_time=0; 19 | long debounce_delay=50; 20 | bool published = true; 21 | 22 | void setup() 23 | { 24 | nh.initNode(); 25 | nh.advertise(pub_button); 26 | 27 | //initialize an LED output pin 28 | //and a input pin for our push button 29 | pinMode(led_pin, OUTPUT); 30 | pinMode(button_pin, INPUT); 31 | 32 | //Enable the pullup resistor on the button 33 | digitalWrite(button_pin, HIGH); 34 | 35 | //The button is a normally button 36 | last_reading = ! digitalRead(button_pin); 37 | 38 | } 39 | 40 | void loop() 41 | { 42 | 43 | bool reading = ! digitalRead(button_pin); 44 | 45 | if (last_reading!= reading){ 46 | last_debounce_time = millis(); 47 | published = false; 48 | } 49 | 50 | //if the button value has not changed for the debounce delay, we know its stable 51 | if ( !published && (millis() - last_debounce_time) > debounce_delay) { 52 | digitalWrite(led_pin, reading); 53 | pushed_msg.data = reading; 54 | pub_button.publish(&pushed_msg); 55 | published = true; 56 | } 57 | 58 | last_reading = reading; 59 | 60 | nh.spinOnce(); 61 | } 62 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/examples/pubsub/pubsub.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | 13 | void messageCb( const std_msgs::Empty& toggle_msg){ 14 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 15 | } 16 | 17 | ros::Subscriber sub("toggle_led", messageCb ); 18 | 19 | 20 | 21 | std_msgs::String str_msg; 22 | ros::Publisher chatter("chatter", &str_msg); 23 | 24 | char hello[13] = "hello world!"; 25 | 26 | void setup() 27 | { 28 | pinMode(13, OUTPUT); 29 | nh.initNode(); 30 | nh.advertise(chatter); 31 | nh.subscribe(sub); 32 | } 33 | 34 | void loop() 35 | { 36 | str_msg.data = hello; 37 | chatter.publish( &str_msg ); 38 | nh.spinOnce(); 39 | delay(500); 40 | } 41 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/tests/array_test/array_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::geometry_msgs::PoseArray Test 3 | * Sums an array, publishes sum 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | bool set_; 15 | 16 | 17 | geometry_msgs::Pose sum_msg; 18 | ros::Publisher p("sum", &sum_msg); 19 | 20 | void messageCb(const geometry_msgs::PoseArray& msg){ 21 | sum_msg.position.x = 0; 22 | sum_msg.position.y = 0; 23 | sum_msg.position.z = 0; 24 | for(int i = 0; i < msg.poses_length; i++) 25 | { 26 | sum_msg.position.x += msg.poses[i].position.x; 27 | sum_msg.position.y += msg.poses[i].position.y; 28 | sum_msg.position.z += msg.poses[i].position.z; 29 | } 30 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 31 | } 32 | 33 | ros::Subscriber s("poses",messageCb); 34 | 35 | void setup() 36 | { 37 | pinMode(13, OUTPUT); 38 | nh.initNode(); 39 | nh.subscribe(s); 40 | nh.advertise(p); 41 | } 42 | 43 | void loop() 44 | { 45 | p.publish(&sum_msg); 46 | nh.spinOnce(); 47 | delay(10); 48 | } 49 | 50 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/tests/float64_test/float64_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Float64 Test 3 | * Receives a Float64 input, subtracts 1.0, and publishes it 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | 10 | ros::NodeHandle nh; 11 | 12 | float x; 13 | 14 | void messageCb( const std_msgs::Float64& msg){ 15 | x = msg.data - 1.0; 16 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 17 | } 18 | 19 | std_msgs::Float64 test; 20 | ros::Subscriber s("your_topic", &messageCb); 21 | ros::Publisher p("my_topic", &test); 22 | 23 | void setup() 24 | { 25 | pinMode(13, OUTPUT); 26 | nh.initNode(); 27 | nh.advertise(p); 28 | nh.subscribe(s); 29 | } 30 | 31 | void loop() 32 | { 33 | test.data = x; 34 | p.publish( &test ); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | 39 | -------------------------------------------------------------------------------- /rosserial_arduino/src/ros_lib/tests/time_test/time_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Time Test 3 | * Publishes current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | std_msgs::Time test; 14 | ros::Publisher p("my_topic", &test); 15 | 16 | void setup() 17 | { 18 | pinMode(13, OUTPUT); 19 | nh.initNode(); 20 | nh.advertise(p); 21 | } 22 | 23 | void loop() 24 | { 25 | test.data = nh.now(); 26 | p.publish( &test ); 27 | nh.spinOnce(); 28 | delay(10); 29 | } 30 | 31 | -------------------------------------------------------------------------------- /rosserial_arduino/src/rosserial_arduino/__init__.py: -------------------------------------------------------------------------------- 1 | from .SerialClient import * 2 | -------------------------------------------------------------------------------- /rosserial_arduino/srv/Test.srv: -------------------------------------------------------------------------------- 1 | string input 2 | --- 3 | string output 4 | -------------------------------------------------------------------------------- /rosserial_chibios/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_chibios 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Added support for ChibiOS clients (`#493 `_) 14 | * Contributors: Hermann von Kleist 15 | -------------------------------------------------------------------------------- /rosserial_chibios/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_chibios) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install( 9 | DIRECTORY src/ros_lib 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 11 | ) 12 | 13 | catkin_install_python( 14 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 16 | ) 17 | -------------------------------------------------------------------------------- /rosserial_chibios/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_chibios 3 | 0.9.2 4 | 5 | rosserial for ChibiOS/HAL platforms. 6 | 7 | Hermann von Kleist 8 | Hermann von Kleist 9 | BSD 10 | http://ros.org/wiki/rosserial_chibios 11 | 12 | catkin 13 | 14 | rosserial_client 15 | 16 | -------------------------------------------------------------------------------- /rosserial_chibios/src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_H_ 36 | #define _ROS_H_ 37 | 38 | #include "ros/node_handle.h" 39 | 40 | #include "ChibiOSHardware.h" 41 | 42 | namespace ros 43 | { 44 | typedef NodeHandle_ NodeHandle; // default 25, 25, 512, 512 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /rosserial_chibios/src/ros_lib/ros_lib.mk: -------------------------------------------------------------------------------- 1 | # Simply include this file in your project ChibiOS Makefile. 2 | ROSLIBDIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) 3 | 4 | ROSLIBSRC = $(ROSLIBDIR)/duration.cpp \ 5 | $(ROSLIBDIR)/time.cpp 6 | ROSLIBINC = $(ROSLIBDIR) 7 | 8 | ALLCPPSRC += $(ROSLIBSRC) 9 | ALLINC += $(ROSLIBINC) 10 | -------------------------------------------------------------------------------- /rosserial_client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_client) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package( 6 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake 7 | ) 8 | 9 | catkin_python_setup() 10 | 11 | if(CATKIN_ENABLE_TESTING) 12 | find_package(rosserial_msgs REQUIRED) 13 | include_directories(src/ros_lib ${rosserial_msgs_INCLUDE_DIRS}) 14 | catkin_add_gtest(float64_test test/float64_test.cpp) 15 | catkin_add_gtest(subscriber_test test/subscriber_test.cpp) 16 | catkin_add_gtest(time_test test/time_test.cpp src/ros_lib/duration.cpp src/ros_lib/time.cpp) 17 | endif() 18 | 19 | install( 20 | DIRECTORY src/ros_lib 21 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 22 | ) 23 | 24 | catkin_install_python( 25 | PROGRAMS scripts/make_libraries src/${PROJECT_NAME}/make_library.py 26 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 27 | ) 28 | -------------------------------------------------------------------------------- /rosserial_client/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_client 3 | 0.9.2 4 | 5 | Generalized client side source for rosserial. 6 | 7 | Michael Ferguson 8 | Adam Stambler 9 | Paul Bouchier 10 | Mike Purvis 11 | BSD 12 | http://ros.org/wiki/rosserial_client 13 | 14 | catkin 15 | rosbash 16 | 17 | rosserial_msgs 18 | std_msgs 19 | rospy 20 | tf 21 | 22 | rosunit 23 | rosserial_msgs 24 | 25 | -------------------------------------------------------------------------------- /rosserial_client/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosserial_client'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rosserial_client/src/ros_lib/tf/tf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TF_H_ 36 | #define ROS_TF_H_ 37 | 38 | #include "geometry_msgs/TransformStamped.h" 39 | 40 | namespace tf 41 | { 42 | 43 | static inline geometry_msgs::Quaternion createQuaternionFromYaw(double yaw) 44 | { 45 | geometry_msgs::Quaternion q; 46 | q.x = 0; 47 | q.y = 0; 48 | q.z = sin(yaw * 0.5); 49 | q.w = cos(yaw * 0.5); 50 | return q; 51 | } 52 | 53 | } 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /rosserial_client/src/ros_lib/tf/transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TRANSFORM_BROADCASTER_H_ 36 | #define ROS_TRANSFORM_BROADCASTER_H_ 37 | 38 | #include "ros.h" 39 | #include "tfMessage.h" 40 | 41 | namespace tf 42 | { 43 | 44 | class TransformBroadcaster 45 | { 46 | public: 47 | TransformBroadcaster() : publisher_("/tf", &internal_msg) {} 48 | 49 | void init(ros::NodeHandle &nh) 50 | { 51 | nh.advertise(publisher_); 52 | } 53 | 54 | void sendTransform(geometry_msgs::TransformStamped &transform) 55 | { 56 | internal_msg.transforms_length = 1; 57 | internal_msg.transforms = &transform; 58 | publisher_.publish(&internal_msg); 59 | } 60 | 61 | private: 62 | tf::tfMessage internal_msg; 63 | ros::Publisher publisher_; 64 | }; 65 | 66 | } 67 | 68 | #endif 69 | 70 | -------------------------------------------------------------------------------- /rosserial_client/src/rosserial_client/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_client/src/rosserial_client/__init__.py -------------------------------------------------------------------------------- /rosserial_client/test/float64_test.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/msg.h" 2 | #include 3 | #include 4 | 5 | class TestFloat64 : public ::testing::Test 6 | { 7 | public: 8 | union 9 | { 10 | double val; 11 | unsigned char buffer[8]; 12 | }; 13 | 14 | static const double cases[]; 15 | static const int num_cases; 16 | }; 17 | 18 | const double TestFloat64::cases[] = 19 | { 20 | 0.0, 10.0, 15642.1, -50.2, 0.0001, -0.321, 21 | 123456.789, -987.654321, 3.4e38, -3.4e38, 22 | 0.0, -0.0, 0.1, -0.1, 23 | M_PI, -M_PI, 123456.789, -123456.789, 24 | INFINITY, -INFINITY, NAN, INFINITY - INFINITY, 25 | 1e38, -1e38, 1e39, -1e39, 26 | 1e-38, -1e-38, 1e-39, -1e-39, 27 | 3.14159e-37,-3.14159e-37, 3.14159e-43, -3.14159e-43, 28 | 1e-60, -1e-60, 1e-45, -1e-45, 29 | 0.99999999999999, -0.99999999999999, 127.999999999999, -127.999999999999 30 | }; 31 | const int TestFloat64::num_cases = sizeof(TestFloat64::cases) / sizeof(double); 32 | 33 | 34 | TEST_F(TestFloat64, testRoundTrip) 35 | { 36 | for (int i = 0; i < num_cases; i++) 37 | { 38 | memset(buffer, 0, sizeof(buffer)); 39 | ros::Msg::serializeAvrFloat64(buffer, cases[i]); 40 | float deserialized = 0; 41 | ros::Msg::deserializeAvrFloat64(buffer, &deserialized); 42 | 43 | if (isnan(cases[i])) 44 | { 45 | // EXPECT_FLOAT_EQ will fail on nan, because nan != nan 46 | EXPECT_EQ(isnan(val), true); 47 | EXPECT_EQ(isnan(deserialized), true); 48 | } 49 | else 50 | { 51 | // Compare against C++ cast results. 52 | // In some of the test cases, truncation and loss of precision is expected 53 | // but it should be the same as what C++ compiler implements for (float). 54 | EXPECT_FLOAT_EQ(static_cast(cases[i]), static_cast(val)); 55 | EXPECT_FLOAT_EQ(static_cast(cases[i]), static_cast(deserialized)); 56 | } 57 | } 58 | } 59 | 60 | 61 | int main(int argc, char **argv) 62 | { 63 | testing::InitGoogleTest(&argc, argv); 64 | return RUN_ALL_TESTS(); 65 | } 66 | -------------------------------------------------------------------------------- /rosserial_client/test/subscriber_test.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/subscriber.h" 2 | #include 3 | 4 | 5 | bool callbackCalled; 6 | unsigned char buffer[1]; 7 | 8 | class DummyMsg 9 | { 10 | public: 11 | int serialize(unsigned char *outbuffer) const 12 | { 13 | return 0; 14 | } 15 | int deserialize(unsigned char *inbuffer) 16 | { 17 | return 0; 18 | } 19 | const char * getType() 20 | { 21 | return ""; 22 | } 23 | const char * getMD5() 24 | { 25 | return ""; 26 | } 27 | }; 28 | 29 | class DummyClass 30 | { 31 | public: 32 | static void staticCallback(const DummyMsg& msg) 33 | { 34 | callbackCalled = true; 35 | } 36 | void memberCallback(const DummyMsg& msg) 37 | { 38 | callbackCalled = true; 39 | } 40 | }; 41 | 42 | 43 | TEST(TestSubscriber, testStaticCallback) 44 | { 45 | ros::Subscriber sub("topic_name", &DummyClass::staticCallback); 46 | 47 | callbackCalled = false; 48 | sub.callback(buffer); 49 | ASSERT_TRUE(callbackCalled); 50 | } 51 | 52 | TEST(TestSubscriber, testMemberCallback) 53 | { 54 | DummyClass cl; 55 | ros::Subscriber sub("topic_name", &DummyClass::memberCallback, &cl); 56 | 57 | callbackCalled = false; 58 | sub.callback(buffer); 59 | ASSERT_TRUE(callbackCalled); 60 | } 61 | 62 | 63 | int main(int argc, char **argv) 64 | { 65 | testing::InitGoogleTest(&argc, argv); 66 | return RUN_ALL_TESTS(); 67 | } 68 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_embeddedlinux) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rosserial_client 6 | ) 7 | 8 | catkin_package(CATKIN_DEPENDS) 9 | 10 | install( 11 | DIRECTORY src/ros_lib 12 | src/examples 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 14 | ) 15 | 16 | catkin_install_python( 17 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py 18 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 19 | ) 20 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_embeddedlinux 3 | 0.9.2 4 | 5 | rosserial for embedded Linux enviroments 6 | 7 | Paul Bouchier 8 | Paul Bouchier 9 | Mike Purvis 10 | BSD 11 | http://ros.org/wiki/rosserial_embeddedlinux 12 | 13 | catkin 14 | 15 | rosserial_client 16 | 17 | rospy 18 | rosserial_msgs 19 | 20 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/ExampleService/ExampleService.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial_embeddedlinux service server example 3 | * 4 | * Advertises a service it offers. Prints the string sent to the service 5 | * and responds with an alternating string. 6 | * The service request can be sent from the ROS command line with e.g. 7 | * $ xxx 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | ros::NodeHandle nh; 16 | using rosserial_examples::Test; 17 | #define ROSSRVR_IP "192.168.15.122" 18 | 19 | int i=0; 20 | void svcCallback(const Test::Request & req, Test::Response & res){ 21 | if((i++)%2) 22 | res.output = "hello"; 23 | else 24 | res.output = "ros"; 25 | printf("Service request message: \"%s\" received, responding with: %s", res.output); 26 | } 27 | ros::ServiceServer server("test_srv",&svcCallback); 28 | 29 | int main() 30 | { 31 | nh.initNode(ROSSRVR_IP); 32 | nh.advertiseService(server); 33 | 34 | while(1) { 35 | nh.spinOnce(); 36 | sleep(1); 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/ExampleServiceClient/ExampleServiceClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial_embeddedlinux service client example 3 | * 4 | * Calls a service offered by a ROS service server and publishes what it receives from 5 | * the service server to the chatter topic. It also prints the received service response. 6 | * 7 | * You can run a suitable service on ROS with: 8 | * $ rosrun rosserial_embeddedlinux client.py 9 | * 10 | * When you run this program on the embedded linux system, client.py on the ROS workstation 11 | * will ask you to respond with a string. The string you give it will be passed back to this 12 | * service client, which will print and publish it. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | ros::NodeHandle nh; 21 | using rosserial_embeddedlinux::Test; 22 | #define ROSSRVR_IP "192.168.15.149" 23 | 24 | ros::ServiceClient client("test_srv"); 25 | 26 | std_msgs::String str_msg; 27 | ros::Publisher chatter("chatter", &str_msg); 28 | 29 | char hello[13] = "hello world!"; 30 | 31 | int main() 32 | { 33 | nh.initNode(ROSSRVR_IP); 34 | nh.serviceClient(client); 35 | nh.advertise(chatter); 36 | while(!nh.connected()) nh.spinOnce(); 37 | printf("Startup complete\n"); 38 | while (1) { 39 | Test::Request req; 40 | Test::Response res; 41 | req.input = hello; 42 | client.call(req, res); 43 | printf("Service responded with \"%s\"\n", res.output); 44 | str_msg.data = res.output; 45 | chatter.publish( &str_msg ); 46 | nh.spinOnce(); 47 | sleep(1); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/ExampleServiceClient/exampleService.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Sample code to use with ServiceClient.pde 5 | """ 6 | 7 | import roslib; roslib.load_manifest("rosserial_arduino") 8 | import rospy 9 | 10 | from rosserial_arduino.srv import * 11 | 12 | def callback(req): 13 | print("The arduino is calling! Please send it a message:") 14 | t = TestResponse() 15 | t.output = raw_input() 16 | return t 17 | 18 | rospy.init_node("service_client_test") 19 | rospy.Service("test_srv", Test, callback) 20 | rospy.spin() 21 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/ExampleSubscriber/ExampleSubscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial_embeddedlinux subscriber example 3 | * 4 | * Prints a string sent on a subscribed ros topic. 5 | * The string can be sent with e.g. 6 | * $ rostopic pub chatter std_msgs/String -- "Hello Embedded Linux" 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | ros::NodeHandle nh; 14 | char *rosSrvrIp = "192.168.15.149"; 15 | 16 | void messageCb(const std_msgs::String& received_msg){ 17 | printf("Received subscribed chatter message: %s\n", received_msg.data); 18 | } 19 | ros::Subscriber sub("chatter", messageCb ); 20 | 21 | int main() 22 | { 23 | //nh.initNode(); 24 | nh.initNode(rosSrvrIp); 25 | nh.subscribe(sub); 26 | 27 | while(1) { 28 | sleep(1); 29 | nh.spinOnce(); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/HelloRos/HelloROS.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello ROS!" 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | std_msgs::String str_msg; 13 | ros::Publisher chatter("chatter", &str_msg); 14 | 15 | char *rosSrvrIp = "192.168.15.121"; 16 | char hello[13] = "Hello ROS!"; 17 | 18 | int main() 19 | { 20 | //nh.initNode(); 21 | nh.initNode(rosSrvrIp); 22 | nh.advertise(chatter); 23 | while(1) { 24 | str_msg.data = hello; 25 | chatter.publish( &str_msg ); 26 | nh.spinOnce(); 27 | printf("chattered\n"); 28 | sleep(1); 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/VEXProMotor13Subscribe/VEXProMotor13Subscribe.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VEXProMotor13Subscribe.cpp 3 | * Control motor 13 speed by publishing the desired speed on a ros topic with e.g. 4 | * $ rostopic pub my_topic std_msgs/Int32 120 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include "qemotoruser.h" 11 | 12 | /* 13 | * Control motor 13 speed by publishing the desired speed on a ros topic with e.g. 14 | * $ rostopic pub my_topic std_msgs/Int32 120 15 | * The range of speeds is -255 to +255 (corresponding to full reverse to full forward). 16 | * Publish negative speeds using the syntax below: 17 | * $ rostopic pub my_topic std_msgs/Int32 -- -120 18 | * (This construct tells the shell to feed everything after -- directly to rostopic.) 19 | */ 20 | 21 | ros::NodeHandle nh; 22 | CQEMotorUser &motor = CQEMotorUser::GetRef(); 23 | char *rosSrvrIp = "192.168.11.9"; 24 | 25 | void messageCb(const std_msgs::Int32& motor13_msg){ 26 | int speed = motor13_msg.data; 27 | printf("Received subscribed motor speed %d\n", speed); 28 | motor.SetPWM(0, speed); 29 | } 30 | ros::Subscriber sub("motor13", messageCb ); 31 | 32 | 33 | int main() 34 | { 35 | //nh.initNode(); 36 | nh.initNode(rosSrvrIp); 37 | nh.subscribe(sub); 38 | 39 | while(1) { 40 | sleep(1); 41 | nh.spinOnce(); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/VEXProRangePublish/VEXProRangePublish.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VEXProRangePublish.cpp 3 | * 4 | * Created on: Jul 12, 2012 5 | * Author: bouchier 6 | * 7 | * Publish the range from an ultrasonic ranging sensor connected to digital 1 (Input) & 8 | * digital 2 (Output) on topic sonar1 9 | */ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "qegpioint.h" 16 | 17 | ros::NodeHandle nh; 18 | std_msgs::Int32 range; 19 | ros::Publisher sonar1("sonar1", &range); 20 | 21 | char *rosSrvrIp = "192.168.11.9"; 22 | 23 | #define USPI 150 24 | #define BIAS 300 25 | 26 | unsigned long diff(struct timeval *ptv0, struct timeval *ptv1) 27 | { 28 | long val; 29 | 30 | val = ptv1->tv_usec - ptv0->tv_usec; 31 | val += (ptv1->tv_sec - ptv0->tv_sec)*1000000; 32 | 33 | return val; 34 | } 35 | 36 | void callback(unsigned int io, struct timeval *ptv, void *userdata) 37 | { 38 | static struct timeval tv0; 39 | static int flag = 0; 40 | int sonarVal; 41 | 42 | if (io==0) 43 | { 44 | flag = 1; 45 | tv0 = *ptv; 46 | } 47 | 48 | if (io==1 && flag) 49 | { 50 | sonarVal = diff(&tv0, ptv); 51 | if (sonarVal>BIAS) 52 | sonarVal = (sonarVal-BIAS)/USPI; 53 | range.data = sonarVal; 54 | printf("%d\n", sonarVal); 55 | } 56 | } 57 | 58 | // Note: connector labeled "INPUT" on sonar sensor goes to 59 | // digital 1 (bit 0), and connector labeled "OUTPUT" goes to 60 | // digital 2 (bit 1). 61 | int main() 62 | { 63 | CQEGpioInt &gpio = CQEGpioInt::GetRef(); 64 | volatile unsigned int d; 65 | 66 | // reset bit 0, set as output for sonar trigger 67 | gpio.SetData(0x0000); 68 | gpio.SetDataDirection(0x0001); 69 | 70 | // set callbacks on negative edge for both bits 0 (trigger) 71 | // and 1 (echo) 72 | gpio.RegisterCallback(0, NULL, callback); 73 | gpio.RegisterCallback(1, NULL, callback); 74 | gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE); 75 | gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE); 76 | 77 | //nh.initNode(); 78 | nh.initNode(rosSrvrIp); 79 | nh.advertise(sonar1); 80 | 81 | // trigger sonar by toggling bit 0 82 | while(1) 83 | { 84 | gpio.SetData(0x0001); 85 | for (d=0; d<120000; d++); 86 | gpio.SetData(0x0000); 87 | sleep(1); // the interrupt breaks us out of this sleep 88 | sleep(1); // now really sleep a second 89 | sonar1.publish( &range ); 90 | nh.spinOnce(); 91 | } 92 | } 93 | 94 | 95 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/examples/VEXProServoSubscribe/VEXProServoSubscribe.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VEXProServoSubscribe.cpp 3 | * 4 | * Created on: Jul 12, 2012 5 | * Author: bouchier 6 | * Drives a servo or motor on VEXPro motor1 connection to the requested value: 0 - 255 7 | * that is received on subscribed topic servo1 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | using namespace std; 15 | 16 | ros::NodeHandle nh; 17 | CQEServo &servo = CQEServo::GetRef(); 18 | char *rosSrvrIp = "192.168.15.149"; 19 | 20 | void messageCb(const std_msgs::Int32& servo1_msg){ 21 | int position = servo1_msg.data; 22 | printf("Received subscribed servo position %d\n", position); 23 | servo.SetCommand(0, position); 24 | } 25 | ros::Subscriber sub("servo1", messageCb ); 26 | 27 | int main() { 28 | //nh.initNode(); 29 | nh.initNode(rosSrvrIp); 30 | nh.subscribe(sub); 31 | 32 | while(1) { 33 | sleep(1); 34 | nh.spinOnce(); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /rosserial_embeddedlinux/src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_H_ 36 | #define _ROS_H_ 37 | 38 | #ifndef BUILD_LIBROSSERIALEMBEDDEDLINUX 39 | #include "embedded_linux_comms.c" 40 | #include "duration.cpp" 41 | #include "time.cpp" 42 | #endif 43 | 44 | #include "ros/node_handle.h" 45 | #include "embedded_linux_hardware.h" 46 | 47 | namespace ros 48 | { 49 | typedef NodeHandle_ NodeHandle; 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /rosserial_mbed/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_mbed 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Use os.path.join for path concatenation (`#495 `_) 14 | * Bump minimum CMake version to 3.7.2 (Melodic). 15 | * Fix py3 print usages and trailing whitespaces (`#469 `_) 16 | * Contributors: Hermann von Kleist, Mike Purvis, acxz 17 | 18 | 0.8.0 (2018-10-11) 19 | ------------------ 20 | * Use the ! prefix introduced in gcc4mbed for mbed examples (`#304 `_) 21 | Follows the changes introduced in the following commit: 22 | https://github.com/adamgreen/gcc4mbed/commit/7d79ef307e65f4f913bed655c887a632352c286c 23 | Refer to `adamgreen/gcc4mbed#64 `_ for more details. 24 | * Contributors: Naoki Mizuno 25 | 26 | 0.7.7 (2017-11-29) 27 | ------------------ 28 | * Fix catkin lint errors (`#296 `_) 29 | * Contributors: Bei Chen Liu 30 | 31 | 0.7.6 (2017-03-01) 32 | ------------------ 33 | 34 | 0.7.5 (2016-11-22) 35 | ------------------ 36 | 37 | 0.7.4 (2016-09-21) 38 | ------------------ 39 | 40 | 0.7.3 (2016-08-05) 41 | ------------------ 42 | 43 | 0.7.2 (2016-07-15) 44 | ------------------ 45 | * Add initial rosserial_mbed package. 46 | * Contributors: Gary Servin, Romain Reignier 47 | -------------------------------------------------------------------------------- /rosserial_mbed/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_mbed) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | ) 7 | 8 | add_message_files(FILES 9 | Adc.msg 10 | ) 11 | 12 | add_service_files(FILES 13 | Test.srv 14 | ) 15 | 16 | generate_messages() 17 | 18 | catkin_package(CATKIN_DEPENDS 19 | message_runtime 20 | ) 21 | 22 | install( 23 | DIRECTORY src/ros_lib 24 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 25 | ) 26 | 27 | catkin_install_python( 28 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py 29 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 30 | ) 31 | -------------------------------------------------------------------------------- /rosserial_mbed/README.md: -------------------------------------------------------------------------------- 1 | ## rosserial_mbed 2 | 3 | This is a rosserial client implementation for the mbed platform. 4 | 5 | Note: This has been tested and currently supports building using the [gcc4mbed](https://github.com/adamgreen/gcc4mbed) ofline compiler for the LPC1768, KL25Z and NUCLEO F401RE. 6 | 7 | ### Issues 8 | 9 | * No support for the mbed online compiler (WIP) 10 | 11 | ### Usage/Workflow 12 | 13 | * Make sure you have the gcc4mbed installed on your system 14 | * Install the library somewhere in your system 15 | * `rosrun rosserial_mbed make_libraries.py ` 16 | * Move to the project's folder and set the env var for the gcc4mbed and the ros-lib paths 17 | * Example: `$ export GCC4MBED_DIR=/home/gary/devel/mbed/gcc4mbed` 18 | * Example: `$ export ROS_LIB_DIR=/home/gary/devel/ros-lib` 19 | * `make all && make deploy` 20 | 21 | Please see the [rosserial Tutorials on the ROS wiki](http://wiki.ros.org/rosserial_mbed/Tutorials) to get started. 22 | -------------------------------------------------------------------------------- /rosserial_mbed/msg/Adc.msg: -------------------------------------------------------------------------------- 1 | uint16 adc0 2 | uint16 adc1 3 | uint16 adc2 4 | uint16 adc3 5 | uint16 adc4 6 | uint16 adc5 7 | -------------------------------------------------------------------------------- /rosserial_mbed/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_mbed 3 | 0.9.2 4 | 5 | rosserial for mbed platforms. 6 | 7 | Gary Servin 8 | Gary Servin 9 | BSD 10 | http://ros.org/wiki/rosserial_mbed 11 | 12 | catkin 13 | 14 | message_generation 15 | 16 | rospy 17 | rosserial_msgs 18 | rosserial_client 19 | message_runtime 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ADC/ADC.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial ADC Example 3 | * 4 | * This is a poor man's Oscilloscope. It does not have the sampling 5 | * rate or accuracy of a commerical scope, but it is great to get 6 | * an analog value into ROS in a pinch. 7 | */ 8 | 9 | #include "mbed.h" 10 | #include 11 | #include 12 | 13 | #if defined(TARGET_LPC1768) 14 | PinName adc0 = p15; 15 | PinName adc1 = p16; 16 | PinName adc2 = p17; 17 | PinName adc3 = p18; 18 | PinName adc4 = p19; 19 | PinName adc5 = p20; 20 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 21 | PinName adc0 = A0; 22 | PinName adc1 = A1; 23 | PinName adc2 = A2; 24 | PinName adc3 = A3; 25 | PinName adc4 = A4; 26 | PinName adc5 = A5; 27 | #else 28 | #error "You need to specify the pins for the adcs" 29 | #endif 30 | 31 | ros::NodeHandle nh; 32 | 33 | rosserial_mbed::Adc adc_msg; 34 | ros::Publisher p("adc", &adc_msg); 35 | 36 | 37 | //We average the analog reading to elminate some of the noise 38 | int averageAnalog(PinName pin) { 39 | int v=0; 40 | for (int i=0; i<4; i++) v+= AnalogIn(pin).read_u16(); 41 | return v/4; 42 | } 43 | 44 | long adc_timer; 45 | 46 | int main() { 47 | nh.initNode(); 48 | 49 | nh.advertise(p); 50 | 51 | while (1) { 52 | adc_msg.adc0 = averageAnalog(adc0); 53 | adc_msg.adc1 = averageAnalog(adc1); 54 | adc_msg.adc2 = averageAnalog(adc2); 55 | adc_msg.adc3 = averageAnalog(adc3); 56 | adc_msg.adc4 = averageAnalog(adc4); 57 | adc_msg.adc5 = averageAnalog(adc5); 58 | 59 | p.publish(&adc_msg); 60 | 61 | nh.spinOnce(); 62 | } 63 | } 64 | 65 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ADC/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_ADC 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Blink/Blink.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Subscriber Example 3 | * Blinks an LED on callback 4 | */ 5 | #include "mbed.h" 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | DigitalOut myled(LED1); 11 | 12 | void messageCb(const std_msgs::Empty& toggle_msg){ 13 | myled = !myled; // blink the led 14 | } 15 | 16 | ros::Subscriber sub("toggle_led", &messageCb); 17 | 18 | int main() { 19 | nh.initNode(); 20 | nh.subscribe(sub); 21 | 22 | while (1) { 23 | nh.spinOnce(); 24 | wait_ms(1); 25 | } 26 | } 27 | 28 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Blink/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_Blink 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Clapper/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_Clapper 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GroveBuzzer/GroveBuzzer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Buzzer Module Example 3 | * 4 | * This tutorial demonstrates the usage of the 5 | * Seeedstudio Buzzer Grove module 6 | * http://www.seeedstudio.com/depot/Grove-Buzzer-p-768.html 7 | * 8 | * Source Code Based on: 9 | * https://developer.mbed.org/teams/Seeed/code/Seeed_Grove_Buzzer/ 10 | */ 11 | 12 | #include "mbed.h" 13 | #include 14 | #include 15 | 16 | ros::NodeHandle nh; 17 | 18 | #ifdef TARGET_LPC1768 19 | PwmOut buzzer(p21); 20 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 21 | PwmOut buzzer(D2); 22 | #else 23 | #error "You need to specify a pin for the sensor" 24 | #endif 25 | 26 | Timeout toff; 27 | bool playing = false; 28 | DigitalOut led(LED1); 29 | 30 | void nobeep() 31 | { 32 | buzzer.write(0.0); 33 | led = 1; 34 | playing = false; 35 | } 36 | void beep(float freq, float time) 37 | { 38 | buzzer.period(1.0 / freq); 39 | buzzer.write(0.5); 40 | toff.attach(nobeep, time); 41 | led = 0; 42 | } 43 | 44 | void messageCb(const std_msgs::Float32MultiArray& msg) 45 | { 46 | if (!playing) 47 | { 48 | playing = true; 49 | // msg.data[0] - Note 50 | // msg.data[1] - duration in seconds 51 | beep(msg.data[0], msg.data[1]); 52 | } 53 | } 54 | 55 | ros::Subscriber sub("buzzer", &messageCb); 56 | 57 | int main() 58 | { 59 | buzzer = 0; 60 | led = 1; 61 | nh.initNode(); 62 | nh.subscribe(sub); 63 | while (1) 64 | { 65 | nh.spinOnce(); 66 | wait_ms(1); 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GroveBuzzer/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_grove_buzzer 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GroveCollision/GroveCollision.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Collission Sensor Example 3 | * 4 | * This tutorial demonstrates the usage of the 5 | * Seeedstudio Collision Grove module 6 | * http://www.seeedstudio.com/wiki/Grove_-_Collision_Sensor 7 | * 8 | * Source Code Based on: 9 | * https://developer.mbed.org/components/Grove-Collision-Sensor/ 10 | */ 11 | 12 | #include "mbed.h" 13 | #include 14 | #include 15 | 16 | ros::NodeHandle nh; 17 | 18 | std_msgs::Bool collision_msg; 19 | ros::Publisher pub_collision("collision", &collision_msg); 20 | 21 | 22 | Timer t; 23 | #ifdef TARGET_LPC1768 24 | DigitalIn sig1(p9); 25 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 26 | DigitalIn sig1(D6); 27 | #else 28 | #error "You need to specify a pin for the sensor" 29 | #endif 30 | 31 | int main() 32 | { 33 | t.start(); 34 | 35 | nh.initNode(); 36 | nh.advertise(pub_collision); 37 | 38 | long publisher_timer = 0; 39 | 40 | while (1) 41 | { 42 | 43 | if (t.read_ms() > publisher_timer) 44 | { 45 | collision_msg.data = !sig1; 46 | pub_collision.publish(&collision_msg); 47 | publisher_timer = t.read_ms() + 1000; 48 | } 49 | nh.spinOnce(); 50 | } 51 | } 52 | 53 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GroveCollision/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_grove_collision 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GrovePIRMotionSensor/GrovePIRMotionSensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PIR Motion Sensor Example 3 | * 4 | * This tutorial demonstrates the usage of the 5 | * Seeedstudio PIR Motion Sensor Grove module 6 | * http://www.seeedstudio.com/depot/Grove-PIR-Motion-Sensor-p-802.html 7 | * 8 | * Source Code Based on: 9 | * https://developer.mbed.org/teams/Seeed/code/Seeed_Grove_PIR_Motion_Sensor_Example/ 10 | */ 11 | 12 | #include "mbed.h" 13 | #include 14 | #include 15 | 16 | ros::NodeHandle nh; 17 | 18 | std_msgs::Bool motion_msg; 19 | ros::Publisher pub_motion("motion", &motion_msg); 20 | 21 | Timer t; 22 | #ifdef TARGET_LPC1768 23 | DigitalIn sig1(p9); 24 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 25 | DigitalIn sig1(D6); 26 | #else 27 | #error "You need to specify a pin for the sensor" 28 | #endif 29 | 30 | int main() 31 | { 32 | t.start(); 33 | 34 | nh.initNode(); 35 | nh.advertise(pub_motion); 36 | 37 | long publisher_timer = 0; 38 | 39 | while (1) 40 | { 41 | 42 | if (t.read_ms() > publisher_timer) 43 | { 44 | motion_msg.data = sig1; 45 | pub_motion.publish(&motion_msg); 46 | publisher_timer = t.read_ms() + 1000; 47 | } 48 | nh.spinOnce(); 49 | } 50 | } 51 | 52 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GrovePIRMotionSensor/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_pir_motion_sensor 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GroveTemperatureHumidity/GroveTemperatureHumidity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Temperature and Humidity Sensor Example 3 | * 4 | * This tutorial demonstrates the usage of the 5 | * Seeedstudio Temperature and Humidity Grove module 6 | * http://www.seeedstudio.com/wiki/Grove_-_Temperature_and_Humidity_Sensor 7 | * 8 | * Source Code Based of: 9 | * https://developer.mbed.org/teams/Seeed/code/Seeed_Grove_Temp_Humidity_Example/ 10 | */ 11 | 12 | #include "mbed.h" 13 | #include "DHT.h" 14 | #include 15 | #include 16 | #include 17 | 18 | ros::NodeHandle nh; 19 | 20 | sensor_msgs::Temperature temp_msg; 21 | sensor_msgs::RelativeHumidity humidity_msg; 22 | ros::Publisher pub_temp("temperature", &temp_msg); 23 | ros::Publisher pub_humidity("humidity", &humidity_msg); 24 | 25 | Timer t; 26 | #ifdef TARGET_LPC1768 27 | DHT sensor(p9, AM2302); 28 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 29 | DHT sensor(D6, AM2302); 30 | #elif defined(TARGET_NUCLEO_F401RE) 31 | DHT sensor(A6, AM2302); 32 | #else 33 | #error "You need to specify a pin for the sensor" 34 | #endif 35 | 36 | int main() 37 | { 38 | int error = 0; 39 | t.start(); 40 | 41 | nh.initNode(); 42 | nh.advertise(pub_temp); 43 | nh.advertise(pub_humidity); 44 | 45 | long publisher_timer = 0; 46 | temp_msg.header.frame_id = "/base_link"; 47 | humidity_msg.header.frame_id = "/base_link"; 48 | 49 | while (1) 50 | { 51 | 52 | if (t.read_ms() > publisher_timer) 53 | { 54 | error = sensor.readData(); 55 | if (0 == error) 56 | { 57 | temp_msg.temperature = sensor.ReadTemperature(CELCIUS); 58 | temp_msg.header.stamp = nh.now(); 59 | pub_temp.publish(&temp_msg); 60 | 61 | humidity_msg.relative_humidity = sensor.ReadHumidity(); 62 | humidity_msg.header.stamp = nh.now(); 63 | pub_humidity.publish(&humidity_msg); 64 | } 65 | publisher_timer = t.read_ms() + 1000; 66 | } 67 | nh.spinOnce(); 68 | } 69 | } 70 | 71 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/GroveTemperatureHumidity/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_grove_temperature_humidity 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/HelloWorld/HelloWorld.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | */ 5 | 6 | #include"mbed.h" 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | std_msgs::String str_msg; 13 | ros::Publisher chatter("chatter", &str_msg); 14 | 15 | char hello[13] = "hello world!"; 16 | 17 | DigitalOut led = LED1; 18 | 19 | int main() { 20 | nh.initNode(); 21 | nh.advertise(chatter); 22 | 23 | while (1) { 24 | led = !led; 25 | str_msg.data = hello; 26 | chatter.publish( &str_msg ); 27 | nh.spinOnce(); 28 | wait_ms(1000); 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/HelloWorld/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_HelloWorld 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/IrRanger/IrRanger.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial IR Ranger Example 3 | * 4 | * This example is calibrated for the Sharp GP2D120XJ00F. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | sensor_msgs::Range range_msg; 15 | ros::Publisher pub_range( "range_data", &range_msg); 16 | 17 | #if defined(TARGET_LPC1768) 18 | PinName analog_pin = p20; 19 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 20 | PinName analog_pin = A0; 21 | #else 22 | #error "You need to specify a pin for the mic" 23 | #endif 24 | unsigned long range_timer; 25 | 26 | /* 27 | * getRange() - samples the analog input from the ranger 28 | * and converts it into meters. 29 | */ 30 | float getRange(PinName pin_num) { 31 | int sample; 32 | // Get data 33 | sample = AnalogIn(pin_num).read_u16()/4; 34 | // if the ADC reading is too low, 35 | // then we are really far away from anything 36 | if(sample < 10) 37 | return 254; // max range 38 | // Magic numbers to get cm 39 | sample= 1309/(sample-3); 40 | return (sample - 1)/100; //convert to meters 41 | } 42 | 43 | char frameid[] = "/ir_ranger"; 44 | 45 | Timer t; 46 | int main() { 47 | t.start(); 48 | nh.initNode(); 49 | nh.advertise(pub_range); 50 | 51 | range_msg.radiation_type = sensor_msgs::Range::INFRARED; 52 | range_msg.header.frame_id = frameid; 53 | range_msg.field_of_view = 0.01; 54 | range_msg.min_range = 0.03; 55 | range_msg.max_range = 0.4; 56 | 57 | while (1) { 58 | // publish the range value every 50 milliseconds 59 | // since it takes that long for the sensor to stabilize 60 | if ( (t.read_ms()-range_timer) > 50) { 61 | range_msg.range = getRange(analog_pin); 62 | range_msg.header.stamp = nh.now(); 63 | pub_range.publish(&range_msg); 64 | range_timer = t.read_ms() + 50; 65 | } 66 | nh.spinOnce(); 67 | } 68 | } 69 | 70 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/IrRanger/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_IrRanger 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Logging/Logging.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | std_msgs::String str_msg; 13 | ros::Publisher chatter("chatter", &str_msg); 14 | 15 | char hello[13] = "hello world!"; 16 | 17 | char debug[]= "debug statements"; 18 | char info[] = "infos"; 19 | char warn[] = "warnings"; 20 | char errors[] = "errors"; 21 | char fatal[] = "fatalities"; 22 | 23 | int main() { 24 | nh.initNode(); 25 | nh.advertise(chatter); 26 | 27 | while (1) { 28 | str_msg.data = hello; 29 | chatter.publish( &str_msg ); 30 | 31 | nh.logdebug(debug); 32 | nh.loginfo(info); 33 | nh.logwarn(warn); 34 | nh.logerror(errors); 35 | nh.logfatal(fatal); 36 | 37 | nh.spinOnce(); 38 | wait_ms(500); 39 | } 40 | } 41 | 42 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Logging/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_Logging 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/MotorShield/SoftwarePWM/SoftwarePWM.cpp: -------------------------------------------------------------------------------- 1 | #include "SoftwarePWM.h" 2 | #include "mbed.h" 3 | 4 | SoftwarePWM::SoftwarePWM(PinName Pin) : SoftwarePWMPin(Pin) {} 5 | 6 | void SoftwarePWM::SetPosition(int Pos) 7 | { 8 | Position = Pos; 9 | } 10 | 11 | void SoftwarePWM::StartPulse() 12 | { 13 | if (Position <= 0) 14 | { 15 | SoftwarePWMPin = 0 ; 16 | } 17 | else 18 | { 19 | SoftwarePWMPin = 1; 20 | PulseStop.attach_us(this, &SoftwarePWM::EndPulse, Position); 21 | } 22 | } 23 | 24 | void SoftwarePWM::EndPulse() 25 | { 26 | SoftwarePWMPin = 0; 27 | } 28 | 29 | void SoftwarePWM::Enable(int StartPos, int Period) 30 | { 31 | Position = StartPos; 32 | Pulse.attach_us(this, &SoftwarePWM::StartPulse, Period); 33 | } 34 | 35 | void SoftwarePWM::Disable() 36 | { 37 | Pulse.detach(); 38 | } -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/MotorShield/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_motor_shield 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Odom/Odom.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Planar Odometry Example 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | double x = 1.0; 16 | double y = 0.0; 17 | double theta = 1.57; 18 | 19 | char base_link[] = "/base_link"; 20 | char odom[] = "/odom"; 21 | 22 | int main(void) { 23 | nh.initNode(); 24 | broadcaster.init(nh); 25 | 26 | 27 | while (1) { 28 | // drive in a circle 29 | double dx = 0.2; 30 | double dtheta = 0.18; 31 | x += cos(theta)*dx*0.1; 32 | y += sin(theta)*dx*0.1; 33 | theta += dtheta*0.1; 34 | if (theta > 3.14) 35 | theta=-3.14; 36 | 37 | // tf odom->base_link 38 | t.header.frame_id = odom; 39 | t.child_frame_id = base_link; 40 | 41 | t.transform.translation.x = x; 42 | t.transform.translation.y = y; 43 | 44 | t.transform.rotation = tf::createQuaternionFromYaw(theta); 45 | t.header.stamp = nh.now(); 46 | 47 | broadcaster.sendTransform(t); 48 | nh.spinOnce(); 49 | 50 | wait_ms(10); 51 | } 52 | } 53 | 54 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Odom/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_Odom 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServiceClient/ServiceClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Client 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_mbed::Test; 11 | 12 | ros::ServiceClient client("test_srv"); 13 | 14 | std_msgs::String str_msg; 15 | ros::Publisher chatter("chatter", &str_msg); 16 | 17 | char hello[13] = "hello world!"; 18 | 19 | int main() { 20 | nh.initNode(); 21 | nh.serviceClient(client); 22 | nh.advertise(chatter); 23 | while (!nh.connected()) nh.spinOnce(); 24 | nh.loginfo("Startup complete"); 25 | while (1) { 26 | Test::Request req; 27 | Test::Response res; 28 | req.input = hello; 29 | client.call(req, res); 30 | str_msg.data = res.output; 31 | chatter.publish( &str_msg ); 32 | nh.spinOnce(); 33 | wait_ms(100); 34 | } 35 | } 36 | 37 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServiceClient/client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Sample code to use with ServiceClient.pde 5 | """ 6 | 7 | import roslib; roslib.load_manifest("rosserial_mbed") 8 | import rospy 9 | 10 | from rosserial_mbed.srv import * 11 | 12 | def callback(req): 13 | print("The mbed is calling! Please send it a message:") 14 | t = TestResponse() 15 | t.output = raw_input() 16 | return t 17 | 18 | rospy.init_node("service_client_test") 19 | rospy.Service("test_srv", Test, callback) 20 | rospy.spin() 21 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServiceClient/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_ServiceClient 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServiceServer/ServiceServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Server 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_mbed::Test; 11 | 12 | int i; 13 | void callback(const Test::Request & req, Test::Response & res) { 14 | if ((i++)%2) 15 | res.output = "hello"; 16 | else 17 | res.output = "world"; 18 | } 19 | 20 | ros::ServiceServer server("test_srv",&callback); 21 | 22 | std_msgs::String str_msg; 23 | ros::Publisher chatter("chatter", &str_msg); 24 | 25 | char hello[13] = "hello world!"; 26 | 27 | int main(void) { 28 | nh.initNode(); 29 | nh.advertiseService(server); 30 | nh.advertise(chatter); 31 | 32 | 33 | while (1) { 34 | str_msg.data = hello; 35 | chatter.publish( &str_msg ); 36 | nh.spinOnce(); 37 | wait_ms(10); 38 | } 39 | } 40 | 41 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServiceServer/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_ServiceServer 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServoControl/ServoControl.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Servo Control Example 3 | * 4 | * This sketch demonstrates the control of hobby R/C servos 5 | * using ROS and the arduiono 6 | * 7 | * For the full tutorial write up, visit 8 | * www.ros.org/wiki/rosserial_mbed_demos 9 | * 10 | */ 11 | 12 | #include "mbed.h" 13 | #include "Servo.h" 14 | #include 15 | #include 16 | 17 | ros::NodeHandle nh; 18 | 19 | #ifdef TARGET_LPC1768 20 | Servo servo(p21); 21 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 22 | Servo servo(D8); 23 | #else 24 | #error "You need to specify a pin for the Servo" 25 | #endif 26 | DigitalOut myled(LED1); 27 | 28 | void servo_cb( const std_msgs::UInt16& cmd_msg) { 29 | servo.position(cmd_msg.data); //set servo angle, should be from 0-180 30 | myled = !myled; //toggle led 31 | } 32 | 33 | 34 | ros::Subscriber sub("servo", servo_cb); 35 | 36 | int main() { 37 | 38 | nh.initNode(); 39 | nh.subscribe(sub); 40 | 41 | while (1) { 42 | nh.spinOnce(); 43 | wait_ms(1); 44 | } 45 | } 46 | 47 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/ServoControl/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_ServoControl 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Temperature/Temperature.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Temperature Sensor Example 3 | * 4 | * This tutorial demonstrates the usage of the 5 | * Sparkfun TMP102 Digital Temperature Breakout board 6 | * http://www.sparkfun.com/products/9418 7 | * 8 | * Source Code Based off of: 9 | * http://wiring.org.co/learning/libraries/tmp102sparkfun.html 10 | */ 11 | 12 | #include "mbed.h" 13 | #include 14 | #include 15 | 16 | ros::NodeHandle nh; 17 | 18 | std_msgs::Float32 temp_msg; 19 | ros::Publisher pub_temp("temperature", &temp_msg); 20 | 21 | 22 | // From the datasheet the BMP module address LSB distinguishes 23 | // between read (1) and write (0) operations, corresponding to 24 | // address 0x91 (read) and 0x90 (write). 25 | // shift the address 1 bit right (0x91 or 0x90), the Wire library only needs the 7 26 | // most significant bits for the address 0x91 >> 1 = 0x48 27 | // 0x90 >> 1 = 0x48 (72) 28 | 29 | int sensorAddress = 0x91 >>1; // From datasheet sensor address is 0x91 30 | // shift the address 1 bit right, the Wire library only needs the 7 31 | // most significant bits for the address 32 | 33 | Timer t; 34 | #ifdef TARGET_LPC1768 35 | I2C i2c(p9, p10); // sda, scl 36 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 37 | I2C i2c(D14, D15); // sda, scl 38 | #else 39 | #error "You need to specify a pin for the sensor" 40 | #endif 41 | 42 | int main() { 43 | t.start(); 44 | 45 | nh.initNode(); 46 | nh.advertise(pub_temp); 47 | 48 | long publisher_timer =0; 49 | 50 | while (1) { 51 | 52 | if (t.read_ms() > publisher_timer) { 53 | // step 1: request reading from sensor 54 | //Wire.requestFrom(sensorAddress,2); 55 | char cmd = 2; 56 | i2c.write(sensorAddress, &cmd, 1); 57 | 58 | wait_ms(50); 59 | 60 | char msb; 61 | char lsb; 62 | int temperature; 63 | i2c.read(sensorAddress, &msb, 1); // receive high byte (full degrees) 64 | i2c.read(sensorAddress, &lsb, 1); // receive low byte (fraction degrees) 65 | 66 | temperature = ((msb) << 4); // MSB 67 | temperature |= (lsb >> 4); // LSB 68 | 69 | temp_msg.data = temperature*0.0625; 70 | pub_temp.publish(&temp_msg); 71 | 72 | publisher_timer = t.read_ms() + 1000; 73 | } 74 | 75 | nh.spinOnce(); 76 | } 77 | } 78 | 79 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Temperature/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_Temperature 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/TimeTF/TimeTF.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Time and TF Example 3 | * Publishes a transform at current time 4 | */ 5 | 6 | #include "mbed.h" 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | geometry_msgs::TransformStamped t; 14 | tf::TransformBroadcaster broadcaster; 15 | 16 | char base_link[] = "/base_link"; 17 | char odom[] = "/odom"; 18 | 19 | int main() { 20 | nh.initNode(); 21 | broadcaster.init(nh); 22 | 23 | 24 | while (1) { 25 | t.header.frame_id = odom; 26 | t.child_frame_id = base_link; 27 | t.transform.translation.x = 1.0; 28 | t.transform.rotation.x = 0.0; 29 | t.transform.rotation.y = 0.0; 30 | t.transform.rotation.z = 0.0; 31 | t.transform.rotation.w = 1.0; 32 | t.header.stamp = nh.now(); 33 | broadcaster.sendTransform(t); 34 | nh.spinOnce(); 35 | wait_ms(10); 36 | } 37 | } 38 | 39 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/TimeTF/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_TimeTF 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Ultrasound/Ultrasound.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Ultrasound Example 3 | * 4 | * This example is for the Maxbotix Ultrasound rangers. 5 | */ 6 | 7 | #include "mbed.h" 8 | #include 9 | #include 10 | #include 11 | 12 | ros::NodeHandle nh; 13 | 14 | sensor_msgs::Range range_msg; 15 | ros::Publisher pub_range( "/ultrasound", &range_msg); 16 | 17 | #if defined(TARGET_LPC1768) 18 | const PinName adc_pin = p20; 19 | #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE) 20 | const PinName adc_pin = A0; 21 | #else 22 | #error "You need to specify a pin for the sensor" 23 | #endif 24 | 25 | char frameid[] = "/ultrasound"; 26 | 27 | float getRange_Ultrasound(PinName pin_num) { 28 | int val = 0; 29 | for (int i=0; i<4; i++) val += AnalogIn(pin_num).read_u16(); 30 | float range = val; 31 | return range /322.519685; // (0.0124023437 /4) ; //cvt to meters 32 | } 33 | 34 | Timer t; 35 | int main() { 36 | t.start(); 37 | nh.initNode(); 38 | nh.advertise(pub_range); 39 | 40 | range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; 41 | range_msg.header.frame_id = frameid; 42 | range_msg.field_of_view = 0.1; // fake 43 | range_msg.min_range = 0.0; 44 | range_msg.max_range = 6.47; 45 | 46 | //pinMode(8,OUTPUT); 47 | //digitalWrite(8, LOW); 48 | 49 | long range_time=0; 50 | 51 | while (1) { 52 | 53 | //publish the adc value every 50 milliseconds 54 | //since it takes that long for the sensor to stablize 55 | if ( t.read_ms() >= range_time ) { 56 | range_msg.range = getRange_Ultrasound(adc_pin); 57 | range_msg.header.stamp = nh.now(); 58 | pub_range.publish(&range_msg); 59 | range_time = t.read_ms() + 50; 60 | } 61 | 62 | nh.spinOnce(); 63 | } 64 | } 65 | 66 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/Ultrasound/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_Ultrasound 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/button_example/button_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Button Example for Rosserial 3 | */ 4 | 5 | #include "mbed.h" 6 | #include 7 | #include 8 | 9 | #ifdef TARGET_LPC1768 10 | PinName button = p20; 11 | #elif defined(TARGET_KL25Z) 12 | PinName button = D10; 13 | #elif defined(TARGET_NUCLEO_F401RE) 14 | PinName button = USER_BUTTON; 15 | #else 16 | #error "You need to specify a pin for the button" 17 | #endif 18 | 19 | ros::NodeHandle nh; 20 | 21 | std_msgs::Bool pushed_msg; 22 | ros::Publisher pub_button("pushed", &pushed_msg); 23 | 24 | DigitalIn button_pin(button); 25 | DigitalOut led_pin(LED1); 26 | 27 | bool last_reading; 28 | long last_debounce_time=0; 29 | long debounce_delay=50; 30 | bool published = true; 31 | 32 | Timer t; 33 | int main() { 34 | t.start(); 35 | nh.initNode(); 36 | nh.advertise(pub_button); 37 | 38 | //Enable the pullup resistor on the button 39 | button_pin.mode(PullUp); 40 | 41 | //The button is a normally button 42 | last_reading = ! button_pin; 43 | 44 | while (1) { 45 | bool reading = ! button_pin; 46 | 47 | if (last_reading!= reading) { 48 | last_debounce_time = t.read_ms(); 49 | published = false; 50 | } 51 | 52 | //if the button value has not changed for the debounce delay, we know its stable 53 | if ( !published && (t.read_ms() - last_debounce_time) > debounce_delay) { 54 | led_pin = reading; 55 | pushed_msg.data = reading; 56 | pub_button.publish(&pushed_msg); 57 | published = true; 58 | } 59 | 60 | last_reading = reading; 61 | 62 | nh.spinOnce(); 63 | } 64 | } 65 | 66 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/button_example/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_button_example 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/makefile: -------------------------------------------------------------------------------- 1 | # Copyright 2013 Adam Green (http://mbed.org/users/AdamGreen/) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | # Directories to be built 16 | DIRS := ADC\ 17 | Blink\ 18 | Clapper\ 19 | HelloWorld\ 20 | IrRanger\ 21 | Logging\ 22 | Odom\ 23 | ServiceClient\ 24 | ServiceServer\ 25 | ServoControl\ 26 | Temperature\ 27 | TimeTF\ 28 | Ultrasound\ 29 | button_example\ 30 | pubsub 31 | DIRSCLEAN := $(addsuffix .clean,$(DIRS)) 32 | 33 | 34 | # Determine supported devices by looking at *-device.mk makefiles. 35 | ALL_DEVICES := $(patsubst ../build/%-device.mk,%,$(wildcard ../build/*-device.mk)) 36 | 37 | 38 | # Macro to construct device specific rules. 39 | define device_rule #,DEVICE_NAME 40 | .PHONY : $1 $(addsuffix .$1,$(DIRS)) 41 | $1 : $(addsuffix .$1,$(DIRS)) 42 | $(addsuffix .$1,$(DIRS)) : %.$1: 43 | $(Q) $(MAKE) -C $$* $1 44 | 45 | endef 46 | 47 | 48 | # Set VERBOSE make variable to 1 to output all tool commands. 49 | VERBOSE?=0 50 | ifeq "$(VERBOSE)" "0" 51 | Q=@ 52 | else 53 | Q= 54 | endif 55 | 56 | 57 | # Rules 58 | all: $(DIRS) 59 | 60 | clean: $(DIRSCLEAN) 61 | 62 | clean-all: $(DIRSCLEAN) 63 | $(Q) $(MAKE) --no-print-directory -C HelloWorld clean-all 64 | 65 | $(DIRS): 66 | @echo Building $@ 67 | $(Q) $(MAKE) --no-print-directory -C $@ all 68 | 69 | $(DIRSCLEAN): %.clean: 70 | $(Q) $(MAKE) --no-print-directory -C $* clean 71 | 72 | .PHONY: all clean clean-all $(DIRS) $(DIRSCLEAN) 73 | 74 | # Rules to build each of the supported devices. 75 | $(eval $(foreach i,$(ALL_DEVICES),$(call device_rule,$i))) 76 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/pubsub/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_pubsub 2 | DEVICES := LPC1768 KL25Z NUCLEO_F401RE 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | USER_LIBS := !$(ROS_LIB_DIR) $(ROS_LIB_DIR)/BufferedSerial 5 | NO_FLOAT_SCANF := 1 6 | NO_FLOAT_PRINTF := 1 7 | 8 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 9 | -------------------------------------------------------------------------------- /rosserial_mbed/src/examples/pubsub/pubsub.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | #include "mbed.h" 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | DigitalOut myled(LED1); 12 | 13 | void messageCb( const std_msgs::Empty& toggle_msg) { 14 | myled = !myled; // blink the led 15 | } 16 | 17 | ros::Subscriber sub("toggle_led", messageCb ); 18 | 19 | std_msgs::String str_msg; 20 | ros::Publisher chatter("chatter", &str_msg); 21 | 22 | char hello[13] = "hello world!"; 23 | 24 | int main() { 25 | nh.initNode(); 26 | nh.advertise(chatter); 27 | nh.subscribe(sub); 28 | 29 | while (1) { 30 | str_msg.data = hello; 31 | chatter.publish( &str_msg ); 32 | nh.spinOnce(); 33 | wait_ms(500); 34 | } 35 | } 36 | 37 | -------------------------------------------------------------------------------- /rosserial_mbed/src/ros_lib/BufferedSerial.lib: -------------------------------------------------------------------------------- 1 | http://mbed.org/users/sam_grove/code/BufferedSerial/#779304f9c5d2 2 | -------------------------------------------------------------------------------- /rosserial_mbed/src/ros_lib/BufferedSerial/Buffer.lib: -------------------------------------------------------------------------------- 1 | https://mbed.org/users/sam_grove/code/Buffer/#7b754354b99c 2 | -------------------------------------------------------------------------------- /rosserial_mbed/src/ros_lib/BufferedSerial/Buffer/Buffer.cpp: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * @file Buffer.cpp 4 | * @brief Software Buffer - Templated Ring Buffer for most data types 5 | * @author sam grove 6 | * @version 1.0 7 | * @see 8 | * 9 | * Copyright (c) 2013 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | */ 23 | 24 | #include "Buffer.h" 25 | 26 | template 27 | Buffer::Buffer(uint32_t size) 28 | { 29 | _buf = new T [size]; 30 | _size = size; 31 | clear(); 32 | 33 | return; 34 | } 35 | 36 | template 37 | Buffer::~Buffer() 38 | { 39 | delete [] _buf; 40 | 41 | return; 42 | } 43 | 44 | template 45 | uint32_t Buffer::getSize() 46 | { 47 | return this->_size; 48 | } 49 | 50 | template 51 | void Buffer::clear(void) 52 | { 53 | _wloc = 0; 54 | _rloc = 0; 55 | memset(_buf, 0, _size); 56 | 57 | return; 58 | } 59 | 60 | template 61 | uint32_t Buffer::peek(char c) 62 | { 63 | return 1; 64 | } 65 | 66 | // make the linker aware of some possible types 67 | template class Buffer; 68 | template class Buffer; 69 | template class Buffer; 70 | template class Buffer; 71 | template class Buffer; 72 | template class Buffer; 73 | template class Buffer; 74 | template class Buffer; 75 | template class Buffer; 76 | template class Buffer; 77 | -------------------------------------------------------------------------------- /rosserial_mbed/src/ros_lib/MbedHardware.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MbedHardware 3 | * 4 | * Created on: Aug 17, 2011 5 | * Author: nucho 6 | */ 7 | 8 | #ifndef ROS_MBED_HARDWARE_H_ 9 | #define ROS_MBED_HARDWARE_H_ 10 | 11 | #include "mbed.h" 12 | 13 | #include "BufferedSerial.h" 14 | 15 | class MbedHardware { 16 | public: 17 | MbedHardware(PinName tx, PinName rx, long baud = 57600) 18 | :iostream(tx, rx){ 19 | baud_ = baud; 20 | t.start(); 21 | } 22 | 23 | MbedHardware() 24 | :iostream(USBTX, USBRX) { 25 | baud_ = 57600; 26 | t.start(); 27 | } 28 | 29 | void setBaud(long baud){ 30 | this->baud_= baud; 31 | } 32 | 33 | int getBaud(){return baud_;} 34 | 35 | void init(){ 36 | iostream.baud(baud_); 37 | } 38 | 39 | int read(){ 40 | if (iostream.readable()) { 41 | return iostream.getc(); 42 | } else { 43 | return -1; 44 | } 45 | }; 46 | void write(uint8_t* data, int length) { 47 | for (int i=0; i NodeHandle; 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /rosserial_mbed/src/tests/array_test/array_test.cpp: -------------------------------------------------------------------------------- 1 | //#define COMPILE_ARRAY_CODE_RSOSSERIAL 2 | #ifdef COMPILE_ARRAY_CODE_RSOSSERIAL 3 | 4 | /* 5 | * rosserial::geometry_msgs::PoseArray Test 6 | * Sums an array, publishes sum 7 | */ 8 | #include "mbed.h" 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | ros::NodeHandle nh; 15 | 16 | bool set_; 17 | DigitalOut myled(LED1); 18 | 19 | geometry_msgs::Pose sum_msg; 20 | ros::Publisher p("sum", &sum_msg); 21 | 22 | void messageCb(const geometry_msgs::PoseArray& msg) { 23 | sum_msg.position.x = 0; 24 | sum_msg.position.y = 0; 25 | sum_msg.position.z = 0; 26 | for (int i = 0; i < msg.poses_length; i++) { 27 | sum_msg.position.x += msg.poses[i].position.x; 28 | sum_msg.position.y += msg.poses[i].position.y; 29 | sum_msg.position.z += msg.poses[i].position.z; 30 | } 31 | myled = !myled; // blink the led 32 | } 33 | 34 | ros::Subscriber s("poses",messageCb); 35 | 36 | int main() { 37 | nh.initNode(); 38 | nh.subscribe(s); 39 | nh.advertise(p); 40 | 41 | while (1) { 42 | p.publish(&sum_msg); 43 | nh.spinOnce(); 44 | wait_ms(10); 45 | } 46 | } 47 | #endif -------------------------------------------------------------------------------- /rosserial_mbed/src/tests/array_test/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_array_test 2 | DEVICES := LPC1768 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | NO_FLOAT_SCANF := 1 5 | NO_FLOAT_PRINTF := 1 6 | 7 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 8 | -------------------------------------------------------------------------------- /rosserial_mbed/src/tests/float64_test/float64_test.cpp: -------------------------------------------------------------------------------- 1 | //#define COMPLIE_FLOAT64_CODE_ROSSERIAL 2 | #ifdef COMPILE_FLOAT64_CODE_ROSSERIAL 3 | 4 | /* 5 | * rosserial::std_msgs::Float64 Test 6 | * Receives a Float64 input, subtracts 1.0, and publishes it 7 | */ 8 | 9 | #include "mbed.h" 10 | #include 11 | #include 12 | 13 | 14 | ros::NodeHandle nh; 15 | 16 | float x; 17 | DigitalOut myled(LED1); 18 | 19 | void messageCb( const std_msgs::Float64& msg) { 20 | x = msg.data - 1.0; 21 | myled = !myled; // blink the led 22 | } 23 | 24 | std_msgs::Float64 test; 25 | ros::Subscriber s("your_topic", &messageCb); 26 | ros::Publisher p("my_topic", &test); 27 | 28 | int main() { 29 | nh.initNode(); 30 | nh.advertise(p); 31 | nh.subscribe(s); 32 | while (1) { 33 | test.data = x; 34 | p.publish( &test ); 35 | nh.spinOnce(); 36 | wait_ms(10); 37 | } 38 | } 39 | #endif -------------------------------------------------------------------------------- /rosserial_mbed/src/tests/float64_test/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_float64_test 2 | DEVICES := LPC1768 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | NO_FLOAT_SCANF := 1 5 | NO_FLOAT_PRINTF := 1 6 | 7 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 8 | -------------------------------------------------------------------------------- /rosserial_mbed/src/tests/time_test/makefile: -------------------------------------------------------------------------------- 1 | PROJECT := rosserial_mbed_time_test 2 | DEVICES := LPC1768 3 | GCC4MBED_DIR := $(GCC4MBED_DIR) 4 | NO_FLOAT_SCANF := 1 5 | NO_FLOAT_PRINTF := 1 6 | 7 | include $(GCC4MBED_DIR)/build/gcc4mbed.mk 8 | -------------------------------------------------------------------------------- /rosserial_mbed/src/tests/time_test/time_test.cpp: -------------------------------------------------------------------------------- 1 | //#define COMPILE_TIME_CODE_ROSSERIAL 2 | #ifdef COMPILE_TIME_CODE_ROSSERIAL 3 | 4 | /* 5 | * rosserial::std_msgs::Time Test 6 | * Publishes current time 7 | */ 8 | 9 | #include "mbed.h" 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | ros::NodeHandle nh; 16 | 17 | std_msgs::Time test; 18 | ros::Publisher p("my_topic", &test); 19 | 20 | int main() { 21 | nh.initNode(); 22 | nh.advertise(p); 23 | 24 | while (1) { 25 | test.data = nh.now(); 26 | p.publish( &test ); 27 | nh.spinOnce(); 28 | wait_ms(10); 29 | } 30 | } 31 | #endif -------------------------------------------------------------------------------- /rosserial_mbed/srv/Test.srv: -------------------------------------------------------------------------------- 1 | string input 2 | --- 3 | string output 4 | -------------------------------------------------------------------------------- /rosserial_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | ) 7 | 8 | add_message_files(FILES 9 | Log.msg 10 | TopicInfo.msg 11 | ) 12 | 13 | add_service_files(FILES 14 | RequestParam.srv 15 | ) 16 | 17 | generate_messages() 18 | 19 | catkin_package(CATKIN_DEPENDS 20 | message_runtime 21 | ) 22 | -------------------------------------------------------------------------------- /rosserial_msgs/msg/Log.msg: -------------------------------------------------------------------------------- 1 | 2 | #ROS Logging Levels 3 | uint8 ROSDEBUG=0 4 | uint8 INFO=1 5 | uint8 WARN=2 6 | uint8 ERROR=3 7 | uint8 FATAL=4 8 | 9 | uint8 level 10 | string msg 11 | -------------------------------------------------------------------------------- /rosserial_msgs/msg/TopicInfo.msg: -------------------------------------------------------------------------------- 1 | # special topic_ids 2 | uint16 ID_PUBLISHER=0 3 | uint16 ID_SUBSCRIBER=1 4 | uint16 ID_SERVICE_SERVER=2 5 | uint16 ID_SERVICE_CLIENT=4 6 | uint16 ID_PARAMETER_REQUEST=6 7 | uint16 ID_LOG=7 8 | uint16 ID_TIME=10 9 | uint16 ID_TX_STOP=11 10 | 11 | # The endpoint ID for this topic 12 | uint16 topic_id 13 | 14 | string topic_name 15 | string message_type 16 | 17 | # MD5 checksum for this message type 18 | string md5sum 19 | 20 | # size of the buffer message must fit in 21 | int32 buffer_size 22 | -------------------------------------------------------------------------------- /rosserial_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_msgs 3 | 0.9.2 4 | 5 | Messages for automatic topic configuration using rosserial. 6 | 7 | Michael Ferguson 8 | Paul Bouchier 9 | Mike Purvis 10 | BSD 11 | http://ros.org/wiki/rosserial_msgs 12 | 13 | catkin 14 | 15 | message_generation 16 | 17 | message_runtime 18 | 19 | -------------------------------------------------------------------------------- /rosserial_msgs/srv/RequestParam.srv: -------------------------------------------------------------------------------- 1 | string name 2 | 3 | --- 4 | 5 | int32[] ints 6 | float32[] floats 7 | string[] strings 8 | -------------------------------------------------------------------------------- /rosserial_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_python) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | catkin_python_setup() 8 | 9 | catkin_install_python( 10 | PROGRAMS nodes/message_info_service.py nodes/serial_node.py 11 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 12 | ) 13 | -------------------------------------------------------------------------------- /rosserial_python/nodes/message_info_service.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ##################################################################### 4 | # Software License Agreement (BSD License) 5 | # 6 | # Copyright (c) 2013, Clearpath Robotics 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of Willow Garage, Inc. nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | 36 | """ This functionality is no longer needed, as it's now handled 37 | by accessing the Python interpreter directly from rosserial_server. 38 | The file remains to avoid breaking launch files which include it.""" 39 | 40 | import rospy 41 | 42 | 43 | if __name__=="__main__": 44 | rospy.init_node('message_info_service') 45 | rospy.logwarn("The message info service node is no longer needed and will be " 46 | "removed in a future release of rosserial.") 47 | rospy.spin() 48 | -------------------------------------------------------------------------------- /rosserial_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_python 3 | 0.9.2 4 | 5 | A Python-based implementation of the rosserial protocol. 6 | 7 | Michael Ferguson 8 | Paul Bouchier 9 | Mike Purvis 10 | BSD 11 | http://ros.org/wiki/rosserial_python 12 | 13 | catkin 14 | 15 | rospy 16 | rosserial_msgs 17 | diagnostic_msgs 18 | python3-serial 19 | 20 | -------------------------------------------------------------------------------- /rosserial_python/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosserial_python'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rosserial_python/src/rosserial_python/__init__.py: -------------------------------------------------------------------------------- 1 | from .SerialClient import * 2 | -------------------------------------------------------------------------------- /rosserial_server/include/rosserial_server/msg_lookup.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \author Mike Purvis 3 | * \copyright Copyright (c) 2019, Clearpath Robotics, Inc. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of Clearpath Robotics, Inc. nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | * Please send comments, questions, or patches to code@clearpathrobotics.com 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | namespace rosserial_server 34 | { 35 | 36 | struct MsgInfo 37 | { 38 | std::string md5sum; 39 | std::string full_text; 40 | }; 41 | 42 | const MsgInfo lookupMessage(const std::string& message_type, const std::string submodule = "msg"); 43 | 44 | } // namespace rosserial_server 45 | -------------------------------------------------------------------------------- /rosserial_server/launch/serial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /rosserial_server/launch/socket.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /rosserial_server/launch/udp_socket.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /rosserial_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosserial_server 4 | 0.9.2 5 | 6 | A more performance- and stability-oriented server alternative implemented 7 | in C++ to rosserial_python. 8 | 9 | 10 | Mike Purvis 11 | Mike Purvis 12 | 13 | BSD 14 | 15 | catkin 16 | rosserial_msgs 17 | std_msgs 18 | roscpp 19 | topic_tools 20 | python3-dev 21 | libboost-thread-dev 22 | libboost-thread 23 | 24 | -------------------------------------------------------------------------------- /rosserial_server/src/serial_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \file 4 | * \brief Main entry point for the serial node. 5 | * \author Mike Purvis 6 | * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of Clearpath Robotics, Inc. nor the 16 | * names of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY 23 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * Please send comments, questions, or patches to code@clearpathrobotics.com 31 | * 32 | */ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include "rosserial_server/serial_session.h" 41 | 42 | 43 | int main(int argc, char* argv[]) 44 | { 45 | ros::init(argc, argv, "rosserial_server_serial_node"); 46 | 47 | std::string port; 48 | int baud; 49 | ros::param::param("~port", port, "/dev/ttyACM0"); 50 | ros::param::param("~baud", baud, 57600); 51 | 52 | boost::asio::io_service io_service; 53 | rosserial_server::SerialSession serial_session(io_service, port, baud); 54 | io_service.run(); 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /rosserial_server/src/socket_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \file 4 | * \brief Main entry point for the socket server node. 5 | * \author Mike Purvis 6 | * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of Clearpath Robotics, Inc. nor the 16 | * names of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY 23 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * Please send comments, questions, or patches to code@clearpathrobotics.com 31 | * 32 | */ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include "rosserial_server/tcp_server.h" 41 | 42 | 43 | int main(int argc, char* argv[]) 44 | { 45 | ros::init(argc, argv, "rosserial_server_socket_node"); 46 | 47 | int port; 48 | ros::param::param("~port", port, 11411); 49 | 50 | boost::asio::io_service io_service; 51 | rosserial_server::TcpServer<> tcp_server(io_service, port); 52 | 53 | ROS_INFO_STREAM("Listening for rosserial TCP connections on port " << port); 54 | io_service.run(); 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /rosserial_test/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_test 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Fix Travis for Noetic + Python 3 14 | * Bump minimum CMake version to 3.7.2 (Melodic). 15 | * Generate the test client using PYTHON_EXECUTABLE. 16 | * Disable rosserial_python test. 17 | * Properly initialize message_info stub, drop from test. 18 | * Contributors: Asuki Kono, Mike Purvis 19 | 20 | 0.8.0 (2018-10-11) 21 | ------------------ 22 | 23 | 0.7.7 (2017-11-29) 24 | ------------------ 25 | * Fix catkin lint errors (`#296 `_) 26 | * pyserial bug workaround to fix rosserial_test test against SerialClient.py (`#313 `_) 27 | * Contributors: Bei Chen Liu, Tom O'Connell 28 | 29 | 0.7.6 (2017-03-01) 30 | ------------------ 31 | 32 | 0.7.5 (2016-11-22) 33 | ------------------ 34 | 35 | 0.7.4 (2016-09-21) 36 | ------------------ 37 | * Integration tests for rosserial (`#243 `_) 38 | * Contributors: Mike Purvis 39 | -------------------------------------------------------------------------------- /rosserial_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_test) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rosserial_client 7 | rosserial_msgs 8 | rosserial_python 9 | rosserial_server 10 | rostest 11 | std_msgs 12 | ) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS 16 | rosserial_msgs 17 | std_msgs 18 | ) 19 | 20 | if(CATKIN_ENABLE_TESTING) 21 | # Generate a client library for the test harnesses to use. 22 | add_custom_command( 23 | OUTPUT ${PROJECT_BINARY_DIR}/include/rosserial 24 | COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh ${PYTHON_EXECUTABLE} 25 | ${PROJECT_SOURCE_DIR}/scripts/generate_client_ros_lib.py ${PROJECT_BINARY_DIR}/include 26 | ) 27 | add_custom_target(${PROJECT_NAME}_rosserial_lib DEPENDS ${PROJECT_BINARY_DIR}/include/rosserial) 28 | add_dependencies(${PROJECT_NAME}_rosserial_lib ${catkin_EXPORTED_TARGETS}) 29 | 30 | include_directories( 31 | include ${PROJECT_BINARY_DIR}/include ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | # Helper for building and linking test executables. 35 | function(add_${PROJECT_NAME}_executable test_source) 36 | set(target ${PROJECT_NAME}_${test_source}) 37 | add_executable(${target} EXCLUDE_FROM_ALL src/${test_source}) 38 | add_dependencies(${target} ${PROJECT_NAME}_rosserial_lib) 39 | target_link_libraries(${target} ${GTEST_LIBRARIES} ${catkin_LIBRARIES}) 40 | add_dependencies(tests ${target}) 41 | endfunction() 42 | 43 | add_rosserial_test_executable(publish_subscribe) 44 | 45 | add_rostest(test/rosserial_server_socket.test) 46 | add_rostest(test/rosserial_server_serial.test) 47 | add_rostest(test/rosserial_python_socket.test) 48 | # Disabled due to reconnect logic in rosserial_python not being robust enough. 49 | # add_rostest(test/rosserial_python_serial.test) 50 | endif() 51 | -------------------------------------------------------------------------------- /rosserial_test/include/rosserial_test/helpers.h: -------------------------------------------------------------------------------- 1 | 2 | template 3 | class Callback { 4 | public: 5 | Callback() : times_called(0) 6 | { 7 | } 8 | 9 | void callback(const Msg msg) 10 | { 11 | times_called++; 12 | last_msg = msg; 13 | } 14 | 15 | Msg last_msg; 16 | int times_called; 17 | }; 18 | typedef Callback StringCallback; 19 | 20 | 21 | -------------------------------------------------------------------------------- /rosserial_test/include/rosserial_test/ros.h: -------------------------------------------------------------------------------- 1 | #ifndef ROSSERIAL_ROS_H 2 | #define ROSSERIAL_ROS_H 3 | 4 | #include "rosserial/ros/node_handle.h" 5 | #include "rosserial/duration.cpp" 6 | #include "rosserial/time.cpp" 7 | #include 8 | 9 | class ClientComms { 10 | public: 11 | // Can smuggle in an fd representing either the back end of 12 | // a socket or serial pty, and run the same tests over both. 13 | static int fd; 14 | 15 | // Accessible to be manipulated by tests, for test behaviours 16 | // dependent on the passage of time. 17 | static unsigned long millis; 18 | 19 | void init() { 20 | } 21 | int read() { 22 | unsigned char ch; 23 | ssize_t ret = ::read(fd, &ch, 1); 24 | return ret == 1 ? ch : -1; 25 | } 26 | void write(uint8_t* data, int length) { 27 | ::write(fd, data, length); 28 | } 29 | unsigned long time() { 30 | return millis; 31 | } 32 | }; 33 | 34 | int ClientComms::fd = -1; 35 | unsigned long ClientComms::millis = 0; 36 | 37 | namespace ros { 38 | typedef NodeHandle_ NodeHandle; 39 | } 40 | 41 | #endif // ROSSERIAL_ROS_H 42 | -------------------------------------------------------------------------------- /rosserial_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosserial_test 4 | 0.9.2 5 | 6 | A specialized harness which allows end-to-end integration testing of the 7 | rosserial client and server components. 8 | 9 | 10 | Mike Purvis 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | roscpp 17 | gtest 18 | rosserial_client 19 | rosserial_msgs 20 | rosserial_python 21 | rosserial_server 22 | rostest 23 | std_msgs 24 | 25 | -------------------------------------------------------------------------------- /rosserial_test/test/rosserial_python_serial.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rosserial_test/test/rosserial_python_socket.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /rosserial_test/test/rosserial_server_serial.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rosserial_test/test/rosserial_server_socket.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rosserial_tivac/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_tivac 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Use os.path.join for path concatenation (`#495 `_) 14 | * Fix Travis for Noetic + Python 3 15 | * Bump minimum CMake version to 3.7.2 (Melodic). 16 | * Contributors: Hermann von Kleist, Mike Purvis 17 | 18 | 0.8.0 (2018-10-11) 19 | ------------------ 20 | 21 | 0.7.7 (2017-11-29) 22 | ------------------ 23 | * Fix catkin lint errors (`#296 `_) 24 | * Contributors: Bei Chen Liu 25 | 26 | 0.7.6 (2017-03-01) 27 | ------------------ 28 | * Allows to specify chip's silicon revision for TivaWare. (`#268 `_) 29 | * Updating rosserial_tivac for new TivaWare compilation flags (`#260 `_) 30 | * Contributors: Vitor Matos 31 | 32 | 0.7.5 (2016-11-22) 33 | ------------------ 34 | 35 | 0.7.4 (2016-09-21) 36 | ------------------ 37 | 38 | 0.7.3 (2016-08-05) 39 | ------------------ 40 | 41 | 0.7.2 (2016-07-15) 42 | ------------------ 43 | * rosserial package for TivaC Launchpad boards. 44 | * Contributors: Vitor Matos 45 | -------------------------------------------------------------------------------- /rosserial_tivac/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_tivac) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake 8 | ) 9 | 10 | install( 11 | DIRECTORY 12 | src/ros_lib 13 | src/ros_lib_energia 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 15 | ) 16 | 17 | install( 18 | DIRECTORY tivac-cmake 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 20 | ) 21 | 22 | install( 23 | PROGRAMS 24 | src/${PROJECT_NAME}/make_libraries_energia 25 | src/${PROJECT_NAME}/make_libraries_tiva 26 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 27 | ) 28 | -------------------------------------------------------------------------------- /rosserial_tivac/cmake/rosserial_tivac-extras.cmake.em: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | 3 | @[if DEVELSPACE]@ 4 | set(ROSSERIAL_TIVAC_TOOLCHAIN "@(CMAKE_CURRENT_SOURCE_DIR)/tivac-cmake/cmake/TivaCToolchain.cmake") 5 | @[else]@ 6 | set(ROSSERIAL_TIVAC_TOOLCHAIN "${rosserial_tivac_DIR}/../tivac-cmake/cmake/TivaCToolchain.cmake") 7 | @[end if]@ 8 | -------------------------------------------------------------------------------- /rosserial_tivac/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_tivac 3 | 0.9.2 4 | 5 | rosserial for TivaC Launchpad evaluation boards. 6 | 7 | Vitor Matos 8 | BSD 9 | http://wiki.ros.org/rosserial_tivac 10 | https://github.com/ros-drivers/rosserial 11 | Vitor Matos 12 | Vitor Matos 13 | 14 | catkin 15 | 16 | rosserial_msgs 17 | rosserial_client 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Robosavvy Ltd. 3 | * All rights reserved. 4 | * Author: Vitor Matos 5 | * 6 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 7 | * following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | * following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 12 | * following disclaimer in the documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote 14 | * products derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 17 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 19 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 20 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 21 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 22 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | */ 24 | 25 | #ifndef ROSSERIAL_TIVAC_SRC_ROS_LIB_ROS_H 26 | #define ROSSERIAL_TIVAC_SRC_ROS_LIB_ROS_H 27 | 28 | #include 29 | 30 | #ifdef USE_USBCON 31 | #include 32 | #else 33 | #include 34 | #endif 35 | 36 | namespace ros 37 | { 38 | /*! 39 | int MAX_SUBSCRIBERS=25, 40 | int MAX_PUBLISHERS=25, 41 | int INPUT_SIZE=512, 42 | int OUTPUT_SIZE=512 43 | */ 44 | typedef NodeHandle_ NodeHandle; 45 | } 46 | #endif // ROSSERIAL_TIVAC_SRC_ROS_LIB_ROS_H 47 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib/tivac_hardware.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Robosavvy Ltd. 3 | * All rights reserved. 4 | * Author: Vitor Matos 5 | * 6 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 7 | * following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | * following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 12 | * following disclaimer in the documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote 14 | * products derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 17 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 19 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 20 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 21 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 22 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | */ 24 | 25 | #include 26 | #include 27 | 28 | #ifndef USE_USBCON 29 | #include 30 | 31 | tRingBufObject rxBuffer; 32 | tRingBufObject txBuffer; 33 | #endif 34 | 35 | volatile uint32_t g_ui32milliseconds = 0; 36 | volatile uint32_t g_ui32heartbeat = 0; 37 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/examples/chatter/chatter.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | std_msgs::String str_msg; 11 | ros::Publisher chatter("chatter", &str_msg); 12 | 13 | char hello[13] = "hello world!"; 14 | 15 | void setup() 16 | { 17 | nh.initNode(); 18 | nh.advertise(chatter); 19 | } 20 | 21 | void loop() 22 | { 23 | str_msg.data = hello; 24 | chatter.publish( &str_msg ); 25 | nh.spinOnce(); 26 | delay(1000); 27 | } 28 | 29 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/examples/rgb/rgb.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial subscriber Example 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | void color_cb( const std_msgs::ColorRGBA& msg) 9 | { 10 | analogWrite(RED_LED,msg.r*255); 11 | analogWrite(GREEN_LED,msg.g*255); 12 | analogWrite(BLUE_LED,msg.b*255); 13 | } 14 | 15 | ros::NodeHandle nh; 16 | ros::Subscriber color_sub("led", &color_cb ); 17 | 18 | void setup() 19 | { 20 | pinMode(RED_LED, OUTPUT); 21 | pinMode(GREEN_LED, OUTPUT); 22 | pinMode(BLUE_LED, OUTPUT); 23 | 24 | analogWrite(RED_LED,255); 25 | analogWrite(GREEN_LED,255); 26 | analogWrite(BLUE_LED,255); 27 | 28 | nh.initNode(); 29 | nh.subscribe(color_sub); 30 | } 31 | 32 | void loop() 33 | { 34 | nh.spinOnce(); 35 | delay(100); 36 | } 37 | 38 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Robosavvy Ltd. 3 | * All rights reserved. 4 | * Author: Vitor Matos 5 | * 6 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 7 | * following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | * following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 12 | * following disclaimer in the documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote 14 | * products derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 17 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 19 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 20 | * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 21 | * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 22 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | */ 24 | 25 | #ifndef ROS_LIB_ENERGIA_ROS_H 26 | #define ROS_LIB_ENERGIA_ROS_H 27 | 28 | #include "ros/node_handle.h" 29 | #include "TivaCHardware.h" 30 | 31 | namespace ros 32 | { 33 | typedef NodeHandle_ NodeHandle; 34 | } 35 | #endif // ROS_LIB_ENERGIA_ROS_H 36 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/tests/array_test/array_test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::geometry_msgs::PoseArray Test 3 | * Sums an array, publishes sum 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | bool set_; 15 | 16 | 17 | geometry_msgs::Pose sum_msg; 18 | ros::Publisher p("sum", &sum_msg); 19 | 20 | void messageCb(const geometry_msgs::PoseArray& msg){ 21 | sum_msg.position.x = 0; 22 | sum_msg.position.y = 0; 23 | sum_msg.position.z = 0; 24 | for(int i = 0; i < msg.poses_length; i++) 25 | { 26 | sum_msg.position.x += msg.poses[i].position.x; 27 | sum_msg.position.y += msg.poses[i].position.y; 28 | sum_msg.position.z += msg.poses[i].position.z; 29 | } 30 | digitalWrite(RED_LED, HIGH-digitalRead(RED_LED)); // blink the led 31 | } 32 | 33 | ros::Subscriber s("poses",messageCb); 34 | 35 | void setup() 36 | { 37 | pinMode(RED_LED, OUTPUT); 38 | nh.initNode(); 39 | nh.subscribe(s); 40 | nh.advertise(p); 41 | } 42 | 43 | void loop() 44 | { 45 | p.publish(&sum_msg); 46 | nh.spinOnce(); 47 | delay(10); 48 | } 49 | 50 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/tests/array_test/test_data: -------------------------------------------------------------------------------- 1 | Test data example: 2 | 3 | rostopic pub /poses geometry_msgs/PoseArray "header: 4 | seq: 0 5 | stamp: 6 | secs: 0 7 | nsecs: 0 8 | frame_id: '' 9 | poses: 10 | - position: 11 | x: 0.5 12 | y: 0.0 13 | z: 0.0 14 | orientation: 15 | x: 0.0 16 | y: 0.0 17 | z: 0.0 18 | w: 0.0 19 | - position: 20 | x: 0.0 21 | y: 1.7 22 | z: -1.0 23 | orientation: 24 | x: 0.0 25 | y: 0.0 26 | z: 0.0 27 | w: 0.0 28 | - position: 29 | x: 0.0 30 | y: 0.0 31 | z: 2.0 32 | orientation: 33 | x: 0.0 34 | y: 0.0 35 | z: 0.0 36 | w: 0.0" 37 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/tests/float64_test/float64_test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Float64 Test 3 | * Receives a Float64 input, subtracts 1.0, and publishes it 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | 10 | ros::NodeHandle nh; 11 | 12 | float x; 13 | 14 | void messageCb( const std_msgs::Float64& msg){ 15 | x = msg.data - 1.0; 16 | digitalWrite(RED_LED, HIGH-digitalRead(RED_LED)); // blink the led 17 | } 18 | 19 | std_msgs::Float64 test; 20 | ros::Subscriber s("your_topic", &messageCb); 21 | ros::Publisher p("my_topic", &test); 22 | 23 | void setup() 24 | { 25 | pinMode(RED_LED, OUTPUT); 26 | nh.initNode(); 27 | nh.advertise(p); 28 | nh.subscribe(s); 29 | } 30 | 31 | void loop() 32 | { 33 | test.data = x; 34 | p.publish( &test ); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | 39 | -------------------------------------------------------------------------------- /rosserial_tivac/src/ros_lib_energia/tests/time_test/time_test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Time Test 3 | * Publishes current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | std_msgs::Time test; 14 | ros::Publisher p("my_topic", &test); 15 | 16 | void setup() 17 | { 18 | pinMode(RED_LED, OUTPUT); 19 | nh.initNode(); 20 | nh.advertise(p); 21 | } 22 | 23 | void loop() 24 | { 25 | test.data = nh.now(); 26 | p.publish( &test ); 27 | nh.spinOnce(); 28 | delay(10); 29 | } 30 | 31 | -------------------------------------------------------------------------------- /rosserial_tivac/tivac-cmake/tiva.specs: -------------------------------------------------------------------------------- 1 | *link: 2 | --entry ResetISR --gc-sections 3 | 4 | *lib: 5 | -lgcc -lc -lm -lnosys 6 | -------------------------------------------------------------------------------- /rosserial_tivac/tivac-cmake/tm4c123g.ld: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Based on Energia's ld script 3 | ******************************************************************************/ 4 | 5 | MEMORY 6 | { 7 | FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 0x00040000 8 | SRAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00008000 9 | } 10 | 11 | SECTIONS 12 | { 13 | .text : 14 | { 15 | . = ALIGN(4); 16 | _text = .; 17 | KEEP(*(.isr_vector)) 18 | *(.text .text* .gnu.linkonce.t.*) 19 | *(.glue_7t) *(.glue_7) 20 | *(.rodata .rodata* .gnu.linkonce.r.*) 21 | *(.ARM.extab* .gnu.linkonce.armextab.*) 22 | 23 | . = ALIGN(4); 24 | KEEP(*(.init)) 25 | 26 | . = ALIGN(4); 27 | __preinit_array_start = .; 28 | KEEP(*(SORT(.preinit_array*))) 29 | KEEP(*(.preinit_array)) 30 | __preinit_array_end = .; 31 | 32 | . = ALIGN(4); 33 | __init_array_start = .; 34 | KEEP(*(SORT(.init_array.*))) 35 | KEEP(*(.init_array)) 36 | __init_array_end = .; 37 | 38 | . = ALIGN(4); 39 | KEEP(*(.fini)) 40 | 41 | . = ALIGN(4); 42 | __fini_array_start = .; 43 | KEEP(*(.fini_array)) 44 | KEEP(*(SORT(.fini_array.*))) 45 | __fini_array_end = .; 46 | 47 | . = ALIGN(4); 48 | _etext = .; 49 | } > FLASH 50 | 51 | .data : AT(ADDR(.text) + SIZEOF(.text)) 52 | { 53 | . = ALIGN(4); 54 | _data = .; 55 | *(vtable) 56 | *(.data .data* .gnu.linkonce.d.*) 57 | _edata = .; 58 | } > SRAM 59 | 60 | .bss (NOLOAD): 61 | { 62 | . = ALIGN(4); 63 | _bss = .; 64 | __bss_start__ = _bss; 65 | *(.bss*) 66 | *(COMMON) 67 | . = ALIGN(4); 68 | _ebss = .; 69 | __bss_end__ = _ebss; 70 | . = ALIGN(8); 71 | } > SRAM 72 | 73 | PROVIDE_HIDDEN(__exidx_start = .); 74 | .ARM.exidx : 75 | { 76 | *(.ARM.exidx* .gnu.linkonce.armexidx.*) 77 | } > SRAM 78 | PROVIDE_HIDDEN(__exidx_end = .); 79 | 80 | . = ALIGN(4); 81 | _end = . ; 82 | } 83 | 84 | /* end of allocated ram is start of heap, heap grows up towards stack*/ 85 | PROVIDE(end = _end); 86 | 87 | /* top of stack starts at end of ram, stack grows down towards heap */ 88 | PROVIDE(_estack = ORIGIN(SRAM) + LENGTH(SRAM)); 89 | -------------------------------------------------------------------------------- /rosserial_tivac/tivac-cmake/tm4c1294.ld: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Based on Energia's ld script 3 | ******************************************************************************/ 4 | 5 | MEMORY 6 | { 7 | FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 0x00100000 8 | SRAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00040000 9 | } 10 | 11 | SECTIONS 12 | { 13 | .text : 14 | { 15 | . = ALIGN(4); 16 | _text = .; 17 | KEEP(*(.isr_vector)) 18 | *(.text .text* .gnu.linkonce.t.*) 19 | *(.glue_7t) *(.glue_7) 20 | *(.rodata .rodata* .gnu.linkonce.r.*) 21 | *(.ARM.extab* .gnu.linkonce.armextab.*) 22 | 23 | . = ALIGN(4); 24 | KEEP(*(.init)) 25 | 26 | . = ALIGN(4); 27 | __preinit_array_start = .; 28 | KEEP(*(SORT(.preinit_array*))) 29 | KEEP(*(.preinit_array)) 30 | __preinit_array_end = .; 31 | 32 | . = ALIGN(4); 33 | __init_array_start = .; 34 | KEEP(*(SORT(.init_array.*))) 35 | KEEP(*(.init_array)) 36 | __init_array_end = .; 37 | 38 | . = ALIGN(4); 39 | KEEP(*(.fini)) 40 | 41 | . = ALIGN(4); 42 | __fini_array_start = .; 43 | KEEP(*(.fini_array)) 44 | KEEP(*(SORT(.fini_array.*))) 45 | __fini_array_end = .; 46 | 47 | . = ALIGN(4); 48 | _etext = .; 49 | } > FLASH 50 | 51 | .data : AT(ADDR(.text) + SIZEOF(.text)) 52 | { 53 | . = ALIGN(4); 54 | _data = .; 55 | *(vtable) 56 | *(.data .data* .gnu.linkonce.d.*) 57 | _edata = .; 58 | } > SRAM 59 | 60 | .bss (NOLOAD): 61 | { 62 | . = ALIGN(4); 63 | _bss = .; 64 | __bss_start__ = _bss; 65 | *(.bss*) 66 | *(COMMON) 67 | . = ALIGN(4); 68 | _ebss = .; 69 | __bss_end__ = _ebss; 70 | . = ALIGN(8); 71 | } > SRAM 72 | 73 | PROVIDE_HIDDEN(__exidx_start = .); 74 | .ARM.exidx : 75 | { 76 | *(.ARM.exidx* .gnu.linkonce.armexidx.*) 77 | } > SRAM 78 | PROVIDE_HIDDEN(__exidx_end = .); 79 | 80 | . = ALIGN(4); 81 | _end = . ; 82 | } 83 | 84 | /* end of allocated ram is start of heap, heap grows up towards stack*/ 85 | PROVIDE(end = _end); 86 | 87 | /* top of stack starts at end of ram, stack grows down towards heap */ 88 | PROVIDE(_estack = ORIGIN(SRAM) + LENGTH(SRAM)); 89 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_vex_cortex 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 0.9.2 (2021-04-02) 5 | ------------------ 6 | * Fix header mismatch in changelog. 7 | * Contributors: Mike Purvis 8 | 9 | 0.9.1 (2020-09-09) 10 | ------------------ 11 | 12 | 0.9.0 (2020-08-25) 13 | ------------------ 14 | * Use os.path.join for path concatenation (`#495 `_) 15 | * Fix Travis for Noetic + Python 3 16 | * Bump minimum CMake version to 3.7.2 (Melodic). 17 | * Fix py3 print usages and trailing whitespaces (`#469 `_) 18 | * Contributors: Hermann von Kleist, Mike Purvis, acxz 19 | 20 | 0.8.0 (2018-10-11) 21 | ------------------ 22 | * VEX Cortex Usage improvements and VEX V5 Support (`#385 `_) 23 | * Re-attempting rosserial for VEX Cortex (`#380 `_) 24 | * Contributors: CanyonTurtle 25 | 26 | 0.7.7 27 | ----------------------------- 28 | - README fixes 29 | - started tagging versions 30 | - initial was wrong version number, fixed. 31 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_vex_cortex) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package(CATKIN_DEPENDS) 9 | 10 | install( 11 | DIRECTORY src/ros_lib 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 13 | ) 14 | 15 | install( 16 | PROGRAMS scripts/genproject.sh 17 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 18 | ) 19 | 20 | install( 21 | DIRECTORY launch/ 22 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 23 | ) 24 | 25 | catkin_install_python( 26 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 28 | ) 29 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/launch/hello_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/launch/joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/launch/minimal_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_vex_cortex 3 | 0.9.2 4 | 5 | rosserial for Cortex/AVR platforms. 6 | 7 | Cannon 8 | Canyon Turtle 9 | BSD 10 | 11 | 12 | catkin 13 | 14 | rospy 15 | rosserial_client 16 | rosserial_python 17 | 18 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/scripts/genproject.sh: -------------------------------------------------------------------------------- 1 | # generate new pros project with name as first argument 2 | pros conduct new $1 3 | 4 | # go into incude directory and generate ros message library. 5 | cd $1/include 6 | rosrun rosserial_vex_cortex make_libraries.py . 7 | 8 | # custom include for files containing strlen. 9 | # grep -lnrw '.' -e 'strlen' 10 | grep -lrnw '.' -e 'strlen' | xargs sed -i '1 s@^@#include "vexstrlen.h"\n@g; s@strlen(@vexstrlen(@g' 11 | 12 | # move the cpp files into the src directory. 13 | mv ros_lib/*.cpp ../src 14 | mv ros_lib/examples/*.cpp ../src 15 | cd .. 16 | 17 | # delete the incorrect files in src 18 | rm src/*.c 19 | 20 | # replace the common.mk with the correct one. 21 | sed -i '/^INCLUDE=/ s@$@ -I$(ROOT)/include/ros_lib@' common.mk 22 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosserial_vex_cortex'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/auto.cpp: -------------------------------------------------------------------------------- 1 | /** @file auto.cpp 2 | * @brief File for autonomous code 3 | * 4 | * This file should contain the user autonomous() function and any functions related to it. 5 | * 6 | * Any copyright is dedicated to the Public Domain. 7 | * http://creativecommons.org/publicdomain/zero/1.0/ 8 | * 9 | * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be 10 | * obtained from http://sourceforge.net/projects/freertos/files/ or on request. 11 | */ 12 | 13 | #include "main.h" 14 | 15 | /* 16 | * 17 | * 18 | * NO GLOBAL OBJECT DEFINITIONS! 19 | * 20 | * DO NOT globally define either structs or objects. 21 | * See issue: https://github.com/purduesigbots/pros/issues/48 22 | * 23 | * Instead, put all objects/structs inside functions. 24 | * 25 | * 26 | */ 27 | 28 | /* 29 | * Runs the user autonomous code. This function will be started in its own task with the default 30 | * priority and stack size whenever the robot is enabled via the Field Management System or the 31 | * VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is 32 | * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart 33 | * the task, not re-start it from where it left off. 34 | * 35 | * Code running in the autonomous task cannot access information from the VEX Joystick. However, 36 | * the autonomous function can be invoked from another task if a VEX Competition Switch is not 37 | * available, and it can access joystick information if called in this way. 38 | * 39 | * The autonomous task may exit, unlike operatorControl() which should never exit. If it does 40 | * so, the robot will await a switch to another mode or disable/enable cycle. 41 | */ 42 | void autonomous() { 43 | } 44 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/examples/helloworld.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example for VEX Cortex 3 | * Prints "hello world!" 4 | * 5 | * 6 | * 7 | * Note: defining rosserial objects on the global scope will cause segmentation faults. 8 | * put variables in functons. 9 | */ 10 | 11 | #include 12 | #include 13 | 14 | // this loop is run in setup function, which publishes at 50hz. 15 | inline void loop(ros::NodeHandle & nh, ros::Publisher & p, std_msgs::String & str_msg, char* msgdata) 16 | { 17 | str_msg.data = msgdata; 18 | p.publish( &str_msg ); 19 | nh.spinOnce(); 20 | delay(20); 21 | } 22 | 23 | // The setup function will start a publisher on the topic "chatter" and begin publishing there. 24 | inline void setup() 25 | { 26 | // debug logging 27 | vexroslog("\n\n\n\n\r\t\tROSserial for VEX Cortex V2 - June 2018 - START\n\n\n\n\n\r"); 28 | 29 | // make a node handle object, string message, and publisher for that message. 30 | ros::NodeHandle nh; 31 | std_msgs::String str_msg; 32 | ros::Publisher chatter("chatter\0", &str_msg); 33 | 34 | // set up rosserial, and prepare to publish the chatter message 35 | nh.initNode(); 36 | nh.advertise(chatter); 37 | 38 | // message data variable. 39 | char* msg = (char*) malloc(20 * sizeof(char)); 40 | while (1) { 41 | 42 | // send a message about the time! 43 | sprintf(msg, "[%d] Hello there!!", (int) millis()); 44 | loop(nh, chatter, str_msg, msg); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/examples/joydrive.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Rosserial Vex Cortex joystick control demo 3 | * 4 | * This lets you control a VEX Clawbot (or any other robot, with some modification) 5 | * using a Logitech Wireless gamepad F710. 6 | * 7 | * install the joystick_drivers ROS system dependency: http://wiki.ros.org/joystick_drivers?distro=melodic 8 | * then run `rosrun joy joy_node _autorepeat_rate:=20` 9 | * 10 | * This also can work with a PS3 controller / wiimote (see the ROS joystick_drivers metapackage). 11 | * 12 | * Or any other ROS node that publishes joystick events! 13 | */ 14 | 15 | #include "ros.h" 16 | #include "main.h" 17 | #include "sensor_msgs/Joy.h" 18 | 19 | #define MAX_SPEED 80 20 | 21 | // fired every joystick message, uses the message to set motor powers on robot 22 | inline void moveRobot( const sensor_msgs::Joy &joy_msg) { 23 | motorSet(6, joy_msg.buttons[7] * MAX_SPEED - joy_msg.buttons[5] * MAX_SPEED); 24 | motorSet(7, joy_msg.buttons[6] * MAX_SPEED - joy_msg.buttons[4] * MAX_SPEED); 25 | 26 | int lp = 0; 27 | int rp = 0; 28 | if(joy_msg.axes[0] < 0.0) { lp = -MAX_SPEED; rp = MAX_SPEED; } 29 | else if(joy_msg.axes[0] > 0.0) { lp = MAX_SPEED; rp = -MAX_SPEED; } 30 | else if(joy_msg.axes[1] < 0.0) { lp = -MAX_SPEED; rp = -MAX_SPEED; } 31 | else if(joy_msg.axes[1] > 0.0) { lp = MAX_SPEED; rp = MAX_SPEED; } 32 | else { 33 | lp = MAX_SPEED * (joy_msg.axes[3] + joy_msg.axes[2]); 34 | rp = MAX_SPEED * (joy_msg.axes[3] - joy_msg.axes[2]); 35 | } 36 | 37 | lp = (abs(lp) > 10) ? lp : 0; 38 | rp = (abs(rp) > 10) ? rp : 0; 39 | 40 | motorSet(1, -lp); 41 | motorSet(10, rp); 42 | } 43 | 44 | inline void begin(void*){ 45 | // set up nodehandle instance and a subscriber for Joystick events 46 | ros::NodeHandle nh; 47 | ros::Subscriber sub("joy", &moveRobot); 48 | 49 | nh.initNode(); 50 | nh.subscribe(sub); 51 | 52 | // loop for subscriber does not need delay - this ensures the node handle is as reliable as possible. 53 | while (1) { 54 | nh.spinOnce(); 55 | } 56 | } 57 | 58 | // is a setup function. 59 | inline void setup() 60 | { 61 | taskCreate(&begin, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_HIGHEST); 62 | } 63 | 64 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/examples/twistdrive.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Rosserial Vex Cortex keyboard/phone driving demo 3 | * 4 | * For keyboard 5 | * --------- 6 | * install the keyboard twist publisher: http://wiki.ros.org/teleop_twist_keyboard 7 | * 8 | * run the command described in the link, and key control will work on the robot! 9 | * press center key to re-center. 10 | * 11 | * For phone 12 | * -------- 13 | * Install ROS Nav Map from the google play store 14 | * follow setup instructions, use the joypad! 15 | */ 16 | 17 | #include 18 | #include "geometry_msgs/Twist.h" 19 | 20 | inline void handleControl(const geometry_msgs::Twist& t){ 21 | // power motors using the message event! 22 | // (default configuration is for a clawbot set up in the standard fashion). 23 | motorSet(1, -(int) (100 * t.linear.x + 50 * t.angular.z)); 24 | motorSet(10, (int) (100 * t.linear.x - 50 * t.angular.z)); 25 | } 26 | 27 | // called from opcontrol.cpp 28 | inline void setup() 29 | { 30 | // debug logging 31 | vexroslog("\n\n\n\n\r\t\tROSserial for VEX Cortex V2 - June 2018 - TWIST\n\n\n\n\n\r"); 32 | 33 | // make a node handle object and a subscriber for the Twist message. 34 | ros::NodeHandle nh; 35 | ros::Subscriber sub("cmd_vel\0", &handleControl); 36 | 37 | // set up rosserial, and prepare to publish the chatter message 38 | nh.initNode(); 39 | nh.subscribe(sub); 40 | 41 | // message data variable. 42 | while (1) { 43 | 44 | // send a message about the time! 45 | nh.spinOnce(); 46 | delay(20); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/init.cpp: -------------------------------------------------------------------------------- 1 | /** @file init.cpp 2 | * @brief File for initialization code 3 | * 4 | * This file should contain the user initialize() function and any functions related to it. 5 | * 6 | * Any copyright is dedicated to the Public Domain. 7 | * http://creativecommons.org/publicdomain/zero/1.0/ 8 | * 9 | * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be 10 | * obtained from http://sourceforge.net/projects/freertos/files/ or on request. 11 | */ 12 | 13 | #include "main.h" 14 | 15 | /* 16 | * 17 | * 18 | * NO GLOBAL OBJECT DEFINITIONS! 19 | * 20 | * DO NOT globally define either structs or objects. 21 | * See issue: https://github.com/purduesigbots/pros/issues/48 22 | * 23 | * Instead, put all objects/structs inside functions. 24 | * 25 | * 26 | */ 27 | 28 | /* 29 | * Runs pre-initialization code. This function will be started in kernel mode one time while the 30 | * VEX Cortex is starting up. As the scheduler is still paused, most API functions will fail. 31 | * 32 | * The purpose of this function is solely to set the default pin modes (pinMode()) and port 33 | * states (digitalWrite()) of limit switches, push buttons, and solenoids. It can also safely 34 | * configure a UART port (usartOpen()) but cannot set up an LCD (lcdInit()). 35 | */ 36 | void initializeIO() { 37 | // this is needed to configure the debug serial connection for rosserial. 38 | usartInit(uart2, 57600, SERIAL_8N1); 39 | } 40 | 41 | /* 42 | * Runs user initialization code. This function will be started in its own task with the default 43 | * priority and stack size once when the robot is starting up. It is possible that the VEXnet 44 | * communication link may not be fully established at this time, so reading from the VEX 45 | * Joystick may fail. 46 | * 47 | * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global 48 | * variables, and IMEs. 49 | * 50 | * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks 51 | * will not start. An autonomous mode selection menu like the pre_auton() in other environments 52 | * can be implemented in this task if desired. 53 | */ 54 | void initialize() { 55 | } 56 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/logger.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROSSERIAL_VEX_CORTEX_LOGGER_H_ 2 | #define _ROSSERIAL_VEX_CORTEX_LOGGER_H_ 3 | 4 | #include "API.h" 5 | 6 | // VEXROS defines are used only locally. 7 | 8 | // debugging works over an alternative serial connection. 9 | // 0 for no 10 | // 1 for yes 11 | #define VEXROS_DEBUG_MODE 0 12 | 13 | // which serial connection to output debug messages on. 14 | // To switch the serial connections, switch these values. 15 | // note that stdin/stdout being seperate is unique - stdin/stdout can both be replaced with either uart1 or uart2, 16 | // because UART1 and UART2 work as both input and output serial connections. 17 | #define VEXROS_DEBUG_OUTPUT_SERIAL uart2 18 | #define VEXROS_ROSSERIAL_OUTPUT_SERIAL stdout 19 | #define VEXROS_ROSSERIAL_INPUT_SERIAL stdin 20 | 21 | // logging function for debugging via print statements in the PROS environment. 22 | // this macro simply adds formatting ontop of the regular pros frintf function. 23 | // usage: vexroslog("hello, my favorite number is %d", 3); 24 | // remember to include this header for logging in user code! 25 | #define vexroslog(fmtstr, ...) fprintf((VEXROS_DEBUG_OUTPUT_SERIAL ), "[%d]: " " " fmtstr " " "\r\n", millis() ,##__VA_ARGS__) 26 | 27 | // will print to stdout, used by the serial client.; 28 | #define vexroswrite(...) fprintf((VEXROS_ROSSERIAL_OUTPUT_SERIAL), __VA_ARGS__) 29 | #define vexroswritechar(ch) fputc(ch, VEXROS_ROSSERIAL_OUTPUT_SERIAL) 30 | #define vexrosreadchar() fgetc(VEXROS_ROSSERIAL_INPUT_SERIAL) 31 | 32 | // will only print if debug mode is on (see top of this file). 33 | #define vexroslogdebug(fmtstr, ...) { \ 34 | if(DEBUG_MODE) { \ 35 | vexroslog(fmtstr, ##__VA_ARGS__); \ 36 | } \ 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/opcontrol.cpp: -------------------------------------------------------------------------------- 1 | /** @file opcontrol.cpp 2 | * @brief File for operator control code 3 | * 4 | * This file should contain the user operatorControl() function and any functions related to it. 5 | * 6 | * Any copyright is dedicated to the Public Domain. 7 | * http://creativecommons.org/publicdomain/zero/1.0/ 8 | * 9 | * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be 10 | * obtained from http://sourceforge.net/projects/freertos/files/ or on request. 11 | */ 12 | 13 | #include "main.h" 14 | #include "helloworld.cpp" 15 | 16 | /* 17 | * 18 | * 19 | * NO GLOBAL OBJECT DEFINITIONS! 20 | * 21 | * DO NOT globally define either structs or objects. 22 | * See issue: https://github.com/purduesigbots/pros/issues/48 23 | * 24 | * Instead, put all objects/structs inside functions. 25 | * 26 | * 27 | */ 28 | 29 | // entry point of user control period. 30 | void operatorControl() { 31 | 32 | //running the hello world example 33 | setup(); 34 | 35 | while(1) delay(20); 36 | } 37 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROSSERIAL_VEX_CORTEX_ROS_H_ 36 | #define _ROSSERIAL_VEX_CORTEX_ROS_H_ 37 | 38 | #include "ros/node_handle.h" 39 | 40 | #include "CortexHardware.h" 41 | 42 | namespace ros { 43 | typedef NodeHandle_ NodeHandle; 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/src/ros_lib/vexstrlen.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROSSERIAL_VEX_CORTEX_VEXSTRLEN_H_ 2 | #define _ROSSERIAL_VEX_CORTEX_VEXSTRLEN_H_ 3 | 4 | #define MAX_VEXSTRLEN 2048 5 | 6 | /* substitute string length function. 7 | * if the length is too long or if there is no null terminator, behavior is undefined. 8 | */ 9 | inline uint32_t vexstrlen(const char* st) { 10 | for(int i = 0; i < MAX_VEXSTRLEN; i++) { 11 | if(st[i] == '\0') return i; 12 | } 13 | return MAX_VEXSTRLEN; 14 | } 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /rosserial_vex_cortex/uartdiagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_vex_cortex/uartdiagram.png -------------------------------------------------------------------------------- /rosserial_vex_v5/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_vex_v5 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 0.9.2 (2021-04-02) 5 | ------------------ 6 | 7 | 0.9.1 (2020-09-09) 8 | ------------------ 9 | 10 | 0.9.0 (2020-08-25) 11 | ------------------ 12 | * Use os.path.join for path concatenation (`#495 `_) 13 | * Fix Travis for Noetic + Python 3 14 | * Bump minimum CMake version to 3.7.2 (Melodic). 15 | * Fix py3 print usages and trailing whitespaces (`#469 `_) 16 | * Contributors: Hermann von Kleist, Mike Purvis, acxz 17 | 18 | 0.8.0 (2018-10-11) 19 | ------------------ 20 | * VEX Cortex Usage improvements and VEX V5 Support (`#385 `_) 21 | * Contributors: CanyonTurtle 22 | 23 | 0.7.7 24 | ----------------------------- 25 | - README fixes 26 | - started tagging versions 27 | - initial was wrong version number, fixed. 28 | -------------------------------------------------------------------------------- /rosserial_vex_v5/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_vex_v5) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package(CATKIN_DEPENDS) 9 | 10 | install( 11 | DIRECTORY src/ros_lib 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 13 | ) 14 | 15 | install( 16 | PROGRAMS scripts/genproject.sh 17 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 18 | ) 19 | 20 | install( 21 | DIRECTORY launch/ 22 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 23 | ) 24 | 25 | catkin_install_python( 26 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 28 | ) 29 | -------------------------------------------------------------------------------- /rosserial_vex_v5/launch/hello_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /rosserial_vex_v5/launch/joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rosserial_vex_v5/launch/minimal_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /rosserial_vex_v5/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_vex_v5 3 | 0.9.2 4 | 5 | rosserial for the VEX Cortex V5 Robot Brain platform. 6 | 7 | Cannon 8 | Canyon Turtle 9 | BSD 10 | 11 | 12 | catkin 13 | 14 | rospy 15 | rosserial_client 16 | rosserial_python 17 | 18 | -------------------------------------------------------------------------------- /rosserial_vex_v5/scripts/genproject.sh: -------------------------------------------------------------------------------- 1 | # generate new pros project with name as first argument 2 | prosv5 conductor new-project $1 3 | 4 | # go into incude directory and generate ros message library. 5 | cd $1/include 6 | rosrun rosserial_vex_v5 make_libraries.py . 7 | 8 | # unless the include is a special case 9 | # (e.g. the include is a std library, or ros_lib is already in the path), 10 | # add ros_lib to the start of the include. 11 | grep -irl '#include' ros_lib/ | xargs perl -pi \ 12 | -e 's/^#include "(?!main)(?!pros)(?!stddef)(?!stdlib)(?!string)(?!stdint)(?!math)(?!ros_lib)/#include "ros_lib\//g' \ 13 | 14 | # make directories for all the cpp files. 15 | 16 | # move the cpp files into their proper places. 17 | mv ros_lib/rosserial_vex_v5/examples/*.cpp ../src/ 18 | mv ros_lib/*.cpp ../src/ 19 | cd .. 20 | 21 | # delete the incorrect files in src 22 | rm src/*.c 23 | -------------------------------------------------------------------------------- /rosserial_vex_v5/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosserial_vex_v5'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/autonomous.cpp: -------------------------------------------------------------------------------- 1 | #include "main.h" 2 | 3 | // comp. control has not been fully implemented yet, so 4 | // this function will never be called 5 | 6 | void autonomous() {} 7 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/initialize.cpp: -------------------------------------------------------------------------------- 1 | #include "main.h" 2 | 3 | void on_center_button() { 4 | } 5 | 6 | void initialize() { 7 | } 8 | 9 | // the following functions don't work presently because comp. control 10 | // hasn't been fully implemented 11 | void disabled() {} 12 | void competition_initialize() {} 13 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/opcontrol.cpp: -------------------------------------------------------------------------------- 1 | /** @file opcontrol.cpp 2 | * @brief File for operator control code 3 | * 4 | * This file should contain the user operatorControl() function and any functions related to it. 5 | * 6 | * Any copyright is dedicated to the Public Domain. 7 | * http://creativecommons.org/publicdomain/zero/1.0/ 8 | * 9 | * PROS contains FreeRTOS (http://www.freertos.org) whose source code may be 10 | * obtained from http://sourceforge.net/projects/freertos/files/ or on request. 11 | */ 12 | 13 | #include "main.h" 14 | #include "ros_lib/rosserial_vex_v5/examples/helloworld.h" 15 | 16 | // entry point of user control period. 17 | void opcontrol() { 18 | 19 | //running the hello world example 20 | setup(); 21 | 22 | while(1) pros::c::delay(20); 23 | } 24 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROSSERIAL_VEX_CORTEX_ROS_H_ 36 | #define _ROSSERIAL_VEX_CORTEX_ROS_H_ 37 | 38 | #include "ros_lib/ros/node_handle.h" 39 | 40 | #include "ros_lib/V5Hardware.h" 41 | 42 | namespace ros { 43 | typedef NodeHandle_ NodeHandle; 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/rosserial_vex_v5/examples/helloworld.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example for VEX Cortex 3 | * Prints "hello world!" 4 | */ 5 | 6 | #include "ros_lib/ros.h" 7 | #include "ros_lib/std_msgs/String.h" 8 | 9 | // this loop is run in setup function, which publishes at 50hz. 10 | void loop(ros::NodeHandle & nh, ros::Publisher & p, std_msgs::String & str_msg, char* msgdata) 11 | { 12 | str_msg.data = msgdata; 13 | p.publish( &str_msg ); 14 | nh.spinOnce(); 15 | pros::c::delay(20); 16 | } 17 | 18 | // The setup function will start a publisher on the topic "chatter" and begin publishing there. 19 | void setup() 20 | { 21 | // debug logging 22 | // make a node handle object, string message, and publisher for that message. 23 | ros::NodeHandle nh; 24 | std_msgs::String str_msg; 25 | ros::Publisher chatter("chatter\0", &str_msg); 26 | 27 | // set up rosserial, and prepare to publish the chatter message 28 | nh.initNode(); 29 | nh.advertise(chatter); 30 | 31 | // message data variable. 32 | char* msg = (char*) malloc(20 * sizeof(char)); 33 | while (1) { 34 | 35 | // send a message about the time! 36 | sprintf(msg, "[%d] Hello there!!", (int) pros::c::millis()); 37 | loop(nh, chatter, str_msg, msg); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/rosserial_vex_v5/examples/helloworld.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROSSERIAL_VEX_V5_HELLO_WORLD_H_ 2 | #define _ROSSERIAL_VEX_V5_HELLO_WORLD_H_ 3 | 4 | void setup(); 5 | 6 | #endif 7 | -------------------------------------------------------------------------------- /rosserial_vex_v5/src/ros_lib/rosserial_vex_v5/utils/RingBufHelpers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2015 D. Aaron Wisner 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | 25 | #ifndef EM_RINGBUF_HELPERS_CPP_H 26 | #define EM_RINGBUF_HELPERS_CPP_H 27 | 28 | // TODO fix this 29 | #ifndef NULL 30 | #define NULL (void *)(0) 31 | #endif 32 | 33 | #define RB_ATOMIC_START if(1/*rosvexMutex.take(20)*/) { 34 | #define RB_ATOMIC_END rosvexMutex.give(); } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /rosserial_vex_v5/uartdiagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_vex_v5/uartdiagram.png -------------------------------------------------------------------------------- /rosserial_windows/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_windows 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Use os.path.join for path concatenation (`#495 `_) 14 | * Fix Travis for Noetic + Python 3 15 | * Bump minimum CMake version to 3.7.2 (Melodic). 16 | * Fix py3 print usages and trailing whitespaces (`#469 `_) 17 | * Contributors: Hermann von Kleist, Mike Purvis, acxz 18 | 19 | 0.8.0 (2018-10-11) 20 | ------------------ 21 | 22 | 0.7.7 (2017-11-29) 23 | ------------------ 24 | * Fix catkin lint errors (`#296 `_) 25 | * Contributors: Bei Chen Liu 26 | 27 | 0.7.6 (2017-03-01) 28 | ------------------ 29 | 30 | 0.7.5 (2016-11-22) 31 | ------------------ 32 | 33 | 0.7.4 (2016-09-21) 34 | ------------------ 35 | 36 | 0.7.3 (2016-08-05) 37 | ------------------ 38 | 39 | 0.7.2 (2016-07-15) 40 | ------------------ 41 | 42 | 0.7.1 (2015-07-06) 43 | ------------------ 44 | 45 | 0.7.0 (2015-04-23) 46 | ------------------ 47 | 48 | 0.6.3 (2014-11-05) 49 | ------------------ 50 | 51 | 0.6.2 (2014-09-10) 52 | ------------------ 53 | 54 | 0.6.1 (2014-06-30) 55 | ------------------ 56 | 57 | 0.6.0 (2014-06-11) 58 | ------------------ 59 | 60 | 0.5.6 (2014-05-07) 61 | ------------------ 62 | * first created by Kareem Shehata 63 | -------------------------------------------------------------------------------- /rosserial_windows/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_windows) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | nav_msgs 7 | rosserial_client 8 | sensor_msgs 9 | std_msgs 10 | ) 11 | catkin_package(CATKIN_DEPENDS) 12 | 13 | install( 14 | DIRECTORY src/ros_lib 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 16 | ) 17 | 18 | catkin_install_python( 19 | PROGRAMS src/${PROJECT_NAME}/make_libraries.py 20 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 21 | ) 22 | -------------------------------------------------------------------------------- /rosserial_windows/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_windows 3 | 0.9.2 4 | 5 | rosserial for Windows platforms. 6 | 7 | Kareem Shehata 8 | Kareem Shehata 9 | BSD 10 | http://ros.org/wiki/rosserial_windows 11 | 12 | catkin 13 | 14 | std_msgs 15 | sensor_msgs 16 | geometry_msgs 17 | nav_msgs 18 | rosserial_client 19 | 20 | rospy 21 | rosserial_msgs 22 | rosserial_client 23 | message_runtime 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rosserial_windows/src/examples/TestDrive/readme.md: -------------------------------------------------------------------------------- 1 | rosserial windows TestDrive 2 | =========================== 3 | 4 | This is an example of how to use rosserial_windows to send commands from a 5 | windows app to a robot. All it does is send a cmd_vel Twist to the robot 6 | with a static set of values. This can be updated to change the values based on 7 | joystick or keyboard inputs, or to send other messages. 8 | 9 | For a full tutorial see http://wiki.ros.org/rosserial_windows/Tutorials/Hello%20World 10 | 11 | To use the example: 12 | 13 | 1. Create a new visual studio command line app project 14 | 2. Copy the ros_lib generated by rosserial_windows into your project 15 | 3. Copy the testdrive.cpp instead of your main cpp file 16 | 4. Add the .h and .cpp files from the *top level* of the ros_lib to your VS project (should only be WindowsSocket.h WindowsSocket.cpp duration.cpp time.cpp and ros.h) 17 | 5. Compile the project 18 | 6. Run the exe with the command line argument of the IP address of your robot 19 | 7. Watch your robot spin around! 20 | 21 | Some tips: 22 | 23 | * Do not enable unicode support. If your command line arguments are not working correctly, try turning off unicode as the character set. 24 | * Turn off precompiled headers or WindowsSocket will not compile properly. 25 | -------------------------------------------------------------------------------- /rosserial_windows/src/ros_lib/WindowsSocket.h: -------------------------------------------------------------------------------- 1 | /** 2 | Software License Agreement (BSD) 3 | 4 | \file WindowsSocket.h 5 | \authors Kareem Shehata 6 | \copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 9 | the following conditions are met: 10 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 11 | following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 13 | following disclaimer in the documentation and/or other materials provided with the distribution. 14 | * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 15 | products derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 18 | RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 19 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 20 | DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #ifndef ROS_WINDOWS_SOCKET_H_ 27 | #define ROS_WINDOWS_SOCKET_H_ 28 | 29 | // forward declaration of the implementation class 30 | // this class is defined in the implementation file to abstract all of the 31 | // windows specific crud. It gets in the way of the ROS libraries. 32 | class WindowsSocketImpl; 33 | 34 | class WindowsSocket 35 | { 36 | public: 37 | WindowsSocket (); 38 | 39 | void init (char *server_hostname); 40 | 41 | int read (); 42 | 43 | void write (const unsigned char *data, int length); 44 | 45 | unsigned long time (); 46 | 47 | private: 48 | WindowsSocketImpl * impl; 49 | }; 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /rosserial_windows/src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /** 2 | Software License Agreement (BSD) 3 | 4 | \file ros.h 5 | \authors Kareem Shehata 6 | \copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 9 | the following conditions are met: 10 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 11 | following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 13 | following disclaimer in the documentation and/or other materials provided with the distribution. 14 | * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 15 | products derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 18 | RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 19 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 20 | DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #ifndef _ROS_H_ 27 | #define _ROS_H_ 28 | 29 | #include "WindowsSocket.h" 30 | #include "ros/node_handle.h" 31 | 32 | namespace ros 33 | { 34 | typedef NodeHandle_ NodeHandle; 35 | } 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /rosserial_xbee/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_xbee 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.2 (2021-04-02) 6 | ------------------ 7 | 8 | 0.9.1 (2020-09-09) 9 | ------------------ 10 | 11 | 0.9.0 (2020-08-25) 12 | ------------------ 13 | * Fix Travis for Noetic + Python 3 14 | * Bump minimum CMake version to 3.7.2 (Melodic). 15 | * Update pyserial rosdep. 16 | * Fix py3 print usages and trailing whitespaces (`#469 `_) 17 | * Contributors: Mike Purvis, acxz 18 | 19 | 0.8.0 (2018-10-11) 20 | ------------------ 21 | 22 | 0.7.7 (2017-11-29) 23 | ------------------ 24 | * Fix catkin lint errors (`#296 `_) 25 | * Contributors: Bei Chen Liu 26 | 27 | 0.7.6 (2017-03-01) 28 | ------------------ 29 | 30 | 0.7.5 (2016-11-22) 31 | ------------------ 32 | 33 | 0.7.4 (2016-09-21) 34 | ------------------ 35 | 36 | 0.7.3 (2016-08-05) 37 | ------------------ 38 | 39 | 0.7.2 (2016-07-15) 40 | ------------------ 41 | * Adding inWaiting method to FakeSerial class. Fixing issue `#179 `_ 42 | * Contributors: Lucas 43 | 44 | 0.7.1 (2015-07-06) 45 | ------------------ 46 | 47 | 0.7.0 (2015-04-23) 48 | ------------------ 49 | 50 | 0.6.3 (2014-11-05) 51 | ------------------ 52 | 53 | 0.6.2 (2014-09-10) 54 | ------------------ 55 | 56 | 0.6.1 (2014-06-30) 57 | ------------------ 58 | 59 | 0.6.0 (2014-06-11) 60 | ------------------ 61 | 62 | 0.5.6 (2014-06-11) 63 | ------------------ 64 | 65 | 0.5.5 (2014-01-14) 66 | ------------------ 67 | 68 | 0.5.4 (2013-10-17) 69 | ------------------ 70 | 71 | 0.5.3 (2013-09-21) 72 | ------------------ 73 | 74 | 0.5.2 (2013-07-17) 75 | ------------------ 76 | 77 | * Fix release version 78 | 79 | 0.5.1 (2013-07-15) 80 | ------------------ 81 | * manually merge `#53 `_, revive catkinized rosserial_xbee 82 | 83 | 0.4.5 (2013-07-02) 84 | ------------------ 85 | 86 | 0.4.4 (2013-03-20) 87 | ------------------ 88 | 89 | 0.4.3 (2013-03-13 14:08) 90 | ------------------------ 91 | 92 | 0.4.2 (2013-03-13 01:15) 93 | ------------------------ 94 | 95 | 0.4.1 (2013-03-09) 96 | ------------------ 97 | 98 | 0.4.0 (2013-03-08) 99 | ------------------ 100 | * moving rosserial_xbee to rosserial-experimental repo 101 | -------------------------------------------------------------------------------- /rosserial_xbee/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7.2) 2 | project(rosserial_xbee) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | catkin_python_setup() 8 | 9 | install( 10 | PROGRAMS scripts/setup_xbee.py 11 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 12 | ) 13 | 14 | install( 15 | PROGRAMS scripts/xbee_network.py 16 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 17 | ) 18 | -------------------------------------------------------------------------------- /rosserial_xbee/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_xbee 3 | 0.9.2 4 | 5 | Allows multipoint communication between rosserial 6 | nodes connected to an xbee. All nodes communicate back 7 | to a master xbee connected to a computer running ROS. 8 | 9 | This software currently only works with Series 1 Xbees. 10 | 11 | This pkg includes python code from the python-xbee project: 12 | http://code.google.com/p/python-xbee/ 13 | 14 | Adam Stambler 15 | Paul Bouchier 16 | BSD 17 | http://ros.org/wiki/rosserial_xbee 18 | 19 | catkin 20 | 21 | rospy 22 | rosserial_msgs 23 | diagnostic_msgs 24 | python3-serial 25 | rosserial_python 26 | 27 | 28 | -------------------------------------------------------------------------------- /rosserial_xbee/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosserial_xbee','xbee'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /rosserial_xbee/src/rosserial_xbee/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_xbee/src/rosserial_xbee/__init__.py -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/LICENSE.txt: -------------------------------------------------------------------------------- 1 | The MIT License 2 | 3 | Copyright (c) 2010 Paul Malmsten, Greg Rapp, Brian, Amit Synderman, Marco Sangalli 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | eHE SOFTWARE. 22 | -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/README: -------------------------------------------------------------------------------- 1 | Source Code contained in this folder is based upon the python-xbee 2 | project. 3 | 4 | http://code.google.com/p/python-xbee/ 5 | -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | XBee package initalization file 3 | 4 | By Paul Malmsten, 2010 5 | pmalmsten@gmail.com 6 | """ 7 | 8 | from xbee.ieee import XBee 9 | from xbee.zigbee import ZigBee 10 | -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/helpers/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_xbee/src/xbee/helpers/__init__.py -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/helpers/dispatch/__init__.py: -------------------------------------------------------------------------------- 1 | from xbee.helpers.dispatch.dispatch import Dispatch 2 | -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/helpers/dispatch/tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_xbee/src/xbee/helpers/dispatch/tests/__init__.py -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/helpers/dispatch/tests/fake.py: -------------------------------------------------------------------------------- 1 | """ 2 | fake.py 3 | 4 | By Paul Malmsten, 2010 5 | pmalmsten@gmail.com 6 | 7 | Provides fake objects for testing the dispatch package. 8 | """ 9 | 10 | class FakeXBee(object): 11 | """ 12 | Represents an XBee device from which data can be read. 13 | """ 14 | def __init__(self, data): 15 | self.data = data 16 | 17 | def wait_read_frame(self): 18 | return self.data 19 | -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/tests/Fake.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | """ 3 | Fake.py 4 | 5 | By Paul Malmsten, 2010 6 | pmalmsten@gmail.com 7 | 8 | Provides fake device objects for other unit tests. 9 | """ 10 | import sys 11 | 12 | class FakeDevice: 13 | """ 14 | Represents a fake serial port for testing purposes 15 | """ 16 | def __init__(self): 17 | self.data = '' 18 | 19 | def write(self, data): 20 | """ 21 | Writes data to the fake port for later evaluation 22 | """ 23 | self.data = data 24 | 25 | class FakeReadDevice: 26 | """ 27 | Represents a fake serial port which can be read from in a similar 28 | fashion to the real thing 29 | """ 30 | 31 | def __init__(self, data, silent_on_empty=False): 32 | self.data = data 33 | self.read_index = 0 34 | self.silent_on_empty = silent_on_empty 35 | 36 | def read(self, length=1): 37 | """ 38 | Read the indicated number of bytes from the port 39 | """ 40 | # If too many bytes would be read, raise exception 41 | if self.read_index + length > len(self.data): 42 | if self.silent_on_empty: 43 | sys.exit(0) 44 | else: 45 | raise ValueError("Not enough bytes exist!") 46 | 47 | read_data = self.data[self.read_index:self.read_index + length] 48 | self.read_index += length 49 | 50 | return read_data 51 | 52 | def inWaiting(self): 53 | """ 54 | Returns the number of bytes available to be read 55 | """ 56 | return len(self.data) - self.read_index 57 | -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/rosserial/c169ae2173dcfda7cee567d64beae45198459400/rosserial_xbee/src/xbee/tests/__init__.py -------------------------------------------------------------------------------- /rosserial_xbee/src/xbee/tests/test_fake.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | """ 3 | test_xbee.py 4 | 5 | By Paul Malmsten, 2010 6 | pmalmsten@gmail.com 7 | 8 | Tests fake device objects for proper functionality. 9 | """ 10 | import unittest 11 | from xbee.tests.Fake import FakeReadDevice 12 | 13 | class TestFakeReadDevice(unittest.TestCase): 14 | """ 15 | FakeReadDevice class should work as intended to emluate a serial 16 | port 17 | """ 18 | def setUp(self): 19 | """ 20 | Create a fake read device for each test 21 | """ 22 | self.device = FakeReadDevice("test") 23 | 24 | def test_read_single_byte(self): 25 | """ 26 | reading one byte at a time should work as expected 27 | """ 28 | self.assertEqual(self.device.read(), 't') 29 | self.assertEqual(self.device.read(), 'e') 30 | self.assertEqual(self.device.read(), 's') 31 | self.assertEqual(self.device.read(), 't') 32 | 33 | def test_read_multiple_bytes(self): 34 | """ 35 | reading multiple bytes at a time should work as expected 36 | """ 37 | self.assertEqual(self.device.read(3), 'tes') 38 | self.assertEqual(self.device.read(), 't') 39 | 40 | def test_read_too_many(self): 41 | """ 42 | attempting to read too many bytes should raise an exception 43 | """ 44 | self.assertRaises(ValueError, self.device.read, 5) 45 | --------------------------------------------------------------------------------