[pose_broadcaster] Check for valid pose before attempting to publish a tf for it (backport #1479)

[mergify]

Deactivate the controller otherwise

When using the pose_broadcaster e.g. with mock hardware, the pose state interfaces might not contain any value (NaN), if there are no initial values inside the URDF. However, it might not make sense to set an initial value as this would be plain wrong information. Imagine a robot-calculated forward kinematics that generates that pose. Initializing this to any fixed value will just be wrong. IMHO it would be better to explicitly state that there is no valid information available.

Currently, attempting to publish a tf with no pose information will result in spamming the shell with

TF_NAN_INPUT: Ignoring transform for child_frame_id "tool0_controller" from authority "Authority undetectable" because of a nan value in the transform (nan nan nan) (nan nan nan nan)

In this PR I added a check for a valid pose before attempting to publish a tf and return an error in case of an invalid pose.

---

This is an automatic backport of pull request #1479 done by Mergify.

[mergify]

Cherry-pick of 67096bd has failed:

On branch mergify/bp/humble/pr-1479
Your branch is up to date with 'origin/humble'.

You are currently cherry-picking commit 67096bd.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   pose_broadcaster/src/pose_broadcaster.cpp
	modified:   pose_broadcaster/test/test_pose_broadcaster.cpp

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   pose_broadcaster/test/test_pose_broadcaster.hpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

[saikishor]

LGTM

Thanks for fixing the API calls

[codecov]

Codecov Report

Attention: Patch coverage is 66.66667% with 15 lines in your changes missing coverage. Please review.

Project coverage is 66.76%. Comparing base (b4d79ab) to head (85e89b3).
Report is 1 commits behind head on humble.

Files with missing lines	Patch %	Lines
pose_broadcaster/test/test_pose_broadcaster.cpp	54.83%	0 Missing and 14 partials ⚠️
pose_broadcaster/src/pose_broadcaster.cpp	91.66%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           humble    #1483      +/-   ##
==========================================
+ Coverage   66.74%   66.76%   +0.01%     
==========================================
  Files         113      113              
  Lines       13333    13376      +43     
  Branches     8246     8274      +28     
==========================================
+ Hits         8899     8930      +31     
  Misses       1438     1438              
- Partials     2996     3008      +12     

Flag	Coverage Δ
unittests	66.76% <66.66%> (+0.01%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
pose_broadcaster/test/test_pose_broadcaster.hpp	94.73% <100.00%> (+11.40%)	⬆️
pose_broadcaster/src/pose_broadcaster.cpp	79.16% <91.66%> (+2.69%)	⬆️
pose_broadcaster/test/test_pose_broadcaster.cpp	57.77% <54.83%> (-0.88%)	⬇️