Attached collision mesh visualizer error when attaching to non-active planning groups

[ebruun]

Using compas_fab v0.27.0 with GH/Rhino7 (pre-compiled compas_fab version sent over by @gonzalocasas )

I have a three robot work cell (R1, R2, R3) with separate planning groups. When I attach a collision mesh to the r1_tcp with R1 as the active planning group this is visualized fine, but if I then additionally attach a mesh to R2 I get the following error:

Solution exception:Link name r2_tcp does not exist in planning group

I'm sure this issue is with the FK function of the show_acm part of the visualizer component because everything on the ROS end is fine, i.e. the two meshes show up attached to their respective robots when viewed in RVIZ. Path planning also works fine taking into account both meshes.

Presumably this error is due the code not handling the fact that a mesh could be attached to a link in a non-active planning group.

Discussing with @gonzalocasas earlier and he implemented this quick fix (at line 97 of the GH component), but said that this should be addressed in a more robust way in the future:

######GONZALO MAGIC#########
fk_group = group
for g in robot.semantics.groups:
    if frame_id in robot.get_link_names(group=g):
        fk_group = g
        break

frame = robot.forward_kinematics(
    configuration, fk_group, options=dict(link=frame_id)
)
#############################