head behavior: add collision checking module

[timonegk]

Proposed changes

This pull request introduces a cpp python module that can check for collisions using MoveIt. When a collision occurs, the head stops moving. In general, it works very well. There are still two issues:

• The collision model of the head should be adapted to include the part between the sides just in front of the camera. Currently, the head can get stuck when a bumper is in that space because it can no longer move away.
• Collisions with the arms are not detected yet. This would be possible by listening to /joint_states.

The PR is ready to be tested on the robot (I tested it in rViz).

Related issues

The pull request closes #55.

Necessary checks

• Update package version
• Run catkin build
• Write documentation
• Create issues for future work
• Test on your machine
• Test on the robot
• Put the PR on our Project board

[NFiedler]

When compiling, I get the following warning:

Warnings << bitbots_head_behavior:check /home/nfiedler/documents/Bit-Bots/catkin_ws/logs/bitbots_head_behavior/build.check.004.log
CMake Warning at /usr/share/cmake-3.10/Modules/FindBoost.cmake:1626 (message):
No header defined for python3; skipping header check
Call Stack (most recent call first):
CMakeLists.txt:7 (find_package)

When testing it on my machine in RViz, it seems like the robot is stuck in one position... so I get:
[WARN] [1605262043.620775]: Colliding head position: 1.1254772796251122, -1.2. Not moving.

continuously... The only way to break out of it is to move the ball into the field of view again and moving it slowly out of it. What I want to say is that returning to looking for the boll doesn't seem to work once the robot is stuck.

Additionally I'd like to see some comments in the C part, but it is so straightforward that I wouldn't know where exactly...

[NFiedler]

Oh, and the package version of the blackboard is not updated.

[SammyRamone]

When compiling, I get the following warning:

Warnings << bitbots_head_behavior:check /home/nfiedler/documents/Bit-Bots/catkin_ws/logs/bitbots_head_behavior/build.check.004.log
CMake Warning at /usr/share/cmake-3.10/Modules/FindBoost.cmake:1626 (message):
No header defined for python3; skipping header check
Call Stack (most recent call first):
CMakeLists.txt:7 (find_package)

When testing it on my machine in RViz, it seems like the robot is stuck in one position... so I get:
[WARN] [1605262043.620775]: Colliding head position: 1.1254772796251122, -1.2. Not moving.

continuously... The only way to break out of it is to move the ball into the field of view again and moving it slowly out of it. What I want to say is that returning to looking for the boll doesn't seem to work once the robot is stuck.

Additionally I'd like to see some comments in the C part, but it is so straightforward that I wouldn't know where exactly...

can confirm that the robot gets the same warning during compiling. Could not test it on the robot, since the vision sees no ball, would need the help of some vision people, @johagge @Flova @jaagut

[Flova]

Some comments from my side. Correct me if I am wrong (I'm still in bed and tired af) but the head also collided some times even if it looked okay in rviz. This happened maybe due to the sea and definitely due to the head beeing stuck in der cables over the shoulder that are not part of the collision model.
Furthermore a more advanced handling of such collision events is required. For ball tracking stoping at this position and waiting until the ball reappears isn't ideal but e.g. it the head pattern gets stuck if it's not able to move. A more sufisticated way would include the calculation the last non colliding point to get as close as possible to the desired location.

But thanks @timonegk for this cool implementation!

[SammyRamone]

included small config change to solve bit-bots/bitbots_main#278 for now

[SammyRamone]

Some comments from my side. Correct me if I am wrong (I'm still in bed and tired af) but the head also collided some times even if it looked okay in rviz. This happened maybe due to the sea and definitely due to the head beeing stuck in der cables over the shoulder that are not part of the collision model.
Furthermore a more advanced handling of such collision events is required. For ball tracking stoping at this position and waiting until the ball reappears isn't ideal but e.g. it the head pattern gets stuck if it's not able to move. A more sufisticated way would include the calculation the last non colliding point to get as close as possible to the desired location.

But thanks @timonegk for this cool implementation!

the cables are no problem when they are correctly fixed.
I also think that going to the last valid solution would be better than just stopping

[SammyRamone]

I can confirm the bug that @NFiedler described. when the ball is lost and the next search pattern position would be invalid, the head behavior is stuck until it sees a ball again. this definitely needs to be fixed
Otherwise it seemed to work. There is still the issue with the SEA in the head which leads to some uncertainties with collision checking. maybe also check for having +-5° on the head tilt. and only if all are okay accept
Listening to /joint_states to get the right arm position is also cruatial, otherwise it collides with the shoulders, as it assumes them to be in init

[SammyRamone]

did not test it. looks okay. there are more sophisticated methods than just skipping one search position, but I guess it will be sufficient and not worth the extra work

[timonegk]

This is ready to re-review and test now. It now takes all joint positions into account (especially the arms are crucial, as @SammyRamone pointed out). I also used an existing helper function for roscpp initialization because otherwise, the node did not terminate properly.

[SammyRamone]

@timonegk did you test it in simulation? If yes I think you can merge this, as we will not be able to test anything on the robot in the next time.

[timonegk]

By the way, the cmake warning seems to be a known problem for bionic and can be ignored.

[timonegk]

I just tested in simulation and it works fine with the changes in bit-bots/wolfgang_robot#89.

[jaagut]

@timonegk Do you want @SammyRamone to re-review it or do you wait on @NFiedler feedback?

[timonegk]

Users of packages.bit-bots.de might need to execute sudo apt install --reinstall ros-melodic-moveit-ros-planning-interface for this to work.

[Flova]

I get this warning and the robot does nothing:

[WARN] [1614278802.280998, 7390.528000]: Colliding head position: 0.2633780642070785, -0.25961972788379833. Not moving.
[WARN] [1614278802.348587, 7390.592000]: Colliding head position: 0.2633780373597982, -0.25961978968238997. Not moving.

[jgueldenstein]

@Flova you need to pull wolfgang_robot

[jgueldenstein]

tested and working in simulation

old

[Flova]

I was on the newest version. Timon checked the hash.