Add new node for perception_genie

LICENSE

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2021 LGE-ROS2
+Copyright (c) 2020 LG Electronics.
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

README.md

@@ -1 +1,22 @@
-# genie_perception
+# perception_genie
+
+This node will subscribe '/ground_truth' topic which publish absolute obejct information in world.
+
+Please run below command to make a relative information around target centered object.
+
+If target object name is 'cloi1' and tracking id is '1', give an arguments as ros-args like below.
+
+```shell
+ros2 run perception_genie ground_truth --ros-args -r __ns:=/cloi1 -p tracking_id:=1
+```
+
+Then it will publish the data on  '/cloi1/ground_truth' topic.
+
+The publishing data is very simple.
+
+```python
+target_object.position - other_object.position
+target_object.velocity - other_object.velocity
+```
+
+That's it.

package.xml

@@ -0,0 +1,25 @@
+<?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>perception_genie</name>
+  <version>0.1.0</version>
+
+  <description>Perception genie</description>
+  <maintainer email="[email protected]">Hyunseok Yang</maintainer>
+
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

perception_genie/perception_ground_truth.py

@@ -0,0 +1,94 @@
+
+import rclpy
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+
+from std_msgs.msg import Header
+from perception_msgs.msg import ObjectArray, ObjectInfo
+from geometry_msgs.msg import Vector3, Polygon
+
+class PerceptionGroundTruth(Node):
+
+    def __init__(self):
+        super().__init__('perception_ground_truth')
+
+        self.declare_parameter('tracking_id', -1)
+        self.tracking_id_ = self.get_parameter('tracking_id').value
+
+        self.namespace_ = self.get_namespace() if not self.get_namespace().__eq__("/") else "need_to_add_namespace"
+
+        self.get_logger().info("namespace: %s, target tracking_id: %d" % (self.get_namespace(), self.tracking_id_))
+
+        self.subscription = self.create_subscription(
+            ObjectArray,
+            '/ground_truth',
+            self.listener_callback,
+            5)
+
+        self.publisher = self.create_publisher(
+            ObjectArray,
+            self.namespace_ + '/ground_truth',
+            10
+            )
+
+
+    def listener_callback(self, msg):
+
+        pub_msg = ObjectArray()
+        pub_msg.header = msg.header
+
+        # msg_header_time = msg.header.stamp
+        # self.get_logger().info('Header Time: %d.%d' % (msg_header_time.sec, msg_header_time.nanosec))
+
+        # find target tracking id
+        # output = [idx for idx, element in enumerate(msg.objects) if element.tracking_id == self.tracking_id_]
+        # res = [x for i, item in enumerate(msg.objects) if x.tracking_id == self.tracking_id_]
+
+        # output = (element.tracking_id == self.tracking_id_ for element in msg.objects)
+
+        filtered_target_object = list(filter(lambda x: x.tracking_id == self.tracking_id_, msg.objects));
+        target_object = None if len(filtered_target_object) == 0 else filtered_target_object[0]
+        # self.get_logger().info('result = %d' % (target_object.tracking_id))
+
+        if target_object is not None :
+
+            for objectinfo in msg.objects:
+
+                if objectinfo.tracking_id != self.tracking_id_ :
+                    # objectinfo_header_time = objectinfo.header.stamp;
+                    # self.get_logger().info('====================================')
+                    # self.get_logger().info('object header Time: %d.%d' % (objectinfo_header_time.sec, objectinfo_header_time.nanosec))
+                    # self.get_logger().info('tracking id: %d' % objectinfo.tracking_id)
+                    # self.get_logger().info('class id: %d' % objectinfo.class_id)
+                    # self.get_logger().info('position: "x:%f" y:%f z:%f"' % (objectinfo.position.x, objectinfo.position.y, objectinfo.position.z))
+                    # self.get_logger().info('velocity: "x:%f" y:%f z:%f"' % (objectinfo.velocity.x, objectinfo.velocity.y, objectinfo.velocity.z))
+                    # self.get_logger().info('size: "x:%f y:%f" z:%f"' % (objectinfo.size.x, objectinfo.size.y, objectinfo.size.z))
+                    # self.get_logger().info('footprint: %d' % len(objectinfo.footprint.points))
+                    new_object_relative_info = objectinfo
+
+                    new_object_relative_info.position.x = target_object.position.x - new_object_relative_info.position.x
+                    new_object_relative_info.position.y = target_object.position.y - new_object_relative_info.position.y
+                    new_object_relative_info.position.z = target_object.position.z - new_object_relative_info.position.z
+
+                    new_object_relative_info.velocity.x = target_object.velocity.x - new_object_relative_info.velocity.x
+                    new_object_relative_info.velocity.y = target_object.velocity.y - new_object_relative_info.velocity.y
+                    new_object_relative_info.velocity.z = target_object.velocity.z - new_object_relative_info.velocity.z
+
+                    pub_msg.objects.append(new_object_relative_info)
+
+
+        if len(pub_msg.objects) > 0:
+            self.publisher.publish(pub_msg)
+
+
+def main(args=None):
+    print('Hi, this is perception_genie!!!')
+    rclpy.init(args=args)
+    perception_ground_truth_node = PerceptionGroundTruth()
+    rclpy.spin(perception_ground_truth_node)
+    perception_ground_truth_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

setup.cfg

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

setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'perception_genie'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml'])
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Hyunseok Yang',
+    maintainer_email='[email protected]',
+    description='publish groundtruth relative info ',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'ground_truth = perception_genie.perception_ground_truth:main'
+        ],
+    },
+)

test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'