k4a_result_t K4AROSDevice::getBodyMarker()

in src/k4a_ros_device.cpp [787:822]

• 27 lines of code
• 1 McCabe index (conditional complexity)

k4a_result_t K4AROSDevice::getBodyMarker(const k4abt_body_t& body, MarkerPtr marker_msg, int jointType,
                                         ros::Time capture_time)
{
  k4a_float3_t position = body.skeleton.joints[jointType].position;
  k4a_quaternion_t orientation = body.skeleton.joints[jointType].orientation;

  marker_msg->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_;
  marker_msg->header.stamp = capture_time;

  // Set the lifetime to 0.25 to prevent flickering for even 5fps configurations.
  // New markers with the same ID will replace old markers as soon as they arrive.
  marker_msg->lifetime = ros::Duration(0.25);
  marker_msg->id = body.id * 100 + jointType;
  marker_msg->type = Marker::SPHERE;

  Color color = BODY_COLOR_PALETTE[body.id % BODY_COLOR_PALETTE.size()];

  marker_msg->color.a = color.a;
  marker_msg->color.r = color.r;
  marker_msg->color.g = color.g;
  marker_msg->color.b = color.b;

  marker_msg->scale.x = 0.05;
  marker_msg->scale.y = 0.05;
  marker_msg->scale.z = 0.05;

  marker_msg->pose.position.x = position.v[0] / 1000.0f;
  marker_msg->pose.position.y = position.v[1] / 1000.0f;
  marker_msg->pose.position.z = position.v[2] / 1000.0f;
  marker_msg->pose.orientation.w = orientation.wxyz.w;
  marker_msg->pose.orientation.x = orientation.wxyz.x;
  marker_msg->pose.orientation.y = orientation.wxyz.y;
  marker_msg->pose.orientation.z = orientation.wxyz.z;

  return K4A_RESULT_SUCCEEDED;
}