Also integrated visual ros markers.

extras/ros/docker-compose-3d.yml

@@ -32,6 +32,32 @@ services:
             - "PYTHONUNBUFFERED=1"
         command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt3d_wrapper_cpp rpt3d_wrapper'
 
+    skeleton_markers:
+        image: rapidposetriangulation_ros3d
+        network_mode: "host"
+        ipc: "host"
+        privileged: true
+        volumes:
+            - ../../:/RapidPoseTriangulation/
+            - /tmp/.X11-unix:/tmp/.X11-unix
+            - /dev/shm:/dev/shm
+        environment:
+            - "PYTHONUNBUFFERED=1"
+        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers skeleton_markers'
+
+    cube_markers:
+        image: rapidposetriangulation_ros3d
+        network_mode: "host"
+        ipc: "host"
+        privileged: true
+        volumes:
+            - ../../:/RapidPoseTriangulation/
+            - /tmp/.X11-unix:/tmp/.X11-unix
+            - /dev/shm:/dev/shm
+        environment:
+            - "PYTHONUNBUFFERED=1"
+        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers cube_markers'
+
     skeleton_tfs:
         image: rapidposetriangulation_ros3d
         network_mode: "host"
@@ -43,4 +69,4 @@ services:
             - /dev/shm:/dev/shm
         environment:
             - "PYTHONUNBUFFERED=1"
-        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run transform_publisher skeleton_tfs'
+        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers skeleton_tfs'

extras/ros/dockerfile_3d

@@ -24,14 +24,14 @@ RUN echo "source /project/dev_ws/install/setup.bash" >> ~/.bashrc
 # Copy modules
 COPY ./extras/include/ /RapidPoseTriangulation/extras/include/
 COPY ./rpt/ /RapidPoseTriangulation/rpt/
+COPY ./extras/ros/marker_publishers/ /RapidPoseTriangulation/extras/ros/marker_publishers/
 COPY ./extras/ros/rpt_msgs/ /RapidPoseTriangulation/extras/ros/rpt_msgs/
 COPY ./extras/ros/rpt3d_wrapper_cpp/ /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/
-COPY ./extras/ros/transform_publisher/ /RapidPoseTriangulation/extras/ros/transform_publisher/
 
 # Link and build as ros package
+RUN ln -s /RapidPoseTriangulation/extras/ros/marker_publishers/ /project/dev_ws/src/
 RUN ln -s /RapidPoseTriangulation/extras/ros/rpt_msgs/ /project/dev_ws/src/
 RUN ln -s /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/ /project/dev_ws/src/
-RUN ln -s /RapidPoseTriangulation/extras/ros/transform_publisher/ /project/dev_ws/src/
 RUN /bin/bash -i -c 'cd /project/dev_ws/; colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release'
 
 # Update ros packages -> autocompletion and check

extras/ros/marker_publishers/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(transform_publisher)
+project(marker_publishers)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
@@ -21,6 +21,19 @@ 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)
@@ -28,9 +41,14 @@ target_include_directories(skeleton_tfs PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
-install(TARGETS skeleton_tfs
+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

extras/ros/marker_publishers/package.xml

@@ -1,7 +1,7 @@
 <?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>transform_publisher</name>
+  <name>marker_publishers</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="root@todo.todo">root</maintainer>
@@ -13,6 +13,7 @@
   <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>

extras/ros/marker_publishers/src/cube_markers.cpp

@@ -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;
+}

extras/ros/marker_publishers/src/skeleton_markers.cpp

@@ -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;
+}