ros4hri / hri_fullbody Goto Github PK

A ROS4HRI-conformant node to track and extract 2D and 3D skeletons of humans from RGB/RGB-D cameras

License: Apache License 2.0

CMake 0.72% Python 99.28%

hri_fullbody's Introduction

hri_fullbody

Overview

⚠️ some of the links are yet to be updated and may be pointing to the original ROS page. As soon as all the involved components will be officially documented for ROS 2 as well, we will update this document.

hri_fullbody is a ROS4HRI-compatible 3D body pose estimation node.

It is built on top of Google Mediapipe 3D body pose estimation.

The node provides the 3D pose estimation for the detected humans in the scene, implementing a robust solution to self-occlusions.

This node can be used in two different modes: single body and multi-body. In the former case, the node performs the whole body detection pipeline, publishing information under the ROS4HRI naming convention regarding the body id (on the /humans/bodies/tracked topic), the body bounding box, and the jointstate of the body's skeleton. In the latter, the node only publishes jointstate information, expecting body detection to be performed by another node and subscribing to /humans/bodies/tracked to read the ids of the currently tracked bodies.

These two modes are required since Mediapipe body estimation is not able to estimate more than one body pose per frame. Therefore, in case we want more than one body pose estimated, the adopted strategy is to rely on some other package implementing a human body detection (and tracking in the best case) algorithm. Such package should provide the hri_fullbody detect node the IDs of the detected bodies and the relative images, cropped from the original image following the detected bounding boxes.

To estimate the body position, the node does not need a RGB-D camera, only RGB is required. However, using RGB-D camera provides a more accurate depth estimation.

Important: to estimate the body depth without using a depth sensor, a calibrated RGB camera is required. You can follow this tutorial to properly calibrate your camera.

Launch

ros2 launch hri_fullbody hri_fullbody.launch.py <parameters>

ROS API

Parameters

Node parameters:

single_body (default: True): whether or not running in single body mode (see above for the single body vs multi-body modes).
use_depth (default: False): whether or not to rely on depth images for estimating body movement in the scene. When this is False, the node estimates the body position in the scene solving a P6P problem for the face and approximating the position from this, using pinhole camera model geometry.
stickman_debug (default: False): whether or not to publish frames representing the body skeleton directly using the raw results from mediapipe 3D body pose estimation. These debug frames are not oriented to align with the body links (ie, only the 3D location of the frame is useful).
human_description_<body_id> (read-only): the URDF generated for body_<body_id>. The node generates a URDF for each detected body and uses them to perform kinematically-consistent 3D body pose estimation.

hri_fullbody.launch parameters:

single_body (default: True): equivalent to single_body node parameter.
use_depth (default: False): equivalent to use_depth node parameter.
stickman_debug (default: False): equivalent to stickman_debug node parameter.
rgb_camera (default: ): rgb camera topics namespace.
rgb_camera_topic (default: $(arg rgb_camera)/image_raw): rgb camera raw image topic.
rgb_camera_info (default: $(arg rgb_camera)/camera_info): rgb camera info topic.
depth_camera (default: ): depth camera topics namespace.
depth_camera_topic (default: $(arg depth_camera)/image_rect_raw): depth camera rectified raw image topic.
depth_camera_info (default: $(arg depth_camera)/camera_info): depth camera info topic.

Topics

hri_fullbody follows the ROS4HRI conventions (REP-155). In particular, refer to the REP to know the list and position of the 2D/3D skeleton points published by the node.

Subscribed topics

/camera_info (sensor_msgs/CameraInfo): rgb camera meta information
/depth_image (sensor_msgs/Image): depth image used to estimate the 3D body position with respect to the camera.
/depth_info (sensor_msgs/CameraInfo): depth camera meta information

Single body mode only:

/image (sensor_msgs/Image): rgb image, processed for body detection and 3D body pose estimation.

Multi-body mode only:

/humans/bodies/tracked (hri_msgs/IdsList): list of the bodies currently detected.
/humans/bodies/<body_id>/cropped (sensor_msgs/Image): image used to estimate the 3D body pose.
/humans/bodies/<body_id>/roi (hri_msgs/NormalizedRegionOfInterest2D): body bounding box in normalized image coordinates.

Published topics

/humans/bodies/<body_id>/skeleton2d (hri_msgs/Skeleton2D): detected 2D skeleton points.
/humans/bodies/<body_id>/joint_states (sensor_msgs/JointState): skeleton joints state.
/humans/bodies/<body_id>/position: (geometry_msgs/PointStamped): filtered body position, representing the point between the hips of the tracked body. Only published when use_depth = True.
/humans/bodies/<body_id>/velocity: (geometry_msgs/TwistStamped): filtered body velocity. Only published when use_depth = True.

Single body mode only:

/humans/bodies/tracked (hri_msgs/IdsList): list of the bodies currently detected. There will be only one body in the list.
/humans/bodies/<body_id>/roi (hri_msgs/NormalizedRegionOfInterest2D): body bounding box in normalized image coordinates.

Visualization

It is possible to visualize the results of the body pose estimation in rviz using the hri_rviz Skeleton plugin. A visualization example is reported in the image above.

hri_fullbody's People

Contributors

Stargazers

Watchers

Forkers

juandpenan manuelgitgomes tiago-we-cobot nhoa-project

hri_fullbody's Issues

Visualization does not match actual joint values

I'm running into two separate problems. (Note: I switched to ikpy=3.2.2, but it didn't fix anything)

1) When `single_body = true`

When I run roslaunch ./hri_fullbody_visualize.launch single_body:=true, everything launches, but the skeleton pose does not match my real pose (see image).

And in the terminal, I don't know the if the warning in the last line is relevant.

SUMMARY
========

PARAMETERS
 * /hri_fullbody/single_body: True
 * /hri_fullbody/stickman_debug: False
 * /hri_fullbody/use_depth: False
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    hri_fullbody (hri_fullbody/detect)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[hri_fullbody-1]: started with pid [315166]
process[rviz-2]: started with pid [315174]
[ INFO] [1691167667.906666568]: rviz version 1.14.20
[ INFO] [1691167667.906753062]: compiled against Qt version 5.12.8
[ INFO] [1691167667.906781549]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1691167667.920658333]: Forcing OpenGl version 0.
[ INFO] [1691167668.730617953]: Stereo is NOT SUPPORTED
[ INFO] [1691167668.730709508]: OpenGL device: NVIDIA GeForce RTX 2080 Ti/PCIe/SSE2
[ INFO] [1691167668.730734523]: OpenGl version: 4.6 (GLSL 4.6).
[INFO] [1691167670.148945]: Using depth camera for body position estimation: False
[INFO] [1691167670.150222]: Setting up for single body pose estimation
[WARN] [1691167670.151449]: hri_fullbody running in single body mode: only one skeleton will be detected
[INFO] [1691167670.208985]: Generated single person detector for body_dwkvl
[INFO] [1691167670.209985]: Waiting for frames on topic /camera/color/image_raw
INFO: Created TensorFlow Lite XNNPACK delegate for CPU.
[INFO] [1691167670.335783]: Setting URDF description for body<dwkvl> (param name: human_description_dwkvl)
[INFO] [1691167670.657003]: Spawning a instance of robot_state_publisher for this body...
[INFO] [1691167670.658474]: Executing: rosrun robot_state_publisher robot_state_publisher __name:=robot_state_publisher_body_dwkvl joint_states:=/humans/bodies/dwkvl/joint_states robot_description:=human_description_dwkvl _publish_frequency:=25 _use_tf_static:=false
[WARN] [1691167671.087285]: Could not process inbound connection: topic types do not match: [sensor_msgs/RegionOfInterest] vs. [hri_msgs/NormalizedRegionOfInterest2D]{'callerid': '/rviz', 'md5sum': 'bdb633039d588fcccb441a4d43ccfe09', 'tcp_nodelay': '0', 'topic': '/humans/bodies/dwkvl/roi', 'type': 'sensor_msgs/RegionOfInterest'}

2) When `single_body = false`

When I run roslaunch ./hri_fullbody_visualize.launch single_body:=false, I get an error and it doesn't do any sort of tracking and nothing is published.

cywong@bonnat:~/catkin_ws/src/hri_fullbody/launch$ roslaunch ./hri_fullbody_visualize.launch 
... logging to /home/interaction/cywong/.ros/log/b7ddd5fe-3098-11ee-8b8f-9f56adc66784/roslaunch-bonnat-314761.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://bonnat:37325/

SUMMARY
========

PARAMETERS
 * /hri_fullbody/single_body: False
 * /hri_fullbody/stickman_debug: False
 * /hri_fullbody/use_depth: False
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    hri_fullbody (hri_fullbody/detect)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[hri_fullbody-1]: started with pid [314784]
process[rviz-2]: started with pid [314792]
[ INFO] [1691167522.130342513]: rviz version 1.14.20
[ INFO] [1691167522.130403296]: compiled against Qt version 5.12.8
[ INFO] [1691167522.130414993]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1691167522.141579016]: Forcing OpenGl version 0.
[ INFO] [1691167523.260983912]: Stereo is NOT SUPPORTED
[ INFO] [1691167523.261049501]: OpenGL device: NVIDIA GeForce RTX 2080 Ti/PCIe/SSE2
[ INFO] [1691167523.261069569]: OpenGl version: 4.6 (GLSL 4.6).
[INFO] [1691167525.218348]: Using depth camera for body position estimation: False
[INFO] [1691167525.219671]: Setting up for multibody pose estimation
[INFO] [1691167525.221746]: Waiting for ids on /humans/bodies/tracked
Exception in thread Thread-5:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
    self.run()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 237, in run
    self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
  File "/home/interaction/cywong/catkin_ws/src/hri_fullbody/nodes/detect", line 100, in do_diagnostics
    KeyValue(key="Last detected body ID", value=str(next(reversed(self.detected_bodies)))),
StopIteration

Any help would be appreciated.

help with multi-body

Hey guys, I would like to know if you have a recommendation on what package/library to use to perform the body detection to extract the bounding boxes.

Thanks!

Skeleton Detection not working with Depth !

Hey there,

I was trying to use HRI Fullbody package to detect a human inside Gazebo Simulation.
My sensor is Astra Camera (The same attached to Turtle Bot robot), so the gazebo plugin is the same.
When I'm feeding the corresponding topics to the full_body launch file, it can recognize the human model and define the skeleton. However, when I'm trying to visualize it through Rviz, it seems that the publishing topic is not measuring x,y,z of the detected skeleton properly.

Here is my modified launch file:

<arg name="rgb_camera" default="/camera/Astra_Left/Astra_Left/rgb"/>
<arg name="rgb_camera_topic" default="$(arg rgb_camera)/image_raw"/>
<arg name="rgb_camera_info" default="$(arg rgb_camera)/camera_info"/>

<arg name="use_depth" default="True"/>
<!-- /xtion/depth_registered/points -->
<arg name="depth_camera" default="/camera/Astra_Left/Astra_Left/depth"/>
<arg name="depth_camera_topic" default="$(arg depth_camera)/image_raw"/>
<arg name="depth_camera_info" default="$(arg depth_camera)/camera_info"/>

<!-- publishes a TF tree of the joints, *without* performing IK kinematics to
     properly align the joints frames to the kinematic model of the human -->
<arg name="stickman_debug" default="True"/>

 <!-- 
  If 'single_body' is False, hri_fullbody waits for bodies Region of Interests to be published on /humans/bodies/xxx/roi, and it uses those RoIs to perform 3D skeleton estimation.
  If 'single_body' is True, the node will not use external body detections, and instead will detect *a single* person, and provide the RoI + 3D skeleton of that person.
-->
<arg name="single_body" default="True"/>


<node name="hri_fullbody" pkg="hri_fullbody" type="detect" output="screen">
    <param name="use_depth" value="$(arg use_depth)"/>
    <param name="stickman_debug" value="$(arg stickman_debug)"/>
    <param name="single_body" value="$(arg single_body)"/>

    <remap from="/image" to="$(arg rgb_camera_topic)"/>
    <remap from="/camera_info" to="$(arg rgb_camera_info)"/>
    <remap from="/depth_image" to="$(arg depth_camera_topic)"/>
    <remap from="/depth_info" to="$(arg depth_camera_info)"/>
</node>

Then, the Gazebo Simulation and Rviz visualization results are like below:

I haven't tested it using real Astra Camera yet. I assume that the real RGB-D sensor would make it work and it's more or less an issue related to the astra camera's Gazebo plugin (I also tested it using Xtion Gazebo Plugin, got into the same issue).

Here is also my astra.xacro file:

<xacro:property name="M_PI" value="3.1415926535897931" />

<xacro:property name="astra_dae_display_scale" value="1" />

<xacro:macro name="sensor_astra" params="parent name:=Astra *origin">

<xacro:insert_block name="origin" />

<joint name="${name}_camera_rgb_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_camera_rgb_frame" />
  <child link="${name}_camera_rgb_optical_frame" />
</joint>
<link name="${name}_camera_rgb_optical_frame"/>

<joint name="${name}_camera_joint" type="fixed">
  <origin xyz="0 0 0"
          rpy="0 0 0"/>
  <parent link="${name}_camera_rgb_frame"/>
  <child link="${name}_camera_link"/>
</joint>
<link name="${name}_camera_link">
  <visual>
    <origin xyz="-0.04 0.02 -0.01" rpy="${M_PI/2} 0 ${M_PI/2}"/>
    <geometry>
      <mesh filename="file:///tiago_public_ws/src/tiago_patient_simulation/urdf/astra/meshes/astra.dae" scale="${astra_dae_display_scale} ${astra_dae_display_scale} ${astra_dae_display_scale}"/>
    </geometry>
  </visual>
  <collision>
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
    <geometry>
    <box size="0.0400 0.1650 0.0300"/>
  </geometry>
  </collision>
  <inertial>
    <mass value="0.564" />
    <origin xyz="0 0 0" />
    <inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
             iyy="0.000498940" iyz="0.0"
             izz="0.003879257" />
  </inertial>
</link>

<joint name="${name}__camera_depth_joint" type="fixed">
  <origin xyz="0 0 0" rpy="0 0 0" />
  <parent link="${name}_camera_rgb_frame" />
  <child link="${name}_camera_depth_frame" />
</joint>
<link name="${name}_camera_depth_frame"/>

<joint name="${name}_camera_depth_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_camera_depth_frame" />
  <child link="${name}"/>
</joint>
<link name="${name}"/>

<!-- RGBD sensor for simulation, same as Kinect -->

<gazebo reference="${name}_camera_link">  
  <sensor type="depth" name="$astra">
    <always_on>true</always_on>
    <update_rate>20.0</update_rate>
    <camera>
        <horizontal_fov>${70.0*M_PI/180.0}</horizontal_fov>
      <image>
        <format>B8G8R8</format>
        <width>640</width>
        <height>480</height>
      </image>
      <clip>
        <near>0.4</near>
        <far>15.0</far>
      </clip>
    </camera>
    <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
      <cameraName>camera/${name}</cameraName>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <imageTopicName>${name}/rgb/image_raw</imageTopicName>
      <depthImageTopicName>${name}/depth/image_raw</depthImageTopicName>
      <pointCloudTopicName>${name}/depth/points</pointCloudTopicName>
      <cameraInfoTopicName>${name}/rgb/camera_info</cameraInfoTopicName>
      <depthImageCameraInfoTopicName>${name}/depth/camera_info</depthImageCameraInfoTopicName>
      <frameName>${name}</frameName>
      <baseline>0.1</baseline>
      <distortion_k1>0.0</distortion_k1>
      <distortion_k2>0.0</distortion_k2>
      <distortion_k3>0.0</distortion_k3>
      <distortion_t1>0.0</distortion_t1>
      <distortion_t2>0.0</distortion_t2>
      <pointCloudCutoff>0.4</pointCloudCutoff>
      <pointCloudCutoffMax>8.0</pointCloudCutoffMax>
    </plugin>
  </sensor>
</gazebo>

</xacro:macro>

I'd appreciate it if someone could help me with this problem.

3D Skeleton not Detected

Hi i get a "No transform from [body_rluhf] to [head_rluhf]" eg in the 3D Skeleton Plugin.
in

I have installed hri_msgs,hri_rviz,human_description,libhri,pyhri packages manually.

The commands i run are:
roscore
rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/color/image_raw /usb_cam/camera_info:=/camera_info
roslaunch hri_fullbody hri_fullbody.launch
rviz

In the terminal i get "It was not possible to estimate body position." error
This is the output i get in rviz:

Very small joint states numbers

Hello all!
First of all, thank you for developing this package, as it is very useful. I first hear of you from your presentation at ROSCON 2022.

Now, when I tried to use the package, I cannot get a correct estimation of a 2D/3D skeleton pose using an RGB-D camera.

One thing I noticed is that most values for the joint states, as they are really small (near zero):

position: [0.0, 0.0, 0.0, 0.0, 0.0, 8.909208311087425e-11, 0.0, 1.631412601457227e-10, 0.0, 1.128599690590957e-10, 0.0,

1.1872098531808344e-10, 0.0, 6.628555182943805e-11, -1.8735825248397015e-11, -1.6691366356208623e-10, 

-0.09999942649329777, -0.39999999999998326, 0.6999999999999998, -3.535952186191125e-18]

These values, however, move consistently when the real world joint moves. That is, in a repeating joint movement (rotation of left arm, as an example), the joint values associated follow the movement. These joint values continue to be very small (in the same order of magnitude as the ones presented before).

To tackle this, I have tried to multiply these values by a large constant, with no success. Do you have any suggestion on what might be wrong?

I have a bagfile being uploaded to help you test in my conditions, if you want to.

Thank you for your attention.

Kind regards,
Manuel

ros4hri / hri_fullbody Goto Github PK

hri_fullbody's Introduction

hri_fullbody

Overview

Launch

ROS API

Parameters

Node parameters:

hri_fullbody.launch parameters:

Topics

Subscribed topics

Single body mode only:

Multi-body mode only:

Published topics

Single body mode only:

Visualization

hri_fullbody's People

Contributors

Stargazers

Watchers

Forkers

hri_fullbody's Issues

1) When single_body = true

2) When single_body = false

Recommend Projects

Recommend Topics

Recommend Org

1) When `single_body = true`

2) When `single_body = false`