Adds experimental feature to get correct altitude estimation on ground

control_fsm_default_config.yaml

@@ -37,6 +37,8 @@
     debug_server_topic: /control/fsm/debug_server,
     #Topic for action server
     action_server_topic: /control/fsm/action_server,
+    #Topic for publishing filter state from IDLE
+    idle_pose_topic: /mavros/mocap/pose,
     #When is message considered old?
     message_timeout: 2.0,
     #For what distances is it uneccesary to adjust yaw

include/control/fsm/idle_state.hpp

@@ -15,6 +15,9 @@ class IdleState : public StateInterface {
     ascend_msgs::ControlFSMState getStateMsg() const override; 
     const mavros_msgs::PositionTarget* getSetpointPtr() override;
     void handleManual(ControlFSM &fsm) override;
+    void stateInit(ControlFSM& fsm) override;
+    void loopState(ControlFSM& fsm) override;
+    void stateBegin(ControlFSM& fsm, const EventData& event) override;
 };
 
 #endif

include/control/tools/config.hpp

@@ -73,6 +73,8 @@ class Config {
     static std::string debug_server_topic;
     /// \fsmparam Topic for action server
     static std::string action_server_topic;
+    /// \fsmparam Topic for publishing pose in IDLE state
+    static std::string idle_pose_topic;
     /// \fsmparam Buffer size used for FSMError, FSMWarn etc
     static int fsm_status_buffer_size;
     /// \fsmparam Time gotostate waits before transitioning

src/fsm/idle_state.cpp

@@ -5,6 +5,9 @@
 #include "control/fsm/event_data.hpp"
 #include "control/fsm/control_fsm.hpp"
 
+ros::Publisher pose_pub;
+geometry_msgs::PoseStamped idle_pose;
+
 //Sets setpoint type to IDLE
 IdleState::IdleState() {
     setpoint_ = mavros_msgs::PositionTarget();
@@ -31,6 +34,26 @@ void IdleState::handleEvent(ControlFSM& fsm, const EventData& event) {
     }
 }
 
+void IdleState::stateInit(ControlFSM& fsm) {
+    using control::Config;
+    using geometry_msgs::PoseStamped;
+    auto& nh = fsm.node_handler_;
+    pose_pub = nh.advertise<PoseStamped>(Config::idle_pose_topic, 1);
+    setStateIsReady();
+}
+
+void IdleState::stateBegin(ControlFSM& fsm, const EventData& event) {
+    //Drone assumed to be on ground, stationary (or it will be soon anyways :') )
+    idle_pose = control::DroneHandler::getCurrentLocalPose();
+    idle_pose.pose.position.z = 0.0;
+}
+
+void IdleState::loopState(ControlFSM& fsm) {
+    //Publish to pose filter, forcing z = 0 when on ground!
+    idle_pose.header.stamp = ros::Time::now();
+    pose_pub.publish(idle_pose);
+}
+
 const mavros_msgs::PositionTarget* IdleState::getSetpointPtr() {
     //Sets timestamp, and returns setpoint_ as const pointer
     setpoint_.header.stamp = ros::Time::now();

src/tools/config.cpp

@@ -36,6 +36,7 @@ std::string Config::land_detector_type = "landing_gear";
 std::string Config::obstacle_state_topic = "/perception_obstacle_states";
 std::string Config::debug_server_topic = "/control/fsm/debug_server";
 std::string Config::action_server_topic = "/control/fsm/action_server";
+std::string Config::idle_pose_topic = "/mavros/mocap/pose";
 int Config::fsm_status_buffer_size = 10;
 double Config::go_to_hold_dest_time = 0.5;
 double Config::safe_hover_altitude = 2.0;
@@ -152,6 +153,7 @@ void Config::loadParams() {
     getStringParam("mavros_state_topic", mavros_state_changed_topic);
     getStringParam("local_velocity_topic", mavros_local_vel_topic);
     getStringParam("distance_sensor_topic", mavros_distance_sensor_topic);
+    getStringParam("idle_pose_topic", idle_pose_topic);
     //LandDetector
     getStringParam("land_detector_topic", land_detector_topic);
     getStringParam("land_detector_type", land_detector_type);