Avoidance Interface CI

[mrivi]

This PR is an attempt to get an automated test on the avoidance interface going to check if any future PR would brake the interface.

Test: the vehicle will fly a mission where flying a direct line between the two waypoints leads to a collision with a wall. The avoidance maneuver is simple enough to assure that the local_planner can always find a path around the wall. If the vehicle cannot finish the mission before a timeout, the test fails.

Current Work in Progress: I need to setup the catkin environment for the avoidance to run. All the steps are done in the script rostest_avoidance_run.sh. I see that in the other ROS tests the equivalent script is called here. How can I change it to run rostest_avoidance_run.sh for the avoidance test?

Future Work: we can use the Gazebo collision topic to detect if the vehicle has crashed

[lamping7]

@mrivi I see a couple options to make this work.

1. Don't use the same test node function that generates the SITL tests. We can define the stage manually.
   • The purpose of that function was to remove repeated code and make it easier to make changes to all the tests.
2. Add another parameter to the map that is used as arguments to create the SITL test node.
   • This parameter would specify the script to use to run the test (rostest_avoidance_run.sh in this case or rostest_px4_run.sh for the others).

I'm leaning toward option 1, but have no problem with option 2. 2 is better if you need all the other things that go on after the test runs, such as upload to Flight Review and run the ECL script. I'm not thinking you need or want all the other things, which is why I'm thinking option 1. You just want to run the test, correct?

[mrivi]

@lamping7 I think point 2 is more future proof in case we want to have more complicated tests. For the test we have to run now point 1 is enough. Does point 2 require much more effort?

[lamping7]

@mrivi I did option 2. Jenkins is fine with the test now, but there seems to be other issues. Take a look at http://ci.px4.io:8080/blue/organizations/jenkins/PX4_misc%2FFirmware-SITL_tests/detail/pr-avoidance_ci_rebased/13/pipeline/82 and https://logs.px4.io/plot_app?log=69c95f98-26a4-4d87-a739-73c6baa97fc2

[mrivi]

@lamping7 thank you!!!

I think the problem is that the avoidance is not enabled (param MPC_OBS_AVOID is not set to 1). I'll quickly try to set that in the iris config. I think we need another iris vehicle config (like those *_iris_vision, *_irsi_opt_flow) to set the parameter.

[lamping7]

I'm seeing this in the output:

Laser Plugin (ns = )  <tf_prefix_>, set to ""
Aborted (core dumped)

and

gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::rendering::DepthCamera; typename boost::detail::sp_member_access<T>::type = gazebo::rendering::DepthCamera*]: Assertion `px != 0' failed.

[mrivi]

@lamping7 @dagar avoidance test is now working! thank you for the help!
I'll put the param set for MPC_OBS_AVOID on a different target than the normal iris probably tomorrow.

[lamping7]

Awesome. The Jenkins failure is another data link lost issue.

[mrivi]

@dagar good to merge?

[lamping7]

I'm good with all the Jenkins details, but have questions about the .plan and mission. I pulled up the log from the last run in Jenkins.

• From what I can see the vehicle gets airborne, and only flies for about 10 sec in a straight line. A mission this short causes problems with the ECL script. Can we make this longer?
• Is the avoidance causing it to drop in altitude at the end?
• Sorry, I haven't used the avoidance. Where is the obstacle defined (the world?) and where is it in relation to the flight path?
• Do you have a log and a screenshot of the completed flight path from QGC so we know what to expect to see here?
• Do we want the vehicle to land so that the log gets closed cleanly?

[dagar]

avoidance -> MC_avoidance?

[dagar]

This is fine for now, but we should try to think of a better way to do this. For example having it locally and running repeatedly shouldn't have to clone and build each time.

[mrivi]

@lamping7 fair points.

• I have modified the avoidance.plan to include lading. This is what it does locally on my machine. Lets check if it matches the output on the ci
  

• the obstacle is defined in this world file that is in the avoidance repository

[lamping7]

1. Can you use 5 meters for takeoff alt to match other MC tests?
2. The takeoff altitude isn't the problem.
   • [ERROR] [1541060312.830477210, 44.581000000]: FCU: Failsafe enabled: no global position
   • gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::rendering::DepthCamera; typename boost::detail::sp_member_access<T>::type = gazebo::rendering::DepthCamera*]: 'Assertion px != 0' failed.
   • I'd probably set the gazebo verbose flags in the launch file for testing to see what is going on.

[mrivi]

@lamping7 before there was a mission check failure on the takeoff altitude. I can increase it but I also need to modify the obstacle.
Let me first get the output from avoidance on the console.

[lamping7]

@lamping7 before there was a mission check failure on the takeoff altitude. I can increase it but I also need to modify the obstacle.

Yes, I saw that, but I'm thinking it was a result of not having global position. You don't need to increase it. I was just suggesting it for consistency. I haven't pulled up the world to visualize it, so I didn't know if you could or not easily.

[mrivi]

@lamping7 I can easily update the model. I'll do that. As for the mission height, I got the error even on my machine when loading the mission file. I think I messed up when adding the landing waypoint. That's way I changed the mission plan.

I am seeing that the iris model doens't spawn correctly.

�[0m[ INFO] [1541088548.339971916]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...�[0m
�[0m[ INFO] [1541088548.943145305, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.�[0m
�[0m[ INFO] [1541088549.020836028, 0.097000000]: Physics dynamic reconfigure ready.�[0m
�[0m[ INFO] [1541088550.279815894, 0.202000000]: Laser Plugin: The 'robotNamespace' param was empty�[0m
�[0m[ INFO] [1541088550.279899295, 0.202000000]: Starting Laser Plugin (ns = iris_obs_avoid)�[0m
�[0m[ INFO] [1541088550.290261437, 0.202000000]: Laser Plugin (ns = iris_obs_avoid)  <tf_prefix_>, set to "iris_obs_avoid"�[0m
re:      00000001�[0m 

The camera plugin starts after the vehicle already has attempted to takeoff. Hence all the warnings that a point cloud has not been received by the avoidance.

[mrivi]

@TSC21 , have you ever had this error in gazebo?
gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::rendering::DepthCamera; typename boost::detail::sp_member_access<T>::type = gazebo::rendering::DepthCamera*]: 'Assertion px != 0' failed.

On my machine I have gazebo 7 (instead of 9, installed in the container) and I don't have this issue.

[TSC21]

@mrivi going to try on my side and let you know

[mrivi]

@TSC21 @lamping7 I run the contains on my laptop and retrieve the spwan log

[xmlrpc][INFO] 2018-11-23 11:50:54,698: XML-RPC server binding to 0.0.0.0:0
[xmlrpc][INFO] 2018-11-23 11:50:54,698: Started XML-RPC server [http://9a1b26ca3e60:44181/]
[rospy.impl.masterslave][INFO] 2018-11-23 11:50:54,698: _ready: http://9a1b26ca3e60:44181/
[xmlrpc][INFO] 2018-11-23 11:50:54,698: xml rpc node: starting XML-RPC server
[rospy.registration][INFO] 2018-11-23 11:50:54,699: Registering with master node http://localhost:11311
[rospy.init][INFO] 2018-11-23 11:50:54,702: ROS Slave URI: [http://9a1b26ca3e60:44181/]
[rospy.init][INFO] 2018-11-23 11:50:54,703: registered with master
[rospy.rosout][INFO] 2018-11-23 11:50:54,703: initializing /rosout core topic
[rospy.rosout][INFO] 2018-11-23 11:50:54,705: connected to core topic /rosout
[rospy.simtime][INFO] 2018-11-23 11:50:54,712: initializing /clock core topic
[rospy.simtime][INFO] 2018-11-23 11:50:54,716: connected to core topic /clock
[rosout][INFO] 2018-11-23 11:50:54,721: Loading model XML from Gazebo Model Database
[rosout][INFO] 2018-11-23 11:50:54,721: Waiting for service /gazebo/spawn_sdf_model
[rosout][INFO] 2018-11-23 11:50:54,724: Calling service /gazebo/spawn_sdf_model
[rospy.internal][INFO] 2018-11-23 11:50:54,734: topic[/clock] adding connection to [http://9a1b26ca3e60:35775/], count 0
[rospy.internal][INFO] 2018-11-23 11:50:54,882: topic[/rosout] adding connection to [/rosout], count 0
[rosout][INFO] 2018-11-23 11:50:55,604: Spawn status: SpawnModel: Successfully spawned entity
[rospy.core][INFO] 2018-11-23 11:50:55,605: signal_shutdown [atexit]
[rospy.internal][INFO] 2018-11-23 11:50:55,609: topic[/rosout] removing connection to /rosout
[rospy.internal][INFO] 2018-11-23 11:50:55,609: topic[/clock] removing connection to http://9a1b26ca3e60:35775/
[rospy.impl.masterslave][INFO] 2018-11-23 11:50:55,609: atexit  ```

It seems that the model was successfully spawned

[mrivi]

I rebased because I realized that the OBS_AVOID parameter wasn't up to date. It switched from MPC_OBS_AVOID to COM_OBS_AVOID in #11550

[nicovanduijn]

If we want to merge our efforts, you could cherry-pick 70c9c78 (Which will of course introduce a dependency on PX4/PX4-SITL_gazebo-classic#300)

[mrivi]

It has been long six months to get this PR working so I am not going to change anything at the moment. This is a good first step and I would like to keep the improvements in a separate PR.
@dagar could you please review it? I really would like to see this merged rather sooner than later.

[nicovanduijn]

Ok, I'll work on top of this then. Let's get it in

[dagar]

Thanks everyone!