Imprement vision wrapper

consai2r2_msgs/CMakeLists.txt

@@ -29,6 +29,8 @@ set(msg_files
   "msg/DetectionRobot.msg"
   "msg/DetectionFrame.msg"
   "msg/VisionDetections.msg"
+  "msg/RobotInfo.msg"
+  "msg/BallInfo.msg"
   "msg/RobotCommand.msg"
   "msg/RobotCommands.msg"
 )

consai2r2_msgs/msg/BallInfo.msg

@@ -0,0 +1,10 @@
+# ball information for consai2_game
+
+geometry_msgs/Pose2D pose
+geometry_msgs/Pose2D velocity
+geometry_msgs/Twist  velocity_twist
+bool                 detected
+builtin_interfaces/Time detection_stamp
+
+geometry_msgs/Pose2D last_detection_pose
+bool                 disappeared

consai2r2_msgs/msg/RobotInfo.msg

@@ -0,0 +1,29 @@
+# robot information for consai2_game
+
+# robot id
+uint8                robot_id
+
+# Robot 2D pose
+# This field will be replaced to geometry_msgs/Pose 
+geometry_msgs/Pose2D pose
+
+# Robot Velocity
+# [Deprecated] This field will be removed. 
+geometry_msgs/Pose2D velocity
+
+# Robot Velocity
+geometry_msgs/Twist  velocity_twist
+
+# If robot is detected by vision in this frame, this field is set to true
+bool                 detected
+
+# Latest vision detection time
+# [Deprecated] This field will be removed. 
+builtin_interfaces/Time detection_stamp
+
+#  Latest detected pose.
+# This field is NOT filtered. This is a raw vision value.
+geometry_msgs/Pose2D last_detection_pose
+
+# If a robot disappear from field,  this field is set to true
+bool                 disappeared

consai2r2_vision_wrapper/consai2r2_vision_wrapper/vision_wrapper.py

@@ -0,0 +1,268 @@
+# Copyright (c) 2019 SSL-Roots
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import math
+
+from consai2r2_description import consai2r2_parameters
+
+from consai2r2_msgs.msg import BallInfo, RobotInfo, VisionDetections
+from geometry_msgs.msg import Pose2D
+
+import rclpy
+from rclpy.clock import Clock, ClockType
+from rclpy.node import Node
+from rclpy.time import Time
+
+
+class VisionWrapper(Node):
+
+    def __init__(self):
+        super().__init__('consai2r2_vision_wrapper')
+
+        QUEUE_SIZE = 10
+        self._DISAPPEARED_TIME_THRESH = 3.0
+        self._PUBLISH_ROBOT = {'blue': False, 'yellow': False}
+
+        self.declare_parameter('publish_ball', True)
+        self.declare_parameter('publish_blue', True)
+        self.declare_parameter('publish_yellow', True)
+
+        common_params = consai2r2_parameters(self)
+        self._OUR_COLOR = common_params.our_color
+        self._MAX_ID = common_params.max_id
+
+        self._PUBLISH_BALL = self.get_parameter('publish_ball').value
+        self._PUBLISH_ROBOT['blue'] = self.get_parameter('publish_blue').value
+        self._PUBLISH_ROBOT['yellow'] = self.get_parameter(
+            'publish_yellow').value
+
+        self._pub_ball_info = None
+        self._ball_info = BallInfo()
+
+        if self._PUBLISH_BALL:
+            self._pub_ball_info = self.create_publisher(
+                BallInfo, '~/ball_info', QUEUE_SIZE)
+
+        self._robot_info = {'blue': [], 'yellow': []}
+        self._pubs_robot_info = {'blue': [], 'yellow': []}
+
+        for robot_id in range(self._MAX_ID + 1):
+            # 末尾に16進数の文字列をつける
+            topic_id = hex(robot_id)[2:]
+
+            self._robot_info['blue'].append(RobotInfo())
+            self._robot_info['yellow'].append(RobotInfo())
+
+            if self._PUBLISH_ROBOT['blue']:
+                topic_name = '~/robot_info_blue_' + topic_id
+                pub_robot_info = self.create_publisher(
+                    RobotInfo, topic_name, QUEUE_SIZE)
+                self._pubs_robot_info['blue'].append(pub_robot_info)
+
+            if self._PUBLISH_ROBOT['yellow']:
+                topic_name = '~/robot_info_yellow_' + topic_id
+                pub_robot_info = self.create_publisher(
+                    RobotInfo, topic_name, QUEUE_SIZE)
+                self._pubs_robot_info['yellow'].append(pub_robot_info)
+
+        self.create_subscription(
+            VisionDetections, 'consai2r2_vision_receiver/raw_vision_detections',
+            self._callback_detections, 1)
+
+    def _callback_detections(self, msg):
+        # Visionのデータからロボットとボールの位置を抽出する
+
+        time_stamp = msg.header.stamp
+
+        detection_balls = []
+        detection_blues = []
+        detection_yellows = []
+        for frame in msg.frames:
+            for ball in frame.balls:
+                detection_balls.append(ball)
+
+            for blue in frame.robots_blue:
+                detection_blues.append(blue)
+
+            for yellow in frame.robots_yellow:
+                detection_yellows.append(yellow)
+
+        self._extract_ball_pose(detection_balls, time_stamp)
+        self._extract_robot_pose('blue', detection_blues, time_stamp)
+        self._extract_robot_pose('yellow', detection_yellows, time_stamp)
+
+    def _extract_ball_pose(self, detection_balls, time_stamp):
+        # ボール座標を抽出する
+        # ボール座標が複数ある場合は、平均値を座標とする
+        if detection_balls:
+            average_pose = self._average_pose(detection_balls)
+            velocity = self._velocity(
+                self._ball_info.pose, average_pose, self._ball_info.detection_stamp, time_stamp)
+
+            self._ball_info.pose = average_pose
+            self._ball_info.last_detection_pose = average_pose
+            self._ball_info.velocity = velocity
+            self._ball_info.detected = True
+            self._ball_info.detection_stamp = time_stamp
+            self._ball_info.disappeared = False
+        else:
+            self._ball_info.detected = False
+
+            if self._ball_info.disappeared is False:
+                # 座標を受け取らなかった場合は、速度を用いて線形予測する
+
+                diff_time_stamp = Clock(clock_type=ClockType.ROS_TIME).now(
+                ) - Time.from_msg(self._ball_info.detection_stamp)
+                diff_time_secs = diff_time_stamp.nanoseconds / 1000000000
+
+                self._ball_info.pose = self._estimate(
+                    self._ball_info.last_detection_pose,
+                    self._ball_info.velocity, diff_time_secs)
+
+                # 一定時間、座標を受け取らなかったら消滅判定にする
+                if diff_time_secs > self._DISAPPEARED_TIME_THRESH:
+                    self._ball_info.disappeared = True
+
+        if self._PUBLISH_BALL:
+            self._pub_ball_info.publish(self._ball_info)
+
+    def _extract_robot_pose(self, color, detection_robots, time_stamp):
+        # ロボット姿勢を抽出する
+        # ロボット姿勢が複数ある場合は、平均値を姿勢とする
+        detections = [[] for i in range(len(self._robot_info[color]))]
+
+        # ID毎のリストに分ける
+        for robot in detection_robots:
+            robot_id = robot.robot_id
+            detections[robot_id].append(robot)
+
+        for robot_id, robots in enumerate(detections):
+            if robots:
+                average_pose = self._average_pose(robots)
+                velocity = self._velocity(
+                    self._robot_info[color][robot_id].pose, average_pose,
+                    self._robot_info[color][robot_id].detection_stamp, time_stamp)
+
+                self._robot_info[color][robot_id].robot_id = robot_id
+                self._robot_info[color][robot_id].pose = average_pose
+                self._robot_info[color][robot_id].last_detection_pose = average_pose
+                self._robot_info[color][robot_id].velocity = velocity
+                self._robot_info[color][robot_id].detected = True
+                self._robot_info[color][robot_id].detection_stamp = time_stamp
+                self._robot_info[color][robot_id].disappeared = False
+            else:
+                self._robot_info[color][robot_id].detected = False
+                self._robot_info[color][robot_id].robot_id = robot_id
+
+                # 座標を受け取らなかった場合は、速度を用いて線形予測する
+                if self._robot_info[color][robot_id].disappeared is False:
+                    duration = Clock(clock_type=ClockType.ROS_TIME).now() - \
+                        Time.from_msg(self._robot_info[color][robot_id].detection_stamp)
+                    diff_time_secs = duration.nanoseconds / 1000000000
+
+                    self._robot_info[color][robot_id].pose = self._estimate(
+                        self._robot_info[color][robot_id].last_detection_pose,
+                        self._robot_info[color][robot_id].velocity, diff_time_secs)
+
+                    # 一定時間、座標を受け取らなかったら消滅判定にする
+                    if diff_time_secs > self._DISAPPEARED_TIME_THRESH:
+                        self._robot_info[color][robot_id].disappeared = True
+
+        if self._PUBLISH_ROBOT[color]:
+            for robot_id in range(len(self._pubs_robot_info[color])):
+                self._pubs_robot_info[color][robot_id].publish(
+                    self._robot_info[color][robot_id])
+
+    def _average_pose(self, detections):
+        # 姿勢の平均値を求める
+        # 角度をpi ~ -piに収めるため、極座標系に角度を変換している
+
+        sum_pose = Pose2D()
+        detection_num = float(len(detections))
+
+        # 角度計算用
+        sum_x = 0.0
+        sum_y = 0.0
+        for detection in detections:
+            sum_pose.x += detection.pose.x
+            sum_pose.y += detection.pose.y
+            sum_x += math.cos(detection.pose.theta)
+            sum_y += math.sin(detection.pose.theta)
+        sum_pose.x = sum_pose.x / detection_num
+        sum_pose.y = sum_pose.y / detection_num
+        sum_pose.theta = math.fmod(math.atan2(sum_y, sum_x), math.pi)
+
+        return sum_pose
+
+    def _velocity(self, prev_pose, current_pose, prev_stamp, current_stamp):
+        # 姿勢の差分から速度を求める
+
+        velocity = Pose2D()
+        diff_time_stamp = Time.from_msg(
+            current_stamp) - Time.from_msg(prev_stamp)
+        diff_time_secs = diff_time_stamp.nanoseconds / 1000000000
+
+        if diff_time_secs > 0:
+            diff_pose = Pose2D()
+            diff_pose.x = current_pose.x - prev_pose.x
+            diff_pose.y = current_pose.y - prev_pose.y
+            diff_pose.theta = self._angle_normalize(
+                current_pose.theta - prev_pose.theta)
+
+            velocity.x = diff_pose.x / diff_time_secs
+            velocity.y = diff_pose.y / diff_time_secs
+            velocity.theta = diff_pose.theta / diff_time_secs
+
+        return velocity
+
+    def _estimate(self, pose, velocity, diff_time_secs):
+        # 速度と時間をもとに、姿勢を線形予測する
+
+        estimated_pose = Pose2D()
+
+        estimated_pose.x = pose.x + velocity.x * diff_time_secs
+        estimated_pose.y = pose.y + velocity.y * diff_time_secs
+        estimated_pose.theta = pose.theta + velocity.theta * diff_time_secs
+
+        return estimated_pose
+
+    def _angle_normalize(self, angle):
+        # 角度をpi  ~ -piの範囲に変換する
+        while angle > math.pi:
+            angle -= 2*math.pi
+
+        while angle < -math.pi:
+            angle += 2*math.pi
+
+        return angle
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    wrapper = VisionWrapper()
+
+    rclpy.spin(wrapper)
+
+    wrapper.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

consai2r2_vision_wrapper/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>consai2r2_vision_wrapper</name>
+  <version>0.0.0</version>
+  <description>Vision Wrapper</description>
+  <maintainer email="macakasit@gmail.com">akshota</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>consai2r2_msgs</exec_depend>
+  <exec_depend>consai2r2_receiver</exec_depend>
+  <exec_depend>consai2r2_description</exec_depend>
+
+  <!-- These test dependencies are optional
+  Their purpose is to make sure that the code passes the linters -->
+  <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>

consai2r2_vision_wrapper/setup.cfg

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

consai2r2_vision_wrapper/setup.py

@@ -0,0 +1,35 @@
+from setuptools import setup
+
+package_name = 'consai2r2_vision_wrapper'
+
+setup(
+    name=package_name,
+    version='0.0.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,
+    author='akshota',
+    author_email='macakasit@gmail.com',
+    maintainer='akshota',
+    maintainer_email='macakasit@gmail.com',
+    keywords=['ROS'],
+    classifiers=[
+        'Intended Audience :: Developers',
+        'License :: OSI Approved :: MIT License',
+        'Programming Language :: Python',
+        'Topic :: Software Development',
+    ],
+    description='Convert raw vision message to AI-friendly message',
+    license='MIT License',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'vision_wrapper = consai2r2_vision_wrapper.vision_wrapper:main',
+        ],
+    },
+)