Implement collision checker node

[DaikiMaekawa]

#36 Implement new node for stop motion when collisions are detected.

Do not merge yet !!

[DaikiMaekawa]

We need to add the code for stopping the arms.

[130s]

Copyrights mismatch

[DaikiMaekawa]

Oh, It's copy/pasted from the other program. That is set right here.

[130s]

We need to add the code for stopping the arms.

Adding some background to this;

@DaikiMaekawa and I discussed about what and how we want to address #36, and took this approach: let MoveIt! detect self-collision. When a collision is found then give the "stop" signal by overwriting the command with the current pose of very instantaneous moment.

Testing on the real robot, however, we found that the "stop" signal didn't interrupt the previous command (I think that's why the lines around here are commented out in this PReq).

Another concern in this approach is that there's no effect to the lower level hrpsys commands.

Our conclusion with this PReq is that:

• We want to run low-level, hrpsys "stop" command.
  • A solution idea
• Using MoveIt! as collision detection is good, since we're switching default programming interface to ROS-MoveIt! (work in progress ROS client now inherits RobotCommander class from MoveIt! as a default setting. #299).

[DaikiMaekawa]

Our checked this node behavior in simulation.

It was completed to detecting collisions, but the stop motion is not implement.