Feature/implement grasp detector in grab

[P-ict0]

Merge after #1238

Implementation of active_grasp_detector in grab.py. Also added a retry.

[P-ict0]

From testing, seems like an error from the kinect.

  File "example_grasping.py", line 46, in <module>
    grab_state.execute()

  File "/home/rodtu/ros/noetic/system/src/smach/src/smach/state_machine.py", line 381, in execute
    container_outcome = self._update_once()

  File "/home/rodtu/ros/noetic/system/src/smach/src/smach/state_machine.py", line 269, in _update_once
    raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
smach.exceptions.InvalidUserCodeError: Could not execute state 'PREPARE_GRASP' of type '<robot_smach_states.manipulation.grab.PrepareEdGrasp object at 0x7f178c0cf8e0>': Traceback (most recent call last):

  File "/home/rodtu/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(

  File "/home/rodtu/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/manipulation/grab.py", line 55, in execute
    segm_res = self.robot.ed.update_kinect("%s" % entity.uuid)

  File "/home/rodtu/ros/noetic/system/src/robot_skills/src/robot_skills/world_model_ed.py", line 420, in update_kinect
    res = self._ed_kinect_update_srv(area_description=area_description,

  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)

  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 498, in call
    request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)

  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 105, in args_kwds_to_message
    return data_class(**kwds)

  File "/home/rodtu/ros/noetic/system/devel/lib/python3/dist-packages/ed_sensor_integration_msgs/srv/_Update.py", line 47, in __init__super(UpdateRequest, self).__init__(*args, **kwds)

  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 361, in __init__
    raise AttributeError('%s is not an attribute of %s' % (k, self.__class__.__name__))

AttributeError: fit_supporting_entity is not an attribute of UpdateRequest```

[PetervDooren]

@P-ict0 The reason CI was failing was because the tests are done using the Mockbot. so when we add a robot part to our hero python object we should also add it to the mockbot to remain compatible. I fixed this in b277700

[P-ict0]

A few things we must watch out for in the future: retrying grasp might not always be desired but we'll see when that becomes a problem. Some magic numbers are very hero-specific. but as long as we keep it configurable we should be good.

Yes, the retry option is defaulted to False for these cases. I think this is the correct way of approaching this.

[PetervDooren]

@MatthijsBurgh, your comments have been adressed. can we merge this PR now?

[MatthijsBurgh]

I have also created https://github.com/tue-robotics/tue_robocup/issues/1249. I think this is really important that we keep it structured how robot parts are added.

I sometimes get the feeling we are skipping the complex thoughts on how we keep the code organized for the sake of development. But in the end people are complaining code is messy, not robust, hard to maintain, not modular, etc. That is indeed what you get, when you skip the steps to keep your code organized.

I don't want to put a hold on development. So first get things working, then refactor it, so it is maintainable, etc. But this should be done before merging stuff into master. Because when it is in master, there is no motivation anymore to fix it.

So I am not sure, I accept this code in master yet. Lets have a discussion with everyone tonight how we want to balance between development and good, modular, robust, maintainable, etc. code.

[GustavoDCC]

From testing, I still have this error after add gripperpositiondetector to mockbot.

Traceback (most recent call last):
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/gustavo/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/manipulation/grab.py", line 55, in execute
    segm_res = self.robot.ed.update_kinect("%s" % entity.uuid)
  File "/home/gustavo/ros/noetic/system/src/robot_skills/src/robot_skills/world_model_ed.py", line 420, in update_kinect
    res = self._ed_kinect_update_srv(area_description=area_description,
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 498, in call
    request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds) 
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 105, in args_kwds_to_message
    return data_class(**kwds)
  File "/home/gustavo/ros/noetic/system/devel/lib/python3/dist-packages/ed_sensor_integration_msgs/srv/_Update.py", line 47, in __init__
    super(UpdateRequest, self).__init__(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 361, in __init__
    raise AttributeError('%s is not an attribute of %s' % (k, self.__class__.__name__))
AttributeError: fit_supporting_entity is not an attribute of UpdateRequest

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "example_grasping.py", line 46, in <module>
    grab_state.execute()
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 381, in execute
    container_outcome = self._update_once()
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 269, in _update_once
    raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
smach.exceptions.InvalidUserCodeError: Could not execute state 'PREPARE_GRASP' of type '<robot_smach_states.manipulation.grab.PrepareEdGrasp object at 0x7fad4477eca0>': Traceback (most recent call last):
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/gustavo/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/manipulation/grab.py", line 55, in execute
    segm_res = self.robot.ed.update_kinect("%s" % entity.uuid)
  File "/home/gustavo/ros/noetic/system/src/robot_skills/src/robot_skills/world_model_ed.py", line 420, in update_kinect
    res = self._ed_kinect_update_srv(area_description=area_description,
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 498, in call
    request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 105, in args_kwds_to_message
    return data_class(**kwds)
  File "/home/gustavo/ros/noetic/system/devel/lib/python3/dist-packages/ed_sensor_integration_msgs/srv/_Update.py", line 47, in __init__
    super(UpdateRequest, self).__init__(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 361, in __init__
    raise AttributeError('%s is not an attribute of %s' % (k, self.__class__.__name__))
AttributeError: fit_supporting_entity is not an attribute of UpdateRequest