Implement contact sensor component.

[ghost]

Fixes: #405

[michalpelka]

Looks good, I did not run it yet. Please adjust PR to review.

[michalpelka]

I can see an issue with this PR. Created contact is not equal to the collision. Here is an example:

Gripper is close to the cube that has ROS2ContactSensorComponent attached. The gripper is close to the cube, PhysX created contact (visible in PhysXDebugger, but it is not in collision).
But collision is reported in the message:

---
header:
  stamp:
    sec: 887
    nanosec: 106296000
  frame_id: block
states:
- info: ''
  collision1_name: 'ID: [1887469251207] Name:Block'
  collision2_name: 'ID: [1934713891463] Name:Ground'
  wrenches:
  - force:
      x: 0.0
      y: 0.0
      z: 0.001913785352371633
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  - force:
      x: 0.0
      y: 0.0
      z: 0.0011502510169520974
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  - force:
      x: 0.0
      y: 0.0
      z: 0.0019149517174810171
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  - force:
      x: 0.0
      y: 0.0
      z: 0.0011498068924993277
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  total_wrench:
    force:
      x: 0.0
      y: 0.0
      z: 0.006128794979304075
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  contact_positions:
  - x: 0.33652281761169434
    y: 0.022807355970144272
    z: 0.0
  - x: 0.33607718348503113
    y: -0.027190659195184708
    z: 0.0
  - x: 0.28607916831970215
    y: -0.026745017617940903
    z: 0.0
  - x: 0.28652480244636536
    y: 0.023252997547388077
    z: 0.0
  contact_normals:
  - x: 0.0
    y: 0.0
    z: 1.0
  - x: 0.0
    y: 0.0
    z: 1.0
  - x: 0.0
    y: 0.0
    z: 1.0
  - x: 0.0
    y: 0.0
    z: 1.0
  depths: []
- info: ''
  collision1_name: 'ID: [1887469251207] Name:Block'
  collision2_name: 'ID: [2020613237383] Name:panda_leftfinger'
  wrenches:
  - force:
      x: 0.0
      y: 0.0
      z: -0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  - force:
      x: 0.0
      y: 0.0
      z: -0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  - force:
      x: 0.0
      y: 0.0
      z: -0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  - force:
      x: 0.0
      y: 0.0
      z: -0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  total_wrench:
    force:
      x: 0.0
      y: 0.0
      z: 0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  contact_positions:
  - x: 0.3008630871772766
    y: -0.013886998407542706
    z: 0.07088780403137207
  - x: 0.3056322932243347
    y: -0.00013227888848632574
    z: 0.07104350626468658
  - x: 0.2937586307525635
    y: -0.0024294371251016855
    z: 0.07125639915466309
  - x: 0.2940542995929718
    y: -0.01370509434491396
    z: 0.07107652723789215
  contact_normals:
  - x: 1.860782504081726e-05
    y: 7.487325547117507e-06
    z: -1.0
  - x: 1.860782504081726e-05
    y: 7.487325547117507e-06
    z: -1.0
  - x: 1.860782504081726e-05
    y: 7.487325547117507e-06
    z: -1.0
  - x: 1.860782504081726e-05
    y: 7.487325547117507e-06
    z: -1.0
  depths: []
- info: ''
  collision1_name: 'ID: [1887469251207] Name:Block'
  collision2_name: 'ID: [2024908204679] Name:panda_rightfinger'
  wrenches:
  - force:
      x: -0.0
      y: -0.0
      z: -0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  total_wrench:
    force:
      x: 0.0
      y: 0.0
      z: 0.0
    torque:
      x: 0.0
      y: 0.0
      z: 0.0
  contact_positions:
  - x: 0.30162158608436584
    y: 0.03993232175707817
    z: 0.07134521007537842
  contact_normals:
  - x: -0.004613118711858988
    y: -0.6187670826911926
    z: -0.7855609655380249
  depths: []
---

[michalpelka]

I do not want to merge it until commented case would be resolved.

Error in gh iterface.

[ghost]

@michalpelka
regarding "Created contact is not equal to the collision":
The issue was due to the default values of the contact offset parameter (it sets the distance from the collider where collisions are detected). By default, its value is non-zero and it caused a contact creation in the above example, even though objects weren't in collision. Changing this value to 0.0(to detect contact only when objects collide) wouldn't resolve the problem because it must be greater than the rest offset, and changing it resulted in some other unrelated issues. The issue was resolved by implementing a filter for contacts that do not affect rigid bodies. The filter is based on the contact separation value.