Also integrated visual ros markers.
This commit is contained in:
Daniel
63
extras/ros/marker_publishers/CMakeLists.txt
Normal file
63
extras/ros/marker_publishers/CMakeLists.txt
Normal file
@ -0,0 +1,63 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(marker_publishers)
|
||||
|
||||
# Default to C99
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 99)
|
||||
endif()
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rpt_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
|
||||
add_executable(cube_markers src/cube_markers.cpp)
|
||||
ament_target_dependencies(cube_markers rclcpp geometry_msgs visualization_msgs)
|
||||
target_include_directories(cube_markers PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
add_executable(skeleton_markers src/skeleton_markers.cpp)
|
||||
ament_target_dependencies(skeleton_markers rclcpp rpt_msgs geometry_msgs visualization_msgs)
|
||||
target_include_directories(skeleton_markers PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
add_executable(skeleton_tfs src/skeleton_tfs.cpp)
|
||||
ament_target_dependencies(skeleton_tfs rclcpp rpt_msgs geometry_msgs tf2_ros)
|
||||
target_include_directories(skeleton_tfs PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
install(TARGETS cube_markers
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(TARGETS skeleton_markers
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(TARGETS skeleton_tfs
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# uncomment the line when a copyright and license is not present in all source files
|
||||
#set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# uncomment the line when this package is not in a git repo
|
||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
24
extras/ros/marker_publishers/package.xml
Normal file
24
extras/ros/marker_publishers/package.xml
Normal file
@ -0,0 +1,24 @@
|
||||
<?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>marker_publishers</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rpt_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
122
extras/ros/marker_publishers/src/cube_markers.cpp
Normal file
122
extras/ros/marker_publishers/src/cube_markers.cpp
Normal file
@ -0,0 +1,122 @@
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/point.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
const std::string output_topic = "/markers_cube";
|
||||
static const std::array<std::array<float, 3>, 2> roomparams = {{
|
||||
{4.0, 5.0, 2.2},
|
||||
{1.0, 0.0, 1.1},
|
||||
}};
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
class CubePublisher : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CubePublisher() : Node("cube_publisher")
|
||||
{
|
||||
publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(output_topic, 1);
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::seconds(1), std::bind(&CubePublisher::timer_callback, this));
|
||||
|
||||
cube_edges = generate_cube_edges();
|
||||
}
|
||||
|
||||
private:
|
||||
visualization_msgs::msg::MarkerArray cube_edges;
|
||||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
void timer_callback()
|
||||
{
|
||||
publisher_->publish(cube_edges);
|
||||
}
|
||||
visualization_msgs::msg::MarkerArray generate_cube_edges();
|
||||
};
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
visualization_msgs::msg::MarkerArray CubePublisher::generate_cube_edges()
|
||||
{
|
||||
visualization_msgs::msg::MarkerArray marker_array;
|
||||
visualization_msgs::msg::Marker marker;
|
||||
|
||||
marker.header.frame_id = "world";
|
||||
marker.type = visualization_msgs::msg::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
marker.scale.x = 0.05;
|
||||
marker.id = 0;
|
||||
|
||||
marker.color.a = 1.0;
|
||||
marker.color.r = 1.0;
|
||||
|
||||
std::vector<std::vector<double>> corners = {
|
||||
{-1, -1, -1},
|
||||
{1, -1, -1},
|
||||
{1, 1, -1},
|
||||
{-1, 1, -1},
|
||||
{-1, -1, 1},
|
||||
{1, -1, 1},
|
||||
{1, 1, 1},
|
||||
{-1, 1, 1},
|
||||
};
|
||||
|
||||
for (auto &corner : corners)
|
||||
{
|
||||
for (size_t i = 0; i < corner.size(); ++i)
|
||||
{
|
||||
corner[i] = 0.5 * roomparams[0][i] * corner[i] + roomparams[1][i];
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::pair<int, int>> edge_indices = {
|
||||
{0, 1},
|
||||
{1, 2},
|
||||
{2, 3},
|
||||
{3, 0},
|
||||
{4, 5},
|
||||
{5, 6},
|
||||
{6, 7},
|
||||
{7, 4},
|
||||
{0, 4},
|
||||
{1, 5},
|
||||
{2, 6},
|
||||
{3, 7},
|
||||
};
|
||||
|
||||
for (const auto &edge : edge_indices)
|
||||
{
|
||||
geometry_msgs::msg::Point p1, p2;
|
||||
p1.x = corners[edge.first][0];
|
||||
p1.y = corners[edge.first][1];
|
||||
p1.z = corners[edge.first][2];
|
||||
|
||||
p2.x = corners[edge.second][0];
|
||||
p2.y = corners[edge.second][1];
|
||||
p2.z = corners[edge.second][2];
|
||||
|
||||
marker.points.push_back(p1);
|
||||
marker.points.push_back(p2);
|
||||
}
|
||||
|
||||
marker_array.markers.push_back(marker);
|
||||
return marker_array;
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<CubePublisher>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
255
extras/ros/marker_publishers/src/skeleton_markers.cpp
Normal file
255
extras/ros/marker_publishers/src/skeleton_markers.cpp
Normal file
@ -0,0 +1,255 @@
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/point.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
#include <std_msgs/msg/color_rgba.hpp>
|
||||
|
||||
#include "rpt_msgs/msg/poses.hpp"
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
const std::string input_topic = "/poses/humans3d";
|
||||
const std::string output_topic = "/markers_skeleton";
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
std::array<uint8_t, 3> hue_to_rgb(double hue)
|
||||
{
|
||||
double r, g, b;
|
||||
int h = static_cast<int>(hue * 6);
|
||||
double f = hue * 6 - h;
|
||||
double q = 1 - f;
|
||||
|
||||
switch (h % 6)
|
||||
{
|
||||
case 0:
|
||||
r = 1;
|
||||
g = f;
|
||||
b = 0;
|
||||
break;
|
||||
case 1:
|
||||
r = q;
|
||||
g = 1;
|
||||
b = 0;
|
||||
break;
|
||||
case 2:
|
||||
r = 0;
|
||||
g = 1;
|
||||
b = f;
|
||||
break;
|
||||
case 3:
|
||||
r = 0;
|
||||
g = q;
|
||||
b = 1;
|
||||
break;
|
||||
case 4:
|
||||
r = f;
|
||||
g = 0;
|
||||
b = 1;
|
||||
break;
|
||||
case 5:
|
||||
r = 1;
|
||||
g = 0;
|
||||
b = q;
|
||||
break;
|
||||
default:
|
||||
r = g = b = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
std::array<uint8_t, 3> rgb = {
|
||||
static_cast<uint8_t>(r * 255),
|
||||
static_cast<uint8_t>(g * 255),
|
||||
static_cast<uint8_t>(b * 255)};
|
||||
|
||||
return rgb;
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
class SkeletonPublisher : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
SkeletonPublisher() : Node("skeleton_publisher")
|
||||
{
|
||||
publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(output_topic, 1);
|
||||
subscriber_ = this->create_subscription<rpt_msgs::msg::Poses>(
|
||||
input_topic, 1,
|
||||
std::bind(&SkeletonPublisher::listener_callback, this, std::placeholders::_1));
|
||||
|
||||
connections = {
|
||||
{"shoulder_middle", "head"},
|
||||
{"head", "nose"},
|
||||
{"nose", "eye_left"},
|
||||
{"nose", "eye_right"},
|
||||
{"eye_left", "ear_left"},
|
||||
{"eye_right", "ear_right"},
|
||||
{"shoulder_left", "shoulder_right"},
|
||||
{"hip_middle", "shoulder_left"},
|
||||
{"hip_middle", "shoulder_right"},
|
||||
{"shoulder_left", "elbow_left"},
|
||||
{"elbow_left", "wrist_left"},
|
||||
{"hip_middle", "hip_left"},
|
||||
{"hip_left", "knee_left"},
|
||||
{"knee_left", "ankle_left"},
|
||||
{"shoulder_right", "elbow_right"},
|
||||
{"elbow_right", "wrist_right"},
|
||||
{"hip_middle", "hip_right"},
|
||||
{"hip_right", "knee_right"},
|
||||
{"knee_right", "ankle_right"},
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<std::array<std::string, 2>> connections;
|
||||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher_;
|
||||
rclcpp::Subscription<rpt_msgs::msg::Poses>::SharedPtr subscriber_;
|
||||
|
||||
void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg);
|
||||
visualization_msgs::msg::MarkerArray generate_msg(
|
||||
const std::vector<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names);
|
||||
};
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
void SkeletonPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg)
|
||||
{
|
||||
std::vector<std::vector<std::array<float, 4>>> poses;
|
||||
const std::vector<std::string> &joint_names = msg->joint_names;
|
||||
|
||||
// Unflatten poses
|
||||
size_t idx = 0;
|
||||
std::vector<int32_t> &bshape = msg->bodies_shape;
|
||||
for (int32_t i = 0; i < bshape[0]; ++i)
|
||||
{
|
||||
std::vector<std::array<float, 4>> body;
|
||||
for (int32_t j = 0; j < bshape[1]; ++j)
|
||||
{
|
||||
std::array<float, 4> joint;
|
||||
for (int32_t k = 0; k < bshape[2]; ++k)
|
||||
{
|
||||
joint[k] = msg->bodies_flat[idx];
|
||||
++idx;
|
||||
}
|
||||
body.push_back(std::move(joint));
|
||||
}
|
||||
poses.push_back(std::move(body));
|
||||
}
|
||||
|
||||
auto skelmsg = generate_msg(poses, joint_names);
|
||||
publisher_->publish(skelmsg);
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
visualization_msgs::msg::MarkerArray SkeletonPublisher::generate_msg(
|
||||
const std::vector<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names)
|
||||
{
|
||||
visualization_msgs::msg::MarkerArray marker_array;
|
||||
visualization_msgs::msg::Marker marker;
|
||||
|
||||
marker.header.frame_id = "world";
|
||||
marker.type = visualization_msgs::msg::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
marker.scale.x = 0.02;
|
||||
marker.id = 0;
|
||||
|
||||
size_t num_bodies = poses.size();
|
||||
for (size_t i = 0; i < num_bodies; ++i)
|
||||
{
|
||||
std_msgs::msg::ColorRGBA color;
|
||||
double hue = static_cast<double>(i) / num_bodies;
|
||||
auto rgb = hue_to_rgb(hue);
|
||||
color.r = rgb[0] / 255.0;
|
||||
color.g = rgb[1] / 255.0;
|
||||
color.b = rgb[2] / 255.0;
|
||||
color.a = 1.0;
|
||||
|
||||
for (size_t j = 0; j < connections.size(); ++j)
|
||||
{
|
||||
std::string joint1 = connections[j][0];
|
||||
std::string joint2 = connections[j][1];
|
||||
|
||||
int sidx = -1;
|
||||
int eidx = -1;
|
||||
for (size_t k = 0; k < joint_names.size(); ++k)
|
||||
{
|
||||
if (joint_names[k] == joint1)
|
||||
{
|
||||
sidx = k;
|
||||
}
|
||||
if (joint_names[k] == joint2)
|
||||
{
|
||||
eidx = k;
|
||||
}
|
||||
}
|
||||
if (sidx == -1 || eidx == -1)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (poses[i][sidx][3] <= 0 || poses[i][eidx][3] <= 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Point p1, p2;
|
||||
p1.x = poses[i][sidx][0];
|
||||
p1.y = poses[i][sidx][1];
|
||||
p1.z = poses[i][sidx][2];
|
||||
p2.x = poses[i][eidx][0];
|
||||
p2.y = poses[i][eidx][1];
|
||||
p2.z = poses[i][eidx][2];
|
||||
|
||||
marker.points.push_back(p1);
|
||||
marker.points.push_back(p2);
|
||||
marker.colors.push_back(color);
|
||||
marker.colors.push_back(color);
|
||||
}
|
||||
}
|
||||
|
||||
if (num_bodies == 0)
|
||||
{
|
||||
// Create an invisible line to remove any old skeletons
|
||||
std_msgs::msg::ColorRGBA color;
|
||||
color.r = 0.0;
|
||||
color.g = 0.0;
|
||||
color.b = 0.0;
|
||||
color.a = 0.0;
|
||||
|
||||
geometry_msgs::msg::Point p1, p2;
|
||||
p1.x = 0.0;
|
||||
p1.y = 0.0;
|
||||
p1.z = 0.0;
|
||||
p2.x = 0.0;
|
||||
p2.y = 0.0;
|
||||
p2.z = 0.001;
|
||||
|
||||
marker.points.push_back(p1);
|
||||
marker.points.push_back(p2);
|
||||
marker.colors.push_back(color);
|
||||
marker.colors.push_back(color);
|
||||
}
|
||||
|
||||
marker_array.markers.push_back(marker);
|
||||
return marker_array;
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<SkeletonPublisher>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
335
extras/ros/marker_publishers/src/skeleton_tfs.cpp
Normal file
335
extras/ros/marker_publishers/src/skeleton_tfs.cpp
Normal file
@ -0,0 +1,335 @@
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
|
||||
#include "rpt_msgs/msg/poses.hpp"
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
const std::string input_topic = "/poses/humans3d";
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
class SkeletonTFPublisher : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
SkeletonTFPublisher() : Node("skeleton_tf_publisher")
|
||||
{
|
||||
broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
||||
subscriber_ = this->create_subscription<rpt_msgs::msg::Poses>(
|
||||
input_topic, 1,
|
||||
std::bind(&SkeletonTFPublisher::listener_callback, this, std::placeholders::_1));
|
||||
|
||||
pc_connections = {
|
||||
// main joints
|
||||
{"hip_middle", "shoulder_middle"},
|
||||
{"shoulder_middle", "head"},
|
||||
{"head", "nose"},
|
||||
{"head", "eye_left"},
|
||||
{"head", "eye_right"},
|
||||
{"head", "ear_left"},
|
||||
{"head", "ear_right"},
|
||||
{"shoulder_middle", "shoulder_left"},
|
||||
{"shoulder_middle", "shoulder_right"},
|
||||
{"shoulder_left", "elbow_left"},
|
||||
{"elbow_left", "wrist_left"},
|
||||
{"hip_middle", "hip_left"},
|
||||
{"hip_left", "knee_left"},
|
||||
{"knee_left", "ankle_left"},
|
||||
{"shoulder_right", "elbow_right"},
|
||||
{"elbow_right", "wrist_right"},
|
||||
{"hip_middle", "hip_right"},
|
||||
{"hip_right", "knee_right"},
|
||||
{"knee_right", "ankle_right"},
|
||||
// whole-body joints
|
||||
{"ankle_left", "foot_toe_big_left"},
|
||||
{"ankle_left", "foot_toe_small_left"},
|
||||
{"ankle_left", "foot_heel_left"},
|
||||
{"ankle_right", "foot_toe_big_right"},
|
||||
{"ankle_right", "foot_toe_small_right"},
|
||||
{"ankle_right", "foot_heel_right"},
|
||||
{"ear_right", "face_jaw_right_1"},
|
||||
{"ear_right", "face_jaw_right_2"},
|
||||
{"ear_right", "face_jaw_right_3"},
|
||||
{"ear_right", "face_jaw_right_4"},
|
||||
{"ear_right", "face_jaw_right_5"},
|
||||
{"ear_right", "face_jaw_right_6"},
|
||||
{"ear_right", "face_jaw_right_7"},
|
||||
{"ear_right", "face_jaw_right_8"},
|
||||
{"head", "face_jaw_middle"},
|
||||
{"ear_left", "face_jaw_left_1"},
|
||||
{"ear_left", "face_jaw_left_2"},
|
||||
{"ear_left", "face_jaw_left_3"},
|
||||
{"ear_left", "face_jaw_left_4"},
|
||||
{"ear_left", "face_jaw_left_5"},
|
||||
{"ear_left", "face_jaw_left_6"},
|
||||
{"ear_left", "face_jaw_left_7"},
|
||||
{"ear_left", "face_jaw_left_8"},
|
||||
{"eye_right", "face_eyebrow_right_1"},
|
||||
{"eye_right", "face_eyebrow_right_2"},
|
||||
{"eye_right", "face_eyebrow_right_3"},
|
||||
{"eye_right", "face_eyebrow_right_4"},
|
||||
{"eye_right", "face_eyebrow_right_5"},
|
||||
{"eye_left", "face_eyebrow_left_1"},
|
||||
{"eye_left", "face_eyebrow_left_2"},
|
||||
{"eye_left", "face_eyebrow_left_3"},
|
||||
{"eye_left", "face_eyebrow_left_4"},
|
||||
{"eye_left", "face_eyebrow_left_5"},
|
||||
{"nose", "face_nose_1"},
|
||||
{"nose", "face_nose_2"},
|
||||
{"nose", "face_nose_3"},
|
||||
{"nose", "face_nose_4"},
|
||||
{"nose", "face_nose_5"},
|
||||
{"nose", "face_nose_6"},
|
||||
{"nose", "face_nose_7"},
|
||||
{"nose", "face_nose_8"},
|
||||
{"nose", "face_nose_9"},
|
||||
{"eye_right", "face_eye_right_1"},
|
||||
{"eye_right", "face_eye_right_2"},
|
||||
{"eye_right", "face_eye_right_3"},
|
||||
{"eye_right", "face_eye_right_4"},
|
||||
{"eye_right", "face_eye_right_5"},
|
||||
{"eye_right", "face_eye_right_6"},
|
||||
{"eye_left", "face_eye_left_1"},
|
||||
{"eye_left", "face_eye_left_2"},
|
||||
{"eye_left", "face_eye_left_3"},
|
||||
{"eye_left", "face_eye_left_4"},
|
||||
{"eye_left", "face_eye_left_5"},
|
||||
{"eye_left", "face_eye_left_6"},
|
||||
{"head", "face_mouth_1"},
|
||||
{"head", "face_mouth_2"},
|
||||
{"head", "face_mouth_3"},
|
||||
{"head", "face_mouth_4"},
|
||||
{"head", "face_mouth_5"},
|
||||
{"head", "face_mouth_6"},
|
||||
{"head", "face_mouth_7"},
|
||||
{"head", "face_mouth_8"},
|
||||
{"head", "face_mouth_9"},
|
||||
{"head", "face_mouth_10"},
|
||||
{"head", "face_mouth_11"},
|
||||
{"head", "face_mouth_12"},
|
||||
{"head", "face_mouth_13"},
|
||||
{"head", "face_mouth_14"},
|
||||
{"head", "face_mouth_15"},
|
||||
{"head", "face_mouth_16"},
|
||||
{"head", "face_mouth_17"},
|
||||
{"head", "face_mouth_18"},
|
||||
{"head", "face_mouth_19"},
|
||||
{"head", "face_mouth_20"},
|
||||
{"wrist_left", "hand_wrist_left"},
|
||||
{"wrist_left", "hand_finger_thumb_left_1"},
|
||||
{"wrist_left", "hand_finger_thumb_left_2"},
|
||||
{"wrist_left", "hand_finger_thumb_left_3"},
|
||||
{"wrist_left", "hand_finger_thumb_left_4"},
|
||||
{"wrist_left", "hand_finger_index_left_1"},
|
||||
{"wrist_left", "hand_finger_index_left_2"},
|
||||
{"wrist_left", "hand_finger_index_left_3"},
|
||||
{"wrist_left", "hand_finger_index_left_4"},
|
||||
{"wrist_left", "hand_finger_middle_left_1"},
|
||||
{"wrist_left", "hand_finger_middle_left_2"},
|
||||
{"wrist_left", "hand_finger_middle_left_3"},
|
||||
{"wrist_left", "hand_finger_middle_left_4"},
|
||||
{"wrist_left", "hand_finger_ring_left_1"},
|
||||
{"wrist_left", "hand_finger_ring_left_2"},
|
||||
{"wrist_left", "hand_finger_ring_left_3"},
|
||||
{"wrist_left", "hand_finger_ring_left_4"},
|
||||
{"wrist_left", "hand_finger_pinky_left_1"},
|
||||
{"wrist_left", "hand_finger_pinky_left_2"},
|
||||
{"wrist_left", "hand_finger_pinky_left_3"},
|
||||
{"wrist_left", "hand_finger_pinky_left_4"},
|
||||
{"wrist_right", "hand_wrist_right"},
|
||||
{"wrist_right", "hand_finger_thumb_right_1"},
|
||||
{"wrist_right", "hand_finger_thumb_right_2"},
|
||||
{"wrist_right", "hand_finger_thumb_right_3"},
|
||||
{"wrist_right", "hand_finger_thumb_right_4"},
|
||||
{"wrist_right", "hand_finger_index_right_1"},
|
||||
{"wrist_right", "hand_finger_index_right_2"},
|
||||
{"wrist_right", "hand_finger_index_right_3"},
|
||||
{"wrist_right", "hand_finger_index_right_4"},
|
||||
{"wrist_right", "hand_finger_middle_right_1"},
|
||||
{"wrist_right", "hand_finger_middle_right_2"},
|
||||
{"wrist_right", "hand_finger_middle_right_3"},
|
||||
{"wrist_right", "hand_finger_middle_right_4"},
|
||||
{"wrist_right", "hand_finger_ring_right_1"},
|
||||
{"wrist_right", "hand_finger_ring_right_2"},
|
||||
{"wrist_right", "hand_finger_ring_right_3"},
|
||||
{"wrist_right", "hand_finger_ring_right_4"},
|
||||
{"wrist_right", "hand_finger_pinky_right_1"},
|
||||
{"wrist_right", "hand_finger_pinky_right_2"},
|
||||
{"wrist_right", "hand_finger_pinky_right_3"},
|
||||
{"wrist_right", "hand_finger_pinky_right_4"},
|
||||
};
|
||||
for (auto &pair : pc_connections)
|
||||
{
|
||||
cp_map[pair[1]] = pair[0];
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<std::array<std::string, 2>> pc_connections;
|
||||
std::map<std::string, std::string> cp_map;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
|
||||
rclcpp::Subscription<rpt_msgs::msg::Poses>::SharedPtr subscriber_;
|
||||
|
||||
void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg);
|
||||
std::vector<geometry_msgs::msg::TransformStamped> generate_msg(
|
||||
const std::vector<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names);
|
||||
};
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
void SkeletonTFPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg)
|
||||
{
|
||||
std::vector<std::vector<std::array<float, 4>>> poses;
|
||||
const std::vector<std::string> &joint_names = msg->joint_names;
|
||||
|
||||
// Unflatten poses
|
||||
size_t idx = 0;
|
||||
std::vector<int32_t> &bshape = msg->bodies_shape;
|
||||
for (int32_t i = 0; i < bshape[0]; ++i)
|
||||
{
|
||||
std::vector<std::array<float, 4>> body;
|
||||
for (int32_t j = 0; j < bshape[1]; ++j)
|
||||
{
|
||||
std::array<float, 4> joint;
|
||||
for (int32_t k = 0; k < bshape[2]; ++k)
|
||||
{
|
||||
joint[k] = msg->bodies_flat[idx];
|
||||
++idx;
|
||||
}
|
||||
body.push_back(std::move(joint));
|
||||
}
|
||||
poses.push_back(std::move(body));
|
||||
}
|
||||
|
||||
if (poses.empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
auto tf_msgs = generate_msg(poses, joint_names);
|
||||
if (!tf_msgs.empty())
|
||||
{
|
||||
broadcaster_->sendTransform(tf_msgs);
|
||||
}
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
std::vector<geometry_msgs::msg::TransformStamped> SkeletonTFPublisher::generate_msg(
|
||||
const std::vector<std::vector<std::array<float, 4>>> &poses,
|
||||
const std::vector<std::string> &joint_names)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::TransformStamped> tf_msgs;
|
||||
tf_msgs.reserve(poses.size() * pc_connections.size());
|
||||
rclcpp::Time now = this->get_clock()->now();
|
||||
|
||||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
{
|
||||
const auto &body = poses[i];
|
||||
|
||||
// Find index of "hip_middle" in joint_names
|
||||
int hip_mid_idx = -1;
|
||||
for (size_t k = 0; k < joint_names.size(); ++k)
|
||||
{
|
||||
if (joint_names[k] == "hip_middle")
|
||||
{
|
||||
hip_mid_idx = static_cast<int>(k);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Set "hip_middle" as child of "world"
|
||||
const auto &joint = body[hip_mid_idx];
|
||||
geometry_msgs::msg::TransformStamped tf_stamped;
|
||||
tf_stamped.header.stamp = now;
|
||||
tf_stamped.header.frame_id = "world";
|
||||
tf_stamped.child_frame_id = "s" + std::to_string(i) + "-hip_middle";
|
||||
tf_stamped.transform.translation.x = joint[0];
|
||||
tf_stamped.transform.translation.y = joint[1];
|
||||
tf_stamped.transform.translation.z = joint[2];
|
||||
tf_stamped.transform.rotation.x = 0.0;
|
||||
tf_stamped.transform.rotation.y = 0.0;
|
||||
tf_stamped.transform.rotation.z = 0.0;
|
||||
tf_stamped.transform.rotation.w = 1.0;
|
||||
tf_msgs.push_back(std::move(tf_stamped));
|
||||
|
||||
// Add other joints
|
||||
for (size_t j = 0; j < body.size(); ++j)
|
||||
{
|
||||
// Skip "hip_middle"
|
||||
if (static_cast<int>(j) == hip_mid_idx)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto &joint_child = body[j];
|
||||
const std::string &child_name = joint_names[j];
|
||||
float conf = joint_child[3];
|
||||
if (conf <= 0.0f)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Look up its parent
|
||||
auto it = cp_map.find(child_name);
|
||||
if (it == cp_map.end())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
const std::string &parent_name = it->second;
|
||||
|
||||
// Ensure the parent frame was actually published
|
||||
int parent_idx = -1;
|
||||
for (size_t k = 0; k < joint_names.size(); ++k)
|
||||
{
|
||||
if (joint_names[k] == parent_name)
|
||||
{
|
||||
parent_idx = static_cast<int>(k);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (parent_name != "hip_middle" && body[parent_idx][3] <= 0.0f)
|
||||
{
|
||||
// Parent not visible, skip this child
|
||||
continue;
|
||||
}
|
||||
const auto &joint_parent = body[parent_idx];
|
||||
|
||||
// Publish child frame
|
||||
geometry_msgs::msg::TransformStamped tf_stamped;
|
||||
tf_stamped.header.stamp = now;
|
||||
tf_stamped.header.frame_id = "s" + std::to_string(i) + "-" + parent_name;
|
||||
tf_stamped.child_frame_id = "s" + std::to_string(i) + "-" + child_name;
|
||||
tf_stamped.transform.translation.x = joint_child[0] - joint_parent[0];
|
||||
tf_stamped.transform.translation.y = joint_child[1] - joint_parent[1];
|
||||
tf_stamped.transform.translation.z = joint_child[2] - joint_parent[2];
|
||||
tf_stamped.transform.rotation.x = 0.0;
|
||||
tf_stamped.transform.rotation.y = 0.0;
|
||||
tf_stamped.transform.rotation.z = 0.0;
|
||||
tf_stamped.transform.rotation.w = 1.0;
|
||||
tf_msgs.push_back(std::move(tf_stamped));
|
||||
}
|
||||
}
|
||||
|
||||
return tf_msgs;
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
// =================================================================================================
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<SkeletonTFPublisher>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user