[cob_obstacle_distance] Use MoveIt!-based cob_obstacle_distance_moveit (cob_manipulation)

[fmessmer]

I'm separating this into a new issue from #77

MoveIt!-based approach is proposed in ipa320/cob_manipulation#64 👉 merged!

ToDo:

• separate message/service definitions into "neutral" location (e.g. cob_common) 👉 done in Moving msgs into new package #100 use messages and services from new package cob_control_msgs ipa320/cob_manipulation#66
• use "neutral" message/service definitions within cob_obstacle_distance, cob_obstacle_distance_moveit and cob_twist_controller 👉 done in Moving msgs into new package #100 use messages and services from new package cob_control_msgs ipa320/cob_manipulation#66
• use cob_obstacle_distance_moveit together with cob_twist_controller
  (replacing cob_obstacle_distance)
  • adjust service and topoic names in both cob_twist_controller and cob_obstacle_distance_moveit (i.e. use remap in the launch file provided in prepare obstacle_distance_monitor launch file ipa320/cob_robots#460)
  • remove component-specific instance of cob_obstacle_distance from generic_cartesian_controller.launch file, i.e. this line
• deprecation of cob_obstacle_distance package (optional)

@fmessmer FYI

[MaChristoph]

The following steps are necessary for starting the cob-simulation with the MoveIt distance computation.

1. roslaunch cob_bringup_sim robot.launch
   This command is used for starting the simulation environment.
2. roslaunch cob_obstacle_distance_moveit obstacle_distance.launch
   Start the MoveIt based distance computation node
3. rviz
   Start the visualization environment
4. rosservice call /register_links "data: 'arm_right_7_link'"
rosservice call /unregister_links "data: 'arm_right_7_link'"
   Register/Unregister a link which should be considered in the distance computation
5. rosrun cob_obstacle_distance_moveit visualize_obstacle_distance_node
   Visualize the distance arrows in rviz (Add a MarkerArray in rviz with topic "/obstacle_distance/distance_markers")
6. rosrun cob_obstacle_distance_moveit test_obstacle_publisher_node
   Publish a obstacle (collision object) into rviz.

[fmessmer]

Work on integration is started in https://github.com/ipa320/cob_control/pull/104 and ipa320/cob_robots#467 by @ipa-bfb and @ipa-fxm-cm

[fmessmer]

As discussed with @ipa-bfb maybe we should investigate using cob_obstacle_distance_moveit as a component-specific node first in order to see whether cob_twist_controller can handle the lot more obstacle distances being published (in comparison to what cob_obstacle_distance was publishing)...

If this works, next step would be to see whether it also does work if cob_obstacle_distance_moveit is used as a global instance, i.e. serving all component-specific cob_twist_cotroller instances. Maybe a filter mechanism then needs to be implemented within cob_twist_controller (not cob_obstacle_distance_moveit) in order to filter only the relevant obstacle distances from the topic!

[fmessmer]

Recording feedback from @ipa-fmw and @ipa-mdl after hardware tests - which resulted in removing cartesian_control from robot.xml atm (see e.g. ipa320/cob_robots#593 (comment))

This gets more and more important as (non-moveit) cob_obstacle_distance produces heavy load on the hardware as each component requires its own instance! The high load results in canopen drivers not able to achieve sync times...

To solve known issues with the (moveit-based) cob_obstacle_distance_moveit, i.e. "do not publish obstacle distances of the whole robot to each component, but only the relevant", a selective per-component publisher might be used...

[fmessmer]

@ipa-fxm-cm
please verify whether cob_obstacle_distance_moveit has the same interfaces as cob_obstacle_distance so that cob_twist_controller does not need to be adjusted.
then we should make sure that - in a first test - cob_obstacle_distance_moveit only returns distance information for the same "pairs" as cob_obstacle_distance, i.e. just those...not all...

[fmessmer]

@ipa-fxm-cm implemented another approach based on collision spheres...
@ipa-bfb where can this be found...any intention to make that part a standalone package, i.e. separate it from nmpc package?

[bbrito]

Well the other approach is just for self-collision avoidance and is specific from cob_nmpc which is still not ready to be delivered..

We should keep cob_obstacle_distance.. Maybe add a few documentation about how to use it and what is doing...