ros2: update services and launch files to enable PX4 example #27
Conversation
srmainwaring
force-pushed
the
prs/pr-ardupilot-launch
branch
6 times, most recently
from
685fe65
to
60a632f
Compare
srmainwaring
changed the title
srmainwaring
force-pushed
the
prs/pr-ardupilot-launch
branch
from
60a632f
to
0f6d23f
Compare
srmainwaring
force-pushed
the
prs/pr-ardupilot-launch
branch
2 times, most recently
from
525552b
to
1799d72
Compare
There was a problem hiding this comment.
Just a few questions so far.
For all the commits satisfying code tools, is there a way to enforce it in CI now that it's clean?
Right now, CI doesn't pick up any tests, because colcon doesn't have any test_require fields: https://github.com/ethz-asl/terrain-navigation/pull/27/checks#step:7:8
I saw some changes for indentation, include header ordering, and some missing includes. These are all great things to prevent regression in future contributions.
I also like you included the ArduPilot changes.
srmainwaring
force-pushed
the
prs/pr-ardupilot-launch
branch
from
619c4bb
to
505ea46
Compare
There was a problem hiding this comment.
Thanks for the monumental effort!
Just one minor use regarding the rviz_run, and then everything looks quite nice, and even better with the improvements!
I have some questions, out of curiosity regarding style
- There seems to be some style changes, but the styles were formatted with clang-format-6.0 for ROS1. Could you maybe share the exact style format you are using?
- Some of the inline implementations seems to have now gotten a cpp file. Could you maybe clarify why this is more preferred? I am fine with the changes, was just wondering why this would be better
| <param name="tif_path" value="$(find-pkg-share terrain_navigation_ros)/resources/davosdorf.tif"/> | ||
| <param name="tif_color_path" value="$(find-pkg-share terrain_navigation_ros)/resources/davosdorf_color.tif"/> | ||
| <remap from="elevation_map" to="grid_map" /> | ||
| </node> |
There was a problem hiding this comment.
Actually this lanuchfile was intended for use of a ground station. Therefore it is better to get the elevation map from the planner, so that we know what the planner is using compared to loading it separately with the tif_loader
There was a problem hiding this comment.
@Jaeyoung-Lim - is your suggestion to remove the test_tif_loader node completely or just remove the parameters?
Style changes are probably not intended. Some change, such as comments around unused arguments would be for clang. Line length is probably my changing from habit - working on Gazebo where there is a strict line length policy. If there is a formatter you like run I'd be happy to - @Ryanf55 do you know what is standard for ROS 2 these days?
A couple of reasons. The main one is that the header only versions lead to duplicate symbols in clang. The other reason is that inline code with branch conditions seldom gets compiled inline. Same if the functions are virtual. If the section is critical for performance then inlining may make sense, but most of the time placing the code in a translation unit is better and allows a smaller set of includes to be used in the header (which propagate) with most of the details in the cpp. |
clang-format with google style guide. ethz-asl/grid_map_geo#45 I have no perference if you want to fix formatting now or later; depends if it's going to create a huge diff or not. If the old code was formatted with version 6 or something, it would be best to preserve the old style in this PR, and then migrate versions in a follow up PR once we can enforce it in CI and ignore a single reformat commit to reduce code churn noise. |
|
I got up to the part of launching terrain navigation but there is a problem with getting the origin. planner log |
- Update mavros launch file for ardupilot - Update rviz config to use descriptive names for markers - Add groups in rviz config - Update use of mesh resource in publish vehicle pose - Use file:// prefix on fully qualified resource path. - Update rviz config file to display paths by defaults Signed-off-by: Rhys Mainwaring <[email protected]>
- Update launch file for ardupilot - Install config folder. - Add Python launch file for mavros. - Add ardupilot sitl node. - Update mavros launch file for ardupilot - Update async service calls - Change QoS to best effort for some mavros subscriptions - Move and install resource directory - Add latching qos for grid map. - Update launch file for terrain planner - Fix typo in resources path. - Add Python launch file for visualize_path. - Use mode GUIDED for off-board control - Update formatting - Update terrain planner launch to use quadplane - Add sample mission - Move implementation of geo_conversions and visualization into cpp - Update use of mesh resource in publish vehicle pose - Use file:// prefix on fully qualified resource path. - Add todo notes to terrain_planner_ros - Add separate launch file for running ardupilot sitl - Allow the terrain planner to be run in isolation - Update planning duration calculation - Ensure rclcpp::Time variables are initialised to consistent time sources - Update example mission to include NAV_LOITER_UNLIM required by planner - Add further comments and reduce debug print output - Accept all NAV_LOITER command codes in terrain planner - Print global origin details - Update home location in launch scripts - Add px4 config to mavros launch script - Sync guidance constants with values used for px4 standard_vtol - Disable sitl_dds in terrain planner launch - Declare altitude control variables as node parameters - Declare params before topics in terrain planner node - Add alt control launch args to terrain planner launch script - Clean up terrain planner ros launch files - Add px4 loiter mission for davos - Remove unused publishers - Document the reason for the conversions used in the global origin callback. - Simplify the example ArduPilot mission. - Add takeoff to ArduPilot davosdorf mission. - Add Python node to relay from /mavros/set_point/global to /ap/cmd_gps_pose. - Update relay from /mavros/set_point/global to /ap/cmd_gps_pose. Signed-off-by: Rhys Mainwaring <[email protected]>
- Build standalone example. - Add test rviz config for plugin. - Fix unused variable warning. - Remap grid_map_geo elevation_map to grid_map used in mav_planning_rviz. - Use display context to set fixed frame in mav_planning_rviz - Use mode GUIDED for off-board control - Add debug logging for interactive markers - Update formatting in goal_marker - Add mutex to planning panel - Move inlined functions to cpp in goal_marker - Revert plugin to issue px4 offboard commands Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Use Rviz node for interactive markers This solves the problem of interactive markers not registering properly in rviz
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
srmainwaring
force-pushed
the
prs/pr-ardupilot-launch
branch
from
505ea46
to
4657ff6
Compare
|
@Jaeyoung-Lim and @Ryanf55, do you require any further changes to get this PR merged? I've run ./Tools/fix_code_style.sh ..on the repo, and it runs clean, although for clang-format @Ryanf55 I'd prefer to get this PR merged and then apply your two changes as a follow up, as they need a rebase and this |
We can select clang-format likely - if it minimizes code churn. Yea, feel free to merge this. I wasn't able to get it running all the way, but so far it's a big improvement. |
|
@srmainwaring Unfortunately on myside it doesn't seem to progress after requesting for origin messages: However, probably we should address this in following PRs! Thanks for all the work! |
This PR updates services and launch files in the terrain planner node and rviz plugin to allow the ROS 2 port to run a PX4 example.
Figure: planned paths and the planning tree are now visible
Details
Issues
Other changes
GeographicLib
If you do not have the geoids for GeographicLib install the following:
Usage PX4
The testing uses this branch of PX4 : https://github.com/srmainwaring/PX4-AutoPilot/tree/prs/pr-hinwil-testing-rebased as it contains a small fix to the height rate setpoint forward ported to main, and a DEM for Gazebo of the region used in the terrain planner example.
Start QGC:
Start a PX4 SITL session with Gazebo:
standard_vtol_0and select 'Move To' to centre the camera on the plane.Load the example mission:
terrain_navigation_ros/config/davosdorf_mission.planFigure: Gazebo and QGC in loiter
Launch the terrain planner, mavproxy and rviz nodes in separate terminals. These can be combined but it helps with debugging to run them separately:
Set the start and goal positions. If the interactive marker server is not updating correctly this may done using service calls:
Trigger the planner and set the planner to navigate. The equivalent ROS 2 service calls are:
Figure: rviz after planning
Finally engage the planner by switching the plane to OFFBOARD mode. Using mavros directly this is:
Figure: Gazebo and QGC at the goal
Usage ArduPilot (partial support)
There is work in progress to support ArduPilot, however the ArduPilot plane navigation code does not currently support the an external control mode that works with the global position set points emitted by the planner
Dependencies
There is currently a dependency on the launch scripts for ArduPilot DDS. These should be installed following the instructions here: https://github.com/ArduPilot/ardupilot/blob/master/Tools/ros2/README.md.
The ArduPilot dependencies may be installed in the same workspace as terrain navigation or a separate one. If the latter the ArduPilot workspace should be sourced in addition to the terrain navigation workspace when running examples.
Running
Launch files for each node have been provided to aid debugging. These need to be started in separate terminals.
Launch the
ardupilot_sitlnode:Launch the terrain planner node:
Launch the
mavrosnode:Launch
rviz:Start an interactive
mavproxysession:Use the mavproxy mission editor to load the example mission from
./src/terrain_navigation_ros/config/davosdorf_mission.txt. The mission comprises a VTOL takeoff, a waypoint and an unlimited loiter.In mavproxy, switch to
AUTOand arm:Wait for the map to load in rviz. If it does not appear use the
Load Terrainbutton. The console should report details such as:In another terminal call services to set the start and goal:
$ ros2 service call /terrain_planner/set_start planner_msgs/srv/SetVector3 "{vector: {x: 1570, y: -330, z: -1}}"A green circle near the vehicles loiter position should appear.
$ ros2 service call /terrain_planner/set_goal planner_msgs/srv/SetVector3 "{vector: {x: -100, y: -200, z: -1}}"A green circle on the mountain near the top of the snow should appear.
The terrain planner console should show:
Call the loiter service
ros2 service call /terrain_planner/set_start_loiter planner_msgs/srv/SetService "{}"Put the planner in its HOLD state:
ros2 service call /terrain_planner/set_planner_state planner_msgs/srv/SetPlannerState "{state: 1}"Switch the flight controller to GUIDED:
ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: GUIDED}"Trigger the planner (the z-component of the vector3 is the planning budget time in seconds):
ros2 service call /terrain_planner/trigger_planning planner_msgs/srv/SetVector3 "{vector: {z: 10.0}}"Console output:
Put the planner in its NAVIGATE state:
ros2 service call /terrain_planner/set_planner_state planner_msgs/srv/SetPlannerState "{state: 2}"Verify the setpoints are being published: