diff --git a/README.MD b/README.MD index de8e341..90c9720 100644 --- a/README.MD +++ b/README.MD @@ -3,3 +3,20 @@ This project contains the code needed to build a simulation demonstrator by the OpenADx eco-system. The main content is glue that is used to combine the different tools by participating companies. The work is done in the context of the Simulation Testbed of the OpenADx initiative More information on this testbed, see: https://wiki.eclipse.org/OpenADx + + +## OpenADX Demo example: +Start the demo sender with +``` +ros2 run openadx_demo demo_sender +``` + +Start the demo brake assistant with +``` +ros2 run openadx_demo demo +``` + +To view control messages sent by the demo brake assistant run the following command: +``` +ros2 topic echo /vehicle/control +``` diff --git a/openadx_demo/openadx_demo/__init__.py b/openadx_demo/openadx_demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/openadx_demo/openadx_demo/topics/__init__.py b/openadx_demo/openadx_demo/topics/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/openadx_demo/openadx_demo/topics/demo.py b/openadx_demo/openadx_demo/topics/demo.py new file mode 100644 index 0000000..8652fbf --- /dev/null +++ b/openadx_demo/openadx_demo/topics/demo.py @@ -0,0 +1,59 @@ +import sys + +import rclpy +from rclpy.node import Node + +#from std_msgs.msg import String +from openadx_msgs.msg import (DetectedObjectList, VehicleControl) + +LANE_WIDTH = 2 +BRAKE_DISTANCE = 30 + + +class DemoNode(Node): + def __init__(self): + super().__init__('demo') + self.sub = self.create_subscription( + DetectedObjectList, 'vehicle/objects', self.object_callback) + self.control = self.create_publisher(VehicleControl, 'vehicle/control') + + def object_callback(self, msg): + log = self.get_logger().info + log('Received a message') + log('ID: {}'.format(msg.id)) + log('length: {}'.format(len(msg.objects))) + for obj in msg.objects: + log('{}'.format(obj)) + if self.object_in_path(obj): + self.send_brake_message() + break + + def send_brake_message(self): + self.control.publish( + VehicleControl( + accelerator_pedal=0.0, + brake_pedal=1.0, + steering_wheel=0.0, + gear=VehicleControl.GEAR_KEEP)) + + def object_in_path(self, obj): + if (abs(obj.y) < LANE_WIDTH) and (obj.x < BRAKE_DISTANCE): + return True + return False + + +def main(args=None): + if args is None: + args = sys.argv + + rclpy.init(args=args) + + node = DemoNode() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/openadx_demo/openadx_demo/topics/sender.py b/openadx_demo/openadx_demo/topics/sender.py new file mode 100644 index 0000000..77225a8 --- /dev/null +++ b/openadx_demo/openadx_demo/topics/sender.py @@ -0,0 +1,61 @@ +import sys + +import rclpy +from rclpy.node import Node + +import time +import random + +#from std_msgs.msg import String +from openadx_msgs.msg import (DetectedObject, DetectedObjectList) + + +class DemoSender(Node): + def __init__(self): + super().__init__('demo') + self.pub = self.create_publisher(DetectedObjectList, 'vehicle/objects') + self.counter = 1 + self.tmr = self.create_timer(1.0, self.sender_callback) + + def sender_callback(self): + t = time.time() + sec = int(t) + nanosec = int((t - sec) * 1e9) + msg = DetectedObjectList() + msg.id = self.counter + msg.header.stamp.sec = sec + msg.header.stamp.nanosec = nanosec + msg.header.frame_id = 'world' + msg.objects = [self.get_object() for _ in range(5)] + + self.get_logger().info("Publishing message {}".format(msg.id)) + self.counter += 1 + self.pub.publish(msg) + + def get_object(self): + x = 5 + 50 * random.random() + y = 10 - 20 * random.random() + obj = DetectedObject(id=42, x=x, y=y, v_rad=0.0, a_rad=0.0) + # obj.id = 42 + # obj.x = 1. + # obj.y = 2. + # obj.v_rad = 0.0 + # obj.a_rad = 0.0 + return obj + + +def main(args=None): + if args is None: + args = sys.argv + + rclpy.init(args=args) + + node = DemoSender() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/openadx_demo/package.xml b/openadx_demo/package.xml new file mode 100644 index 0000000..eb5e102 --- /dev/null +++ b/openadx_demo/package.xml @@ -0,0 +1,18 @@ + + + + openadx_demo + 0.0.1 + OpenADX Hackathon demo + Stefan Lietzau + Eclipse Public License - v 2.0 + Stefan Lietzau + + rclpy + openadx_msgs + std_msgs + + + ament_python + + diff --git a/openadx_demo/resource/openadx_demo b/openadx_demo/resource/openadx_demo new file mode 100644 index 0000000..e69de29 diff --git a/openadx_demo/setup.cfg b/openadx_demo/setup.cfg new file mode 100644 index 0000000..cbf448b --- /dev/null +++ b/openadx_demo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/openadx_demo +[install] +install-scripts=$base/lib/openadx_demo diff --git a/openadx_demo/setup.py b/openadx_demo/setup.py new file mode 100644 index 0000000..1ed7a5f --- /dev/null +++ b/openadx_demo/setup.py @@ -0,0 +1,38 @@ +from setuptools import find_packages +from setuptools import setup + +package_name = 'openadx_demo' + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='Stefan Lietzau', + author_email='lietzau@tesis.de', + maintainer='Stefan Lietzau', + maintainer_email='lietzau@tesis.de', + keywords=['ROS2'], + classifiers=[ + 'Intended Audience :: Developers', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description=( + 'Demo Node to showcase for OpenADX hackathon' + ), + license='fill in later', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'demo = openadx_demo.topics.demo:main', + 'demo_sender = openadx_demo.topics.sender:main', + ], + }, +) diff --git a/openadx_msgs/CMakeLists.txt b/openadx_msgs/CMakeLists.txt new file mode 100644 index 0000000..98d52ce --- /dev/null +++ b/openadx_msgs/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) + +project(openadx_msgs) + +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wextra") +endif() + +find_package(ament_cmake REQUIRED) +# find the ROS message code generators +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +# declare the message files to generate code for +set(msg_files + "msg/VehicleControl.msg" + "msg/VehicleState.msg" + "msg/DetectedObject.msg" + "msg/DetectedObjectList.msg" + ) + +# declare the service files to generate code for +set(srv_files + ) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES builtin_interfaces geometry_msgs + ) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/openadx_msgs/msg/DetectedObject.msg b/openadx_msgs/msg/DetectedObject.msg new file mode 100644 index 0000000..4a03134 --- /dev/null +++ b/openadx_msgs/msg/DetectedObject.msg @@ -0,0 +1,21 @@ +################################################################################ +# Copyright (c) 2018 TESIS GmbH +# All rights reserved. This program and the accompanying materials +# are made available under the terms of the Eclipse Public License v2.0 +# which accompanies this distribution, and is available at +# http://www.eclipse.org/legal/epl-v20.html +# +# Contributors: +# Frederic Chucholowski (TESIS GmbH) - initial implementation +############################################################################### + +# ID of the detected object +int32 id +# longitudinal distance of the object to the sensor (sensor system) +float32 x +# lateral distance of the object to the sensor (sensor system) +float32 y +# radial velocity of the object relative to the sensor +float32 v_rad +# radial acceleration of the object relative to the sensor +float32 a_rad diff --git a/openadx_msgs/msg/DetectedObjectList.msg b/openadx_msgs/msg/DetectedObjectList.msg new file mode 100644 index 0000000..a2a6964 --- /dev/null +++ b/openadx_msgs/msg/DetectedObjectList.msg @@ -0,0 +1,16 @@ +################################################################################ +# Copyright (c) 2018 TESIS GmbH +# All rights reserved. This program and the accompanying materials +# are made available under the terms of the Eclipse Public License v2.0 +# which accompanies this distribution, and is available at +# http://www.eclipse.org/legal/epl-v20.html +# +# Contributors: +# Frederic Chucholowski (TESIS GmbH) - initial implementation +############################################################################### + +std_msgs/Header header +# ID of the sensor +int16 id +# list of detected objects +DetectedObject[] objects diff --git a/openadx_msgs/msg/VehicleControl.msg b/openadx_msgs/msg/VehicleControl.msg new file mode 100644 index 0000000..0b0c32c --- /dev/null +++ b/openadx_msgs/msg/VehicleControl.msg @@ -0,0 +1,35 @@ +############################################################################### +# Copyright (c) 2018 TESIS GmbH +# All rights reserved. This program and the accompanying materials +# are made available under the terms of the Eclipse Public License v2.0 +# which accompanies this distribution, and is available at +# http://www.eclipse.org/legal/epl-v20.html +# +# Contributors: +# Frederic Chucholowski (TESIS GmbH) - initial implementation +# Stefan Lietzau (TESIS GmbH) - Create ROS2 compatible package +############################################################################### + +# ############################################################################### +# constants + +# don't change the gear +int32 GEAR_KEEP=-1 +# parking gear +int32 GEAR_PARK=0 +# neutral gear +int32 GEAR_NEUTRAL=1 +# drive gear +int32 GEAR_DRIVE=2 +# reversing gear +int32 GEAR_REVERSE=3 + + +# intended accelerator pedal position [0, 1] +float64 accelerator_pedal +# intended brake pedal position [0, 1] +float64 brake_pedal +# intended steering wheel position [-1, 1] +float64 steering_wheel +# intended gear +int32 gear diff --git a/openadx_msgs/msg/VehicleState.msg b/openadx_msgs/msg/VehicleState.msg new file mode 100644 index 0000000..2179d48 --- /dev/null +++ b/openadx_msgs/msg/VehicleState.msg @@ -0,0 +1,20 @@ +############################################################################### +# Copyright (c) 2018 TESIS GmbH +# All rights reserved. This program and the accompanying materials +# are made available under the terms of the Eclipse Public License v2.0 +# which accompanies this distribution, and is available at +# http://www.eclipse.org/legal/epl-v20.html +# +# Contributors: +# Frederic Chucholowski (TESIS GmbH) - initial implementation +# Stefan Lietzau (TESIS GmbH) - Create ROS2 compatible package +############################################################################### + +# Position and Orientation in space +geometry_msgs/Pose position + +# the current velocity (linear and angular) with respect to the world system in vehicle coordinates +geometry_msgs/Twist twist + +# the current acceleration (linear and angular) with respect to the world system in vehicle coordinates +geometry_msgs/Accel acceleration diff --git a/openadx_msgs/package.xml b/openadx_msgs/package.xml new file mode 100644 index 0000000..d5e5d6c --- /dev/null +++ b/openadx_msgs/package.xml @@ -0,0 +1,27 @@ + + + + openadx_msgs + 0.0.1 + Messages used by OpenADX + Maintainer + Eclipse Public License - v 2.0 + + ament_cmake + + rosidl_default_generators + + builtin_interfaces + geometry_msgs + std_msgs + + rosidl_default_runtime + builtin_interfaces + geometry_msgs + std_msgs + + rosidl_interface_packages + + ament_cmake + +