ekf2 failed to use Vision for localization and PX4 keeps noticing: requesting home position

[JaimeParker]

Hi, thank you for checking this, I'm working on using ORB SLAM2 on my drone in the gazebo environment. After I get everything done, it seems efk2 didn't get Vision information or failed to change to Vision mode, below are my steps:

edit rcS

disable GPS localization and Barometer for height measurement, using Vision instead

    # GPS used
    #param set EKF2_AID_MASK 1
    # Vision used and GPS denied
    param set EKF2_AID_MASK 24

    # Barometer used for hight measurement
    #param set EKF2_HGT_MODE 0
    # Barometer denied and vision used for hight measurement
    param set EKF2_HGT_MODE 3

and importantly, delete parameters load in .ros:

rm ~/.ros/eeprom/parameters*
rm -rf ~/.ros/sitl*

so that the next time I launch the gazebo, rcS parameters will be loaded again.

add a stereo camera in the launch file

I'm using mavros_posix_sitl.launch, and edit like this:

    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <!-- add stereo camera for iris -->
    <arg name="my_camera" default="iris_stereo_camera"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <!-- also need to revise sdf -->
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_camera)/$(arg my_camera).sdf"/>

set .sh file

then I created a .sh file for ORB SLAM2, contents below:

rosrun ORB_SLAM2 Stereo 
~/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt 
~/catkin_ws/src/ORB_SLAM2/Examples/Stereo/px4_sitl.yaml true
 /camera/left/image_raw:=/stereo/left/image_raw 
/camera/right/image_raw:=/stereo/right/image_raw 
/orbslam2/vision_pose/pose:=/mavros/vision_pose/pose

Run

Finally, I started my trail

roslaunch px4 mavros_posix_sitl.launch
sh that.sh

I guess ORB is working well, I put a screenshot here, According to others, there should be information like below to show that Vision is functioning well, but it didn't appear on my screen.

INFO  [ecl/EKF] 23860000: reset position to ev position
INFO  [ecl/EKF] 23860000: commencing external vision position fusion
INFO  [ecl/EKF] 23860000: commencing external vision yaw fusion

and my PX4 keeps noticing me that hp: requesting home position, I can't use commander takeoff and I can't get into offboard mode, I'm really confused, can anyone help me? here is a bug picture,

[ INFO] [1647332081.616824996, 463.948000000]: HP: requesting home position
WARN  [commander] Failsafe enabled: no RC and no offboard
[ERROR] [1647332086.858252172, 466.871000000]: FCU: Failsafe enabled: no RC and no offboard
[ INFO] [1647332086.916993721, 466.923000000]: FCU: Failsafe mode activated
[ INFO] [1647332097.829447664, 473.031000000]: FCU: DISARMED by Auto disarm initiated
[ INFO] [1647332099.221400643, 473.948000000]: HP: requesting home position
INFO  [logger] closed logfile, bytes written: 560350
[ INFO] [1647332116.884205512, 483.948000000]: HP: requesting home position
[ INFO] [1647332134.370511761, 493.948000000]: HP: requesting home position
[ INFO] [1647332152.956565434, 503.948000000]: HP: requesting home position
[ INFO] [1647332168.367523905, 513.948000000]: HP: requesting home position
[ INFO] [1647332183.421351360, 523.949000000]: HP: requesting home position
[ INFO] [1647332198.407902287, 533.948000000]: HP: requesting home position
[ INFO] [1647332212.284682896, 543.948000000]: HP: requesting home position

and if I use offboard program, end up like: my offboard_node program and part of it:

ros::Time last_request = ros::Time::now();
    ROS_INFO("Off boarding");
    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Off-board mode enabling...");
            }
            last_request = ros::Time::now();

        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if (current_state.mode != "OFFBOARD") ROS_INFO("Off board mode was shut unexpectedly");
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

Thanks in advance!

[Jaeyoung-Lim]

@JaimeParker I see two problems

a)

[ERROR] [1647332086.858252172, 466.871000000]: FCU: Failsafe enabled: no RC and no offboard

You have the RC failsafe enabled for offboard mode. This means that if you have no manual control input the vehicle will reject entering offboard mode. You can turn this off if you want, but this is a safety feature for real vehicles

b) If you are not sending a setpoint, you cannot get into offboard mode. Otherwise, it is unclear what the vehicle should do in offboard mode

[JaimeParker]

@JaimeParker I see two problems

a)

[ERROR] [1647332086.858252172, 466.871000000]: FCU: Failsafe enabled: no RC and no offboard

You have the RC failsafe enabled for offboard mode. This means that if you have no manual control input the vehicle will reject entering offboard mode. You can turn this off if you want, but this is a safety feature for real vehicles

b) If you are not sending a setpoint, you cannot get into offboard mode. Otherwise, it is unclear what the vehicle should do in offboard mode

@Jaeyoung-Lim , thanks for your reply, sir. a) that's right, after I started QGroundControl, opening virtual joystick, this error is fixed I guess. b) thanks for what you mentioned, I just realized that we need to send some points in some rate before entering into offborad mode, which can be referred here: https://docs.px4.io/master/en/ros/mavros_offboard.html However, I still have no info said that Vision is in use and can't use commander takeoff, still no answer. Again, thank you, sir.

[JaimeParker]

fixed, need to revise ORB_SLAM2/Example/ROS/ORB_SLAM2/src/ros_stereo.cc, link here: https://gitee.com/hazyparker/my-research/blob/master/3_Onboard_SLAM/XTDrone_Method/ros_stereo.cc

[mengchaoheng]

The same problem