Create openadx demo

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
+```

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()

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()

openadx_demo/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>openadx_demo</name>
+  <version>0.0.1</version>
+  <description>OpenADX Hackathon demo</description>
+  <maintainer email="lietzau@tesis.de">Stefan Lietzau</maintainer>
+  <license>Eclipse Public License - v 2.0</license>
+  <author email="lietzau@tesis.de">Stefan Lietzau</author>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>openadx_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

openadx_demo/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/openadx_demo
+[install]
+install-scripts=$base/lib/openadx_demo

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',
+        ],
+    },
+)

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()

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

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

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

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

openadx_msgs/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>openadx_msgs</name>
+  <version>0.0.1</version>
+  <description>Messages used by OpenADX</description>
+  <maintainer email="maintainer@example.com">Maintainer</maintainer>
+  <license>Eclipse Public License - v 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>