Invalid endpoint_state and FK

[gerardcanal]

Hi!
First of all, say that this happens in Ubuntu 18.04 with melodic, which I am not sure it's supported. I tried with a 16.04 vm and gazebo segfaults so I could not verify whether the issue is there too.
The simulation in 18.04 works well (there's one missing joint in the wrist, but rviz displays it properly).

The issue I am observing is that the endpoint_state topic /robot/limb/right/endpoint_state is always providing invalid poses, which are (0,0,0). I tracked this down to ArmKinematicsInterface::computePositionFK, where it seems that the KDL's JntToCart returns an invalid solution. I'm not familiar with KDL.

I find it quite strange that it's FK that fails (was expecting anything to not work but this). I am using the melodic-devel branch from sns_ik, although I guess that's not related to sns_ik.
Moreover, all TFs seem correct in rviz.

I am not sure about what can be going on, or whether it's KDL's, sawyer_simulator or if it's just not supported yet in 18.04.

[AdrianZw]

Hi Gerad!

Thanks for reporting this issue. If you are still interested in the Sawyer Gazebo simulation, you could try out the following Pull Request #52. It was tested for ROS Noetic but, I am quite confident that this branch will also fix the issue in melodic.