Visit here for instruction video for SITL:
https://www.youtube.com/watch?v=9QvqmMlA_oY
2021/6/30: CmakeList.txt update for noetic build (Qt5) 2020/1/17: I configured launch files for airsim simulation.
This package is a module for px4 position control from user input or trajectory planner. Currently, SITL (refer mavros and px4 for details) with Gazebo and HITL (real hard ware) were validated. The package implemented two nodes: 1) gcs with gui(mav_gcs_node) and 2) wrapping module(mav_wrapping_node).
mav_gcs_node features various service clients for initialization or re-initalization of setpoint for the case of keyboard control (initialize homing button in gui), arming and disarming in offboard control (Arming button) and offboard mode change for px4(Setoffboard and flight). Also, it alternates setpoint publish mode between keyboard and planner by keyboard & planner button .
mav_wrapper_node overrides the local position of px4 with external pose (/mavros/vision_pose/pose). The external pose will be fused with IMU of px4 to yield /mavros/local_position/pose which will be used for position control in offboard mode. Also, it provides clients for the mentioned service in mav_gcs_node in onboard computer side.
The px4_code equipped with mav_gcs_node and mav_wrapper_node will help you in the following cases: 1) real world MAV position control in either motion capture system or VIO (ZED stereo camera VIO was tested in my hardware environment) 2) gazeob SITL simulation from px4.
​
I didn't instate the setpoint publisher in ground station. Instead, I put it in onboard side. Thus, to change the setpoint from keyboard input from gcs, I put multiple service clients in GCS with the above gui.
Node [/mav_wrapper_node] Publications:
- /mavros/setpoint_position/local [geometry_msgs/PoseStamped]
- /mavros/vision_pose/pose [geometry_msgs/PoseStamped]
Subscriptions:
- /vicon/mav_felipe/mav_felipe [geometry_msgs/PoseStamped]
- /mav_wrapper/setpoint_planning [geometry_msgs/PoseStamped]
- /mavros/state [mavros_msgs/State]
- /tf [tf2_msgs/TFMessage]
- /tf_static [unknown type]
Services:
-
/mav_wrapper/init_home : initialize keyboard setpoint with current /mavros/local_position/pose
-
/mav_wrapper/keyboard_in : change the setpoint from keyboard mode.
w: forward / s: backword / a:move left / d: move right (those who enjoys shooting game, you will..)
z: descent / c: ascent / q: spin left / e: spin right
-
/mav_wrapper/swtich_mode: determine setpoint publishing in either from keyboard or planner which publishes /mav_wrapper/setpoint_planning [geometry_msgs/PoseStamped]. 0: keyboard 1:planner for request.
This package is not a ROS package! In our case, this package is required for SITL(simulation) gazebo. This package will help us spawn drone on gazebo and mimic the behavior of actual drone.
git clone https://github.com/PX4/Firmware.git
No build is mandatory at present step in STIL. Follow make instruction
This package is for mavlink-ROS communication with px4 on SITL. As a ROS user, we can access px4 via this ROS node. Install it from source.
In this package, we will extract tf information coming from gazebo in SITL. In order for it, the following package will publish tf information of all objects in gazebo simulator. Basically, this is fake gps.
http://wiki.ros.org/gazebo2rviz
You might want to try this package for path generation where you can directly infuse control pose.
Clone here : https://github.com/icsl-Jeon/traj_gen
sudo apt-get install qt4-dev-tools
- Install
cd ~/catkin_ws
catkin build px4_code
If build fails due to service related files (~Request.h or ~Response.h), then comment all the add_executable
and try build. If it succeeded, then uncomment the above lines and re-try.
You have to configure px4 parameters in QGroundControl so that the pixhawk can utilize the external position estimate from vicon or VIO module (you can even fuse them with GPS in HTIL). In order for that,
-
Change EKF2 estimator group rather than LPE.
-
EKF2_AID_MASK = Vision
-
EKF2_HGT_MODE = Vision
git clone https://github.com/PX4/Firmware.git
Add the following in your bash (to enable roscd px4)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/path/to/Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/path/to/Firmware/Tools/sitl_gazebo
# in terminal opend at path/to/px4_firmware folder:
no_sim=1 make px4_sitl_default gazebo
# register gazebo model path so that worlds can be loaded from any directory
source ./Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
# or in your bashrc
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/home/jbs/lib/Firmware/build/px4_sitl_default/build_gazebo
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/home/jbs/lib/Firmware/Tools/sitl_gazebo/models
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/jbs/lib/Firmware/build/px4_sitl_default/build_gazebo
Especially, if you want your custom world for px4_sitl simulation, modifiy world_file
parameter in run_px4_for_sitl.launch
. Readily available world files can be found in ./worlds folder.
# in any terminal, prepare the world and spwan drone
roslaunch px4_code run_px4_for_sitl.launch
# in terminal opend at path/to/px4_firmware folder, turn on px4
no_sim=1 make px4_sitl_default gazebo
Assuming you added GAZEBO_MODEL_PATH
in bashrc as described above, include the model in .world file.
<include>
<uri>model://iris_fpv_cam</uri>
<pose>2.0 0.0 0.2 0 0 0.0</pose>
</include>
Then, make sure the following 2 physics parameters are configured as follows
...
<real_time_update_rate>250</real_time_update_rate>
...
<max_step_size>0.004</max_step_size>
...
This will run the two nodes mentioned in introduction.
You might have to inspect the arguments in the launch file before proceding.
- mav_frame_id: gazebo2rviz reads ModelStates in gazebo world and broadcasts /tf. You have to identify the tf name when a drone is spwaned. (e.g. iris_fpv_cam_iris__base_link)
- pose_topic : tf2poseStamped_node will read /tf of drone and publish the corresponding pose with topic
pose_topic
. This topic will be read in mav_wrapper to output /mavros/vision_pose/pose. The process described may seem inefficient, I focused on compatibility ofpx_code
with gazebo(SITL) or real world (HITL)
You may procede with the launch after checking them.
roslaunch px4_code run_mav_gcs_wrapper_sitl.launch
If things was configured whell, your px4 shell px4_shell will say after the px4_code launch
INFO [ecl/EKF] EKF commencing external vision position fusion
INFO [ecl/EKF] EKF commencing external vision yaw fusion
-
check if /mavros/local_position/pose and /mavros/vision_pose/pose similar(they should be very close.). If not, check whether the two has same time stamp!
-
If mav_wrapper_node received external pose topic and
mavros/local_position/pose
, the following will appear from px4_code terminal[ INFO] [1563892246.976191576, 241.848000000]: received external pose. [ INFO] [1563892247.004177214, 241.876000000]: [Mav Wrapper] Initializing the homing point with the current pose [ INFO] [1563892247.028217821, 241.900000000]: Start publishing setpoint. [ INFO] [1563892299.438848425, 294.224000000]: [Mav Wrapper] Initializing the homing point with the current pose
Also, you could see the topics and tf in as the following rviz.
As can be seen, the /mavros/local_position/pose should be almost same with the tf coming from gazebo.
The red arrow denotes the setpoint which is currently initialized with the current pose.
rosrun px4_code mav_gcs_node
- Step 1 : press ros connect in gui! Once done, mav_gcs_node will appear in rosnode list
- Step 2: press initialize button. This will initialize the current setpoint from keyboard.
- Step 3: press disarmed button. From this, you wll see rotor0 operating
- Step 4. press set offboard button. The flight mode will change from manual into offboard from which mavros will listen setpoint as control object. Move mav with keyboard!
- Step 5. If you have node which publishes /mav_wrapper/setpoint_planning [geometry_msgs/PoseStamped], try pressing planner button. This will listen the setpoint from planner.
From this, you will have to run mav_gcs_node and mav_wrapper separately. We should run mav_wrapper in onboard computer where mavros
will be launched
roslaunch mavros px4.launch
roslaunch px4_code run_mav_wrapper.launch
make sure <param name="external_pose_topic" value="$(some topic)"/>
roalaunch vicon_bridge vicon.launch (in my setting, it provides pose topic )
rosrun px4_code mav_gcs_node
#TODO roslaunch px4_code run_mav_gcs_node_onboard.launch (including rviz)
$cd ~/catkin_ws/src/
$https://github.com/microsoft/AirSim.git
$cd ./Airsim
$./setup.sh
$./build.sh
$cd ~/catkin_ws
$catkin build airsim_ros_pkgs
$catkin build arisim_tutorial_pkgs
$git clone https://github.com/PX4/Firmware.git
$cd Firmware
$git checkout v1.9.2 # Pick a well known "good" release tag.
$ roscd px4_code
$ cp ./airsim_setting/settings.json ~/Documents/Airsim/settings.json
EKF_HGT_MODE = GPS
and EKF_AID_MAKE = GPS
cd ~/Firmware
make px4_sitl_default none_iris
roslaunch px4_code airsim_sitl.launch