Ground robot landing FSM implementation

CMakeLists.txt

@@ -72,18 +72,22 @@ target_link_libraries(control_fsm_lib
 
 add_executable(control_fsm_main src/nodes/control_fsm_main)
 add_executable(action_interface_client src/nodes/action_interface_client.cpp)
+add_executable(fake_snake_data src/nodes/mocapConverter.cpp)
 
 ## Specify libraries to link a library or executable target against
 
 target_link_libraries(control_fsm_main control_fsm_lib)
 target_link_libraries(action_interface_client ${catkin_LIBRARIES})
+target_link_libraries(fake_snake_data ${catkin_LIBRARIES})
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(control_fsm_main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(control_fsm_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(control_tools_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(action_interface_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(fake_snake_data ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
     add_rostest_gtest(utest test/launch_test.test test/utest.cpp)

control_fsm_default_config.yaml

@@ -33,6 +33,8 @@
     distance_sensor_topic: /mavros/distance_sensor/hrlv_ez4_pub,
     #Topic for recieving mavros state
     mavros_state_topic: /mavros/state,
+    #Topic for recieving ground robot states
+    ground_robot_state_topic: /ai/world_observation,
     #Topic for debug server
     debug_server_topic: /control/fsm/debug_server,
     #Topic for action server
@@ -63,6 +65,16 @@
     obstacle_state_topic: perception/obstacles,
     #Margin to determine if velocity is reached
     velocity_reached_margin: 0.1,
+    #TODO Add description
+    land_gb_tracking_xy: 2.0,
+    #TODO Add description
+    land_gb_tracking_z: 0.04,
+    #TODO Add description
+    land_gb_interception_xy: 2.0,
+    #TODO Add description
+    land_gb_interception_z: 0.01,
+    #TODO Add description
+    land_gb_max_distance: 2.0,
     #Frame ID for global frame
     global_frame_id: map,
     #Frame ID for local frame

include/control/fsm/control_fsm.hpp

@@ -13,8 +13,7 @@
 #include "takeoff_state.hpp"
 #include "blind_hover_state.hpp"
 #include "position_hold_state.hpp"
-#include "track_gb_state.hpp"
-#include "interact_gb_state.hpp"
+#include "land_gb_state.hpp"
 #include "go_to_state.hpp"
 #include "land_state.hpp"
 #include "manual_flight_state.hpp"
@@ -33,8 +32,7 @@ class ControlFSM {
     friend class TakeoffState;
     friend class BlindHoverState;
     friend class PositionHoldState;
-    friend class TrackGBState;
-    friend class InteractGBState;
+    friend class LandGBState;
     friend class GoToState;
     friend class LandState;
     friend class ManualFlightState;
@@ -46,8 +44,7 @@ class ControlFSM {
     static TakeoffState TAKEOFF_STATE;
     static BlindHoverState BLIND_HOVER_STATE;
     static PositionHoldState POSITION_HOLD_STATE;
-    static TrackGBState TRACK_GB_STATE;
-    static InteractGBState INTERACT_GB_STATE;
+    static LandGBState LAND_GB_STATE;
     static GoToState GO_TO_STATE;
     static LandState LAND_STATE;
     static ManualFlightState MANUAL_FLIGHT_STATE;

include/control/fsm/event_data.hpp

@@ -22,8 +22,7 @@ enum class RequestType {
     POSHOLD, 
     GOTO, //GOTO XYZ
     LAND, 
-    TRACKGB, //Track/follow ground robot
-    INTERGB, //Interact with ground robot (tap)
+    LANDGB, //Interact with ground robot (tap)
     MANUALFLIGHT
 };
 
@@ -43,7 +42,7 @@ enum class CommandType {
     LANDXY, //Request part of command to land at XY
     GOTOXYZ, //Reqeust part of going to specified XYZ
     TAKEOFF,
-    //LANDGB //Request part of interacting with ground robot
+    LANDGB //Request part of interacting with ground robot
 };
 
 ///Defines a position goal
@@ -93,7 +92,8 @@ class EventData {
     PositionGoal setpoint_target;
     ///If event is a command, what type?
     CommandType command_type = CommandType::NONE; //No command as default
-
+    ///Ground robot id
+    int gb_id = -1;
     ///Setter function for complete callback
     void setOnCompleteCallback(std::function<void()> callback) { on_complete_ = std::move(callback); }
     ///Setter function for error callback
@@ -114,6 +114,10 @@ class EventData {
     bool isValidRequest() const { return (event_type == EventType::REQUEST && request != RequestType::NONE); }
     ///Chekcs if this event is a specific valid request type
     bool isValidRequest(RequestType type) const { return isValidRequest() && request == type; }
+    ///Clear event
+    void clear() { *this = EventData(); }
+    ///On aborted
+    void abort() { eventError("ABORTED"); clear();}
 };
 
 class TakeoffCMDEvent : public EventData {
@@ -147,13 +151,17 @@ class GoToXYZCMDEvent : public EventData {
 ///Wrapper class for LandGB CMD events
 class LandGBCMDEvent : public EventData {
 public:
-    //TODO Implement event type
+    explicit LandGBCMDEvent(int gb) {
+        gb_id = gb;
+        event_type = EventType::COMMAND;
+        command_type = CommandType::LANDGB;
+    }
 };
 
 ///Wrapper class for requests events
 class RequestEvent : public EventData {
 public:
-    RequestEvent(RequestType r) {
+    explicit RequestEvent(RequestType r) {
         event_type = EventType::REQUEST;
         request = r;
     }

include/control/fsm/interact_gb_state.hpp → include/control/fsm/land_gb_state.hpp

@@ -3,13 +3,15 @@
 #include "state_interface.hpp"
 
 ///Interacts with a ground robot
-class InteractGBState : public StateInterface {
+class LandGBState : public StateInterface {
+private:
+    EventData cmd_;
 public:
-    InteractGBState();
+    LandGBState();
     void handleEvent(ControlFSM& fsm, const EventData& event) override;
     void stateBegin(ControlFSM& fsm, const EventData& event) override;
     void loopState(ControlFSM& fsm) override;
-    std::string getStateName() const override { return "InteractWithGroundRobot"; }
+    std::string getStateName() const override { return "LandGB"; }
     const mavros_msgs::PositionTarget* getSetpointPtr() override;
     ascend_msgs::ControlFSMState getStateMsg() const override;
     void handleManual(ControlFSM &fsm) override;

include/control/fsm/position_hold_state.hpp

@@ -16,6 +16,7 @@ class PositionHoldState : public StateInterface {
     ascend_msgs::ControlFSMState getStateMsg() const override;
     const mavros_msgs::PositionTarget* getSetpointPtr() override;
     void handleManual(ControlFSM &fsm) override;
+    void processCommand(ControlFSM& fsm, const EventData& cmd);
 };
 
 #endif

include/control/tools/config.hpp

@@ -69,6 +69,8 @@ class Config {
     static std::string land_detector_type;
     /// \fsmparam Topic for recieving obstacle positions
     static std::string obstacle_state_topic;
+    /// \fsmparam Topic for recieving ground robot state
+    static std::string ground_robot_state_topic;
     /// \fsmparam Topic for recieving debug service requests
     static std::string debug_server_topic;
     /// \fsmparam Topic for action server
@@ -91,6 +93,18 @@ class Config {
     static bool require_obstacle_detection;
     /// \fsmparam When is data considered old?
     static double valid_data_timeout;
+    //"Tracking speed" in the x-y plane
+    static double tracking_param_xy;
+    //Tracking speed in the z direction
+    static double tracking_param_z; 
+    //Variable that control the change of the speed when intercepting, xy-plane
+    static double interception_param_xy;
+    //Variable that control the change of the speed when intercepting, z-direction   
+    static double interception_param_z;
+    // \fsmparam Variable to tweak the low boundry of interception algorithm
+    static double interception_low_alt_boundry;
+    // Maximal distance away from the roomba to run interception algorithm
+    static double max_distance;
     /// \fsmparam Frame id for global frame
     static std::string global_frame_id;
     /// \fsmparam Frame id for local frame

include/control/tools/gb_estimator.h

@@ -0,0 +1,17 @@
+#include <ros/ros.h>
+namespace control {
+
+class GbEstimator{
+private:
+	float current_pos_x; 
+	float current_pos_y; 
+	float gb_speed; 
+	ros::Time last_time_stamp; 
+	bool valid; 
+
+public: 
+	GbEstimator(float gb_x, float gb_y);
+	GbEstimator(); 
+	void update(const float *gb_direction, float *gb_position); 
+};
+}

include/control/tools/ground_robot_handler.hpp

@@ -0,0 +1,47 @@
+//
+// Created by haavard on 19.02.18.
+//
+
+#ifndef CONTROL_FSM_GROUND_ROBOT_HANDLER_HPP
+#define CONTROL_FSM_GROUND_ROBOT_HANDLER_HPP
+
+#include <ros/ros.h>
+#include <ascend_msgs/AIWorldObservation.h>
+
+#include <utility>
+
+namespace control {
+class GroundRobotHandler {
+    using MsgType = ascend_msgs::AIWorldObservation;
+public:
+    using GBVectorType = boost::array<ascend_msgs::GRState, 10>;
+private:
+    ///Shared instance
+    static std::unique_ptr<GroundRobotHandler> shared_instance_p_;
+    ///ROS nodehandle
+    ros::NodeHandle n_;
+    ///ROS subscriber to ground robot data
+    ros::Subscriber gb_sub_;
+    ///Last recieved message
+    MsgType::ConstPtr last_gb_msg_p_;
+
+    ///Callback for recieving ros data
+    void gbCB(MsgType::ConstPtr msg_p) { last_gb_msg_p_ = std::move(msg_p); }
+    ///Get valid shared instance
+    static const GroundRobotHandler* getSharedInstancePtr();
+    ///Constructor
+    GroundRobotHandler();
+    ///Get ground robots from instance
+    const GBVectorType& getGroundRobots() const;
+
+public:
+    static GBVectorType getCurrentGroundRobots() { return getSharedInstancePtr()->getGroundRobots(); }
+    ///Check if ready
+    static bool isReady();
+
+
+
+};
+}
+
+#endif //CONTROL_FSM_GROUND_ROBOT_HANDLER_HPP

include/control/tools/intercept_groundbot.hpp

@@ -0,0 +1,21 @@
+#ifndef INTERCEPT_GROUNDBOT_HPP
+#define INTERCEPT_GROUNDBOT_HPP
+
+#include "control/tools/intercept_groundbot.hpp"
+
+namespace control {
+namespace gb {
+/** Calculate setpoint to intercept a ground robot
+ * @param quad_position Drones current pose
+ * @param roomba_position Tracked ground robot
+ * @setpoint Setpoint to be modified
+ * @return true if setpoint is modified (all ok), false if not
+ */
+bool interceptGB(const geometry_msgs::PoseStamped& quad_position, 
+                 const ascend_msgs::GRState&       roomba_position, 
+                 mavros_msgs::PositionTarget&      setpoint); 
+}
+}
+
+
+#endif

include/control/tools/obstacle_state_handler.hpp

@@ -21,17 +21,17 @@ class ObstacleStateHandler {
     void onMsgRecievedCB(ascend_msgs::DetectedRobotsGlobalPositions::ConstPtr msg_p);
     ///Private constructor
     ObstacleStateHandler();
-public:
     ///Get shared pointer to shared instance
     static const ObstacleStateHandler* getSharedObstacleHandlerPtr();
-    ///Get last obstacles from shared instance
-    static const ascend_msgs::DetectedRobotsGlobalPositions& getCurrentObstacles();
     ///Returns obstacles from instance
     const ascend_msgs::DetectedRobotsGlobalPositions& getObstacles() const;
-    ///Is shared instance ready?
-    static bool isInstanceReady();
     ///is current instance ready
     bool isReady() const;
+public:
+    ///Get last obstacles from shared instance
+    static const ascend_msgs::DetectedRobotsGlobalPositions& getCurrentObstacles();
+    ///Is shared instance ready?
+    static bool isInstanceReady();
 };
 }
 #endif

launch/testdrone.launch

@@ -7,7 +7,7 @@
     	<arg name="node_name" value="$(arg fsm_name)"/>
     </include>
     <param name="$(arg fsm_name)/require_obs_detection" value="false"/>
-    <param name="$(arg fsm_name)/land_detector_topic" value="/land_detector"/>
+    <param name="$(arg fsm_name)/land_detector_topic" value="land_detector"/>
     <param name="$(arg fsm_name)/land_detector_type" value="landing_gear"/>
 
 </launch>

src/fsm/action_server.cpp

@@ -163,11 +163,20 @@ void ActionServer::startLandXY(GoalSharedPtr goal_p, ControlFSM* fsm_p) {
 }
 
 void ActionServer::startLandGB(GoalSharedPtr goal_p, ControlFSM* fsm_p) {
-    //TODO Implement when landgb procedure is decided
-    ROS_WARN("[Control ActionServer] LandGB not implemented!!");
-    ascend_msgs::ControlFSMResult result;
-    action_is_running_ = false;
-    as_.setAborted(result);
+    LandGBCMDEvent land_xy_event(goal_p->target_id);
+    //Set callback to run on complete
+    land_xy_event.setOnCompleteCallback([this]() {
+        onActionComplete();
+    });
+    //Set callback to run on complete
+    land_xy_event.setOnFeedbackCallback([this](const std::string& msg) {
+        onActionFeedback(msg);
+    });
+    land_xy_event.setOnErrorCallback([this](const std::string& msg) {
+        onActionError(msg);
+    });
+    fsm_p->handleEvent(land_xy_event);
+    action_is_running_ = true;
 }
 
 void ActionServer::startTakeoff(GoalSharedPtr goal_p, ControlFSM* fsm_p) {

src/fsm/control_fsm.cpp

@@ -6,14 +6,14 @@
 #include <control/tools/config.hpp>
 #include <control/tools/logger.hpp>
 #include <control/tools/obstacle_state_handler.hpp>
+#include <control/tools/ground_robot_handler.hpp>
 
 BeginState ControlFSM::BEGIN_STATE;
 IdleState ControlFSM::IDLE_STATE;
 TakeoffState ControlFSM::TAKEOFF_STATE;
 BlindHoverState ControlFSM::BLIND_HOVER_STATE;
 PositionHoldState ControlFSM::POSITION_HOLD_STATE;
-TrackGBState ControlFSM::TRACK_GB_STATE;
-InteractGBState ControlFSM::INTERACT_GB_STATE;
+LandGBState ControlFSM::LAND_GB_STATE;
 GoToState ControlFSM::GO_TO_STATE;
 LandState ControlFSM::LAND_STATE;
 ManualFlightState ControlFSM::MANUAL_FLIGHT_STATE;
@@ -134,6 +134,10 @@ bool ControlFSM::isReady() {
             if (!LandDetector::isReady()) {
                 control::handleWarnMsg("Preflight Check: No valid land detector data!");
                 return false;
+            } 
+            if(!control::GroundRobotHandler::isReady()) {
+                control::handleWarnMsg("Preflight Check: No valid ground robot data!");
+                return false;
             }
         } catch(const std::exception& e) {
             ///Critical bug -