crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Drop old code.

Browse Source
This commit is contained in:
Daniel
2025-01-21 18:01:14 +01:00
parent 2ba9e096ac
commit 9778c75bf0
15 changed files with 1 additions and 809 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
extras/ros/docker-compose.yml
View File

@ -35,7 +35,6 @@ services:
- DISPLAY
- QT_X11_NO_MITSHM=1
- "PYTHONUNBUFFERED=1"
# command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt2D_wrapper_py rpt2D_wrapper'
command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt2D_wrapper_cpp rpt2D_wrapper'
pose_visualizer:

2
extras/ros/dockerfile
View File

@ -63,12 +63,10 @@ RUN pip3 install --no-cache-dir "setuptools<=58.2.0"
COPY ./extras/include/ /RapidPoseTriangulation/extras/include/
COPY ./scripts/ /RapidPoseTriangulation/scripts/
COPY ./extras/ros/pose2D_visualizer/ /RapidPoseTriangulation/extras/ros/pose2D_visualizer/
COPY ./extras/ros/rpt2D_wrapper_py/ /RapidPoseTriangulation/extras/ros/rpt2D_wrapper_py/
COPY ./extras/ros/rpt2D_wrapper_cpp/ /RapidPoseTriangulation/extras/ros/rpt2D_wrapper_cpp/
# Link and build as ros package
RUN ln -s /RapidPoseTriangulation/extras/ros/pose2D_visualizer/ /project/dev_ws/src/pose2D_visualizer
RUN ln -s /RapidPoseTriangulation/extras/ros/rpt2D_wrapper_py/ /project/dev_ws/src/rpt2D_wrapper_py
RUN ln -s /RapidPoseTriangulation/extras/ros/rpt2D_wrapper_cpp/ /project/dev_ws/src/rpt2D_wrapper_cpp
RUN /bin/bash -i -c 'cd /project/dev_ws/; colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release'

18
extras/ros/rpt2D_wrapper_py/package.xml
View File

@ -1,18 +0,0 @@
<?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>rpt2D_wrapper_py</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="root@todo.todo">root</maintainer>
<license>TODO: License declaration</license>
<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>

0
extras/ros/rpt2D_wrapper_py/resource/rpt2D_wrapper_py
View File

0
extras/ros/rpt2D_wrapper_py/rpt2D_wrapper_py/__init__.py
View File

196
extras/ros/rpt2D_wrapper_py/rpt2D_wrapper_py/rpt2D_wrapper.py
View File

@ -1,196 +0,0 @@
import json
import os
import sys
import threading
import time
import numpy as np
import rclpy
from cv_bridge import CvBridge
from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from sensor_msgs.msg import Image
from std_msgs.msg import String
filepath = os.path.dirname(os.path.realpath(__file__)) + "/"
sys.path.append(filepath + "../../../scripts/")
import test_triangulate
import utils_2d_pose
# ==================================================================================================
bridge = CvBridge()
node = None
publisher_pose = None
cam_id = "camera01"
img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw"
pose_out_topic = "/poses/" + cam_id
last_input_image = None
last_input_time = 0.0
kpt_model = None
joint_names_2d = test_triangulate.joint_names_2d
lock = threading.Lock()
stop_flag = False
# Model config
min_bbox_score = 0.4
min_bbox_area = 0.1 * 0.1
batch_poses = True
# ==================================================================================================
def callback_images(image_data):
global last_input_image, last_input_time, lock
# Convert into cv images from image string
if image_data.encoding == "bayer_rggb8":
bayer_image = bridge.imgmsg_to_cv2(image_data, "bayer_rggb8")
elif image_data.encoding == "mono8":
bayer_image = bridge.imgmsg_to_cv2(image_data, "mono8")
elif image_data.encoding == "rgb8":
color_image = bridge.imgmsg_to_cv2(image_data, "rgb8")
bayer_image = test_triangulate.rgb2bayer(color_image)
else:
raise ValueError("Unknown image encoding:", image_data.encoding)
time_stamp = image_data.header.stamp.sec + image_data.header.stamp.nanosec / 1.0e9
with lock:
last_input_image = bayer_image
last_input_time = time_stamp
# ==================================================================================================
def callback_model():
global last_input_image, last_input_time, kpt_model, lock
ptime = time.time()
if last_input_time == 0.0:
time.sleep(0.0001)
return
# Collect inputs
images_2d = []
timestamps = []
with lock:
img = last_input_image
ts = last_input_time
images_2d.append(img)
timestamps.append(ts)
last_input_image = None
last_input_time = 0.0
# Predict 2D poses
images_2d = [test_triangulate.bayer2rgb(img) for img in images_2d]
poses_2d = utils_2d_pose.get_2d_pose(kpt_model, images_2d)
poses_2d = test_triangulate.update_keypoints(poses_2d, joint_names_2d)
poses_2d = poses_2d[0]
# Drop persons with no joints
poses_2d = np.asarray(poses_2d)
mask = np.sum(poses_2d[..., 2], axis=1) > 0
poses_2d = poses_2d[mask]
# Round poses
poses2D = [np.array(p).round(3).tolist() for p in poses_2d]
# Publish poses
ts_pose = time.time()
poses = {
"bodies2D": poses2D,
"joints": joint_names_2d,
"num_persons": len(poses2D),
"timestamps": {
"image": timestamps[0],
"pose": ts_pose,
"z-images-pose": ts_pose - timestamps[0],
},
}
publish(poses)
msg = "Detected persons: {} - Prediction time: {:.4f}s"
print(msg.format(poses["num_persons"], time.time() - ptime))
# ==================================================================================================
def callback_wrapper():
global stop_flag
while not stop_flag:
callback_model()
time.sleep(0.001)
# ==================================================================================================
def publish(data):
# Publish json data
msg = String()
msg.data = json.dumps(data)
publisher_pose.publish(msg)
# ==================================================================================================
def main():
global node, publisher_pose, kpt_model, stop_flag
# Start node
rclpy.init(args=sys.argv)
node = rclpy.create_node("rpt2D_wrapper")
# Quality of service settings
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
)
# Create subscribers
_ = node.create_subscription(
Image,
img_input_topic,
callback_images,
qos_profile,
)
# Create publishers
publisher_pose = node.create_publisher(String, pose_out_topic, qos_profile)
# Load 2D pose model
whole_body = test_triangulate.whole_body
if any((whole_body[k] for k in whole_body)):
kpt_model = utils_2d_pose.load_wb_model(
min_bbox_score, min_bbox_area, batch_poses
)
else:
kpt_model = utils_2d_pose.load_model(min_bbox_score, min_bbox_area, batch_poses)
node.get_logger().info("Finished initialization of pose estimator")
# Start prediction thread
p1 = threading.Thread(target=callback_wrapper)
p1.start()
# Run ros update thread
rclpy.spin(node)
stop_flag = True
p1.join()
node.destroy_node()
rclpy.shutdown()
# ==================================================================================================
if __name__ == "__main__":
main()

4
extras/ros/rpt2D_wrapper_py/setup.cfg
View File

@ -1,4 +0,0 @@
[develop]
script_dir=$base/lib/rpt2D_wrapper_py
[install]
install_scripts=$base/lib/rpt2D_wrapper_py

23
extras/ros/rpt2D_wrapper_py/setup.py
View File

@ -1,23 +0,0 @@
from setuptools import setup
package_name = "rpt2D_wrapper_py"
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,
maintainer="root",
maintainer_email="root@todo.todo",
description="TODO: Package description",
license="TODO: License declaration",
tests_require=["pytest"],
entry_points={
"console_scripts": ["rpt2D_wrapper = rpt2D_wrapper_py.rpt2D_wrapper:main"],
},
)

27
extras/ros/rpt2D_wrapper_py/test/test_copyright.py
View File

@ -1,27 +0,0 @@
# 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.
import pytest
from ament_copyright.main import main
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(
reason="No copyright header has been placed in the generated source file."
)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=[".", "test"])
assert rc == 0, "Found errors"

25
extras/ros/rpt2D_wrapper_py/test/test_flake8.py
View File

@ -1,25 +0,0 @@
# 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.
import pytest
from ament_flake8.main import main_with_errors
@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)

23
extras/ros/rpt2D_wrapper_py/test/test_pep257.py
View File

@ -1,23 +0,0 @@
# 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.
import pytest
from ament_pep257.main import main
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=[".", "test"])
assert rc == 0, "Found code style errors / warnings"