Visualization does not match actual joint values

[chrisywong]

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.

[LukaJuricic]

Hi @chrisywong , answering in order:

1. This mismatch is probably due to an outdated version of the hri package, which is used by the hri_rviz plugins to display the skeleton. Could you try updating both of them?
2. This is a bug, thank you for letting us know! We will fix it soon, in the meantime you can fix it by substituting https://github.com/ros4hri/hri_fullbody/blob/master/nodes/detect#L100 with

           KeyValue(key="Last detected body ID", value=str(next(reversed(self.detected_bodies), ''))),

   Please note that in single_body=False mode, the package will not find all the bodies in an image per se but rather relies on an external body detector/tracker to provide the body IDs in /humans/bodies/tracked and relative cropped images in /humans/bodies/<body_id>/cropped. This kind of body detector is currently not part of the ROS4HRI released packages, so you would have to provide your own for this use case.

[chrisywong]

Hi @LukaJuricic

Thanks for the tips! I removed all the Debian packages and compiled everything from source and I no longer get that error and detection for a single person works. Unfortunately, now there's a problem with the TF tree between the upper and lower halves of the body. Detect works fine as it shows all the keypoints, but there is no tf between torso and body. I am using ikpy==3.2.2.

[LorenzoFerriniCodes]

Hi @chrisywong,

Could you please provide us the parameters you're using to run hri_fullbody?

[chrisywong]

Hi @LorenzoFerriniCodes ,

I'm launching ~/catkin_ws/src/hri_fullbody/launch$ roslaunch ./hri_fullbody_visualize.launch single_body:=true.

I modified some of the default parameters for the launch file, but mostly just the debug parameters.

For hri_fullbody_visualize.launch:

<launch>

    <arg name="rgb_camera" default="/camera/color/"/>
    <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"/>
    <arg name="depth_camera" default="/camera/depth/"/>
    <arg name="depth_camera_topic" default="$(arg depth_camera)/image_rect_raw"/>
    <arg name="depth_camera_info" default="$(arg depth_camera)/camera_info"/>

    <!-- console output of detected bounding boxes, meant for single body execution -->
    <arg name="textual_debug" default="true"/>

    <!-- publishes a TF tree of the joints, *without* performing IK kinematics to
         properly align the joints' frames to the kinematci 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"/>

    <include file="$(find hri_fullbody)/launch/hri_fullbody.launch" pass_all_args="true" />

    <!-- <node name="usb_cam" pkg="usb_cam" type="usb_cam_node">
        <remap from="/usb_cam/image_raw" to="$(arg rgb_camera_topic)"/>
        <remap from="/usb_cam/camera_info" to="$(arg rgb_camera_info)"/>
    </node> -->

    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find hri_fullbody)/rviz/fullbody_urdf_visualization.rviz" output="screen"/>

</launch>

For hri_fullbody.launch:

<launch>

    <arg name="rgb_camera" default="/camera/color/"/>
    <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="False"/>
    <arg name="depth_camera" default="/camera/depth/"/>
    <arg name="depth_camera_topic" default="$(arg depth_camera)/image_rect_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="False"/>

     <!-- 
      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>

</launch>

[LorenzoFerriniCodes]

I've tried to replicate the error using the same parameters, but everything seems to be working fine here. Could you try disabling the RobotModel plugin and try using the Skeleton 3D plugin?

[LorenzoFerriniCodes]

@chrisywong any update on this?

[chrisywong]

Hi @LorenzoFerriniCodes, I am currently away and won't be able to test for another 2 weeks. I will post an update once I can.

[chrisywong]

@LorenzoFerriniCodes For some reason, everything is back to the issue I initially had with the skeleton not matching the real pose in https://github.com/ros4hri/hri_fullbody/issues/4#issue-1837083307

It's weird because I did not change anything since the last time (other than performing regular updates). All the ROS4HRI packages are built from source and not from the Debian packages, and I am using ikpy==3.2.2. I currently no longer have the problem mentioned in https://github.com/ros4hri/hri_fullbody/issues/4#issuecomment-1670221676