Huge commit adding fully working integration test

[Hugo-L3174]

added fully integrated walking+lidar+aicp, fixed exit conditions on tests, fixed install and dependencies

[nim65s]

Hi,

Thanks for this !

Currently, I can't test it, because robotpkg-ros-ouster-gazebo-simulation depends on a ros-control version that is not compatible with the one required by robotpkg-py27-talos-dev: we need the fork from PAL. I'll fix this on Monday, and try again.

[nim65s]

This shouldn't be required. The point of the robotpkg-py27-talos-dev package, is that this package contains nothing but comes with dependencies on everything we need. robotpkg-ros-ouster-gazebo-simulation is not yet included, that's true, but I'd prefer to fix that, instead of documenting a workaround.

[Hugo-L3174]

Ok, should I add the necessary dependencies in robotpkg-p27-talos-dev directly then? And even though robotpkg-ros-ouster-gazebo-simulation is not in it yet, shouldn't at least talos-simulation for example already be in it?...

[nim65s]

robotpkg-talos-simulation is an indirect dependency of robotpkg-py27-talos-dev. So installing robotpkg-py27-talos-dev is a sufficient condition to get robotpkg-talos-simulation.

[Hugo-L3174]

I added these installations because they were missing after installing robotpkg-sot-core-v3 and robotpkg-py27-talos-dev, which made the build fail. Perhaps ouster_gazebo_simulation was the only one really necessary on top of these two, I'll try again later.

[nim65s]

Hi @Hugo-L3174,

I have updated robotpkg-py27-talos-dev, and it should now come with all required dependencies.
But I can't test anything anymore, because I'm stuck with #18.

[nim65s]

Hi,

Trying this again, I had to remove gpu="true" (because Invalid parameter "gpu") and reduced="false" (because Invalid parameter "reduced") in head_ouster.urdf.xacro:155.

But this still doesn't work for me:

Error:   Failed to build tree: parent link [head_2] of joint [os1_base_mount_joint] not found.  This is not valid according to the URDF spec. 
Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this 
joint [os1_base_mount_joint] from your urdf file, or add "<link name="head_2" />" to your urdf file.

Am I doing something wrong ?

[Hugo-L3174]

What are you launching that gives you this error?

[nim65s]

rostest talos_integration_tests test_ouster_walking.test

[Hugo-L3174]

I'm guessing the error happens when the robot is spawning, can you try launching a roscore, and do
roslaunch talos_bauzil script_walking_bauzil.launch world_name:=bauzil_skins robot:=full_v2_ouster enable_leg_passive:=false
and: roslaunch talos_integration_tests talos_spawn_hs_ouster.launch ?

[nim65s]

If I do this, I have the exact same error, which appears in the log of the first roslaunch, but only after I run the second one.

[Hugo-L3174]

I think the issue comes from this change you made because if I change it this way I also get the "invalid parameter gpu" problems you had.
I am not sure the os1_64.urdf.xacro files both present in talos_bauzil and ouster_os1_64 are the same and you might actually need the latter.
I must say my installation is from the sources of talos_integration_tests, talos-bauzil, aicp_mapping, and ouster-gazebo-simulation

[nim65s]

You were right, this looks better now: I can see a talos with the lidar on its head inside the bauzil room (previously, the room here, but empty) :)

But we're still not done:

  File "/usr/lib/python2.7/unittest/case.py", line 329, in run
    testMethod()
  File "/ws/install/lib/talos_integration_tests/start_sot_ouster_walking.py", line 140, in runTest
    aicp_path = arospack.get_path('aicp_ros')
  File "/usr/lib/python2.7/dist-packages/rospkg/rospack.py", line 207, in get_path
    raise ResourceNotFound(name, ros_paths=self._ros_paths)

I'll add ros-aicp-mapping to py-talos-dev…

[nim65s]

Now I'm getting:

[/opt/openrobots/share/aicp_ros/launch/aicp_mapping.launch] is not a launch file name
  File "/usr/lib/python2.7/unittest/case.py", line 329, in run
    testMethod()
  File "/ws/install/lib/talos_integration_tests/start_sot_ouster_walking.py", line 149, in runTest
    roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)]
  File "/opt/ros/melodic/lib/python2.7/dist-packages/roslaunch/rlutil.py", line 107, in resolve_launch_arguments
    raise roslaunch.core.RLException("[%s] is not a launch file name" % args[0])

I guess this is caused by Gepetto/aicp_mapping#5

[Hugo-L3174]

Yes I forgot to make a pull request on some changes I made in the CMakeLists.txt files for aicp_mapping, but they were just supposed to export and install the files correctly. Without that you might get some of these messages because it can not find the files in the install folders...

[nim65s]

I can now see Talos walking in bauzil Room.

Is this what I'm supposed to see ?
The test also say * RESULT: SUCCESS.

[Hugo-L3174]

The success of the test comes if the robot reaches the right position (here I only evaluated x and z so it does not matter how long he walks as long as it is in a straight line). The principle is the same as in the online_walking tests. What you see on rviz are the transforms, but you are also supposed to see the map building itself.
The issue probably comes from the fact that the rostest framework launches itself on a random rosmaster (to be able to launch several tests at once) so here the nodes can't communicate. Try adding the option --reuse-master to the rostest command line and you should get the map.

[nim65s]

Running roscore in a shell and rostest --reuse-master talos_integration_tests test_ouster_walking.test gives exactly the same result in my case, including a RLException: roscore cannot run as another roscore/master is already running.

[Hugo-L3174]

The roscore error you get when running the script is normal, it just tries to launch a roscore but won't if there is already one. It is just a warning but won't change anything. Could you get in your workspace and do :

cd install/lib/talos_integration_tests
python start_sot_ouster_walking.py

preferably after running a roscore in a separate terminal.
After doing this you should be able to get more output in the terminal, including aicp_mapping.

[nim65s]

This gave me more logs, but I don't see anything useful: https://framabin.org/p/?c1bc04b2003397e1#jx0nAJjyhR3UiUgFiXU3YIj62SIwRP/OO/Eynfo8VhY=

[Hugo-L3174]

Is aicp outputting an error on your terminal? Like pose not initialized or map service disabled?
And can you check with rostopic list and rostopic echo -1 that /os1_cloud_node/points and /aicp/output_map exist and are being published?

[nim65s]

/os1_cloud_node/points is published, but not /aicp/output_map. I have aligned_cloud, aligned_map, pose_corrected, poses, & prior_map

[Hugo-L3174]

You are right I meant /aicp/aligned_map. Is it not being published? Is it ticked in the left section of rviz (called output map here)?
If so the map should be building every two reference points in the robot's trajectory. You can also tick Velodyne if you want to see the continuous point cloud being sent by the simulated lidar

[nim65s]

Everything (including output map) is ticked, except Velodyne, and when I tick it, I get a segfault, and both RVIZ & gazebo close.