Test pathplanner obstacleavoidance

.gitignore

@@ -8,4 +8,5 @@ cmake-build-debug/
 *.out
 tags
 .vscode
+*.swp
 

CMakeLists.txt

@@ -73,11 +73,26 @@ 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_library(path_planner_lib src/path_planner/path_planner src/path_planner/node)
+
+add_executable(path_planner_node src/nodes/path_planner_node)
+
+add_executable(test_path_planner_node src/nodes/test_path_planner_node)
+
 ## 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(path_planner_node 
+        ${catkin_LIBRARIES}
+        control_tools_lib
+        path_planner_lib)
+
+target_link_libraries(test_path_planner_node 
+        ${catkin_LIBRARIES}
+        control_tools_lib)
+
 ## 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})
@@ -87,6 +102,9 @@ add_dependencies(action_interface_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${c
 if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
     add_rostest_gtest(utest test/launch_test.test test/utest.cpp)
+    target_link_libraries(utest control_fsm_lib path_planner_lib)
     target_link_libraries(utest control_fsm_lib)
+    target_link_libraries(utest ${catkin_LIBRARIS})
     roslaunch_add_file_check(launch)
 endif()
+

control_fsm_default_config.yaml

@@ -8,13 +8,13 @@
     #When is a destination reached
     dest_reached_margin: 0.2,
     #Topic for publishing error messages
-    fsm_error_topic: control/fsm/on_error, 
+    fsm_error_topic: control/fsm/on_error,
     #Topic for publishing info messages
-    fsm_info_topic: control/fsm/on_info, 
+    fsm_info_topic: control/fsm/on_info,
     #Topic for publishing current state
     fsm_state_changed_topic: control/fsm/state_changed,
     #Topic for publishing warning messages
-    fsm_warn_topic: control/fsm/on_warn, 
+    fsm_warn_topic: control/fsm/on_warn,
     #Altitude for optimal gb detection
     gb_search_altitude: 1.0, 
     #Time to wait before automatic landing
@@ -38,31 +38,44 @@
     #Topic for action server
     action_server_topic: /control/fsm/action_server,
     #When is message considered old?
-    message_timeout: 2.0, 
+    message_timeout: 2.0,
     #For what distances is it uneccesary to adjust yaw
-    no_yaw_correct_dist: 0.2, 
-    #Safety distance 
-    obstacle_too_close_dist: 2.0, 
+    no_yaw_correct_dist: 0.2,
+    #Safety distance
+    obstacle_too_close_dist: 2.0,
     #Require all data streams to enable FSM
     require_all_streams: true,
-    #Require obstacle detection 
-    require_obs_detection: true, 
+    #Require obstacle detection
+    require_obs_detection: true,
+    #Safe altitude for hovering
+    safe_hover_alt: 1.0,
     #Margin to determine if we're close enough to setpoint
     setp_reached_margin: 0.2, 
     #Buffersize for status messages
     status_msg_buffer_size: 10,
     #Takeoff altitude
-    takeoff_altitude: 1.0, 
+    takeoff_altitude: 1.0,
     #Margin to determine if we're close enough to yaw setpoint
     yaw_reached_margin: 0.02,
     #Altitude for goto in landxy
     land_xy_goto_alt: 1.0,
     #Minimum flying altitude
     min_in_air_alt: 0.5,
     #Topic for recieving obstacle state data
-    obstacle_state_topic: perception/obstacles,
+    obstacle_state_topic: perception/obstacles/lidar,
     #Margin to determine if velocity is reached
-    velocity_reached_margin: 0.1,
+    velocity_reached_margin: 0.2,
+    #Path planner client topic
+    path_planner_client_topic: /path_planner_service,
+    #Path planner plan topic
+    path_planner_plan_topic: /control/planner/plan,
+    #Path planner timeout
+    path_plan_timeout: 1.0,
+    #Obstacle avoidance clearance 
+    obstacle_clearance_max: 1.1,
+    obstacle_clearance_min: 1.0,
+    #Obstacle avoidance checkradius
+    obstacle_clearance_checkradius: 40.0,
     #Frame ID for global frame
     global_frame_id: map,
     #Frame ID for local frame

include/control/fsm/control_fsm.hpp

@@ -123,7 +123,6 @@ class ControlFSM {
 
     ///Handles loss of offboard mode
     void handleManual();
-
 protected:
     /**
      * @brief Changes the current running state
@@ -133,6 +132,8 @@ class ControlFSM {
      * @param event Which event triggered the transition request
      */
     void transitionTo(StateInterface& state, StateInterface* caller_p, const EventData& event);
+
+
 
 public:
     ///Constructor sets default/starting state

include/control/fsm/go_to_state.hpp

@@ -2,14 +2,24 @@
 #define GO_TO_STATE_HPP
 #include "state_interface.hpp"
 #include <ros/ros.h>
-#include <ascend_msgs/PathPlannerPlan.h>
+#include <ascend_msgs/PointArrayStamped.h>
 #include <memory>
 #include <functional>
 
 ///Moves drone to XYZ
 class GoToState : public StateInterface {
 private:
-
+    ///Get nodehandle, constructed on first use!
+    static ros::NodeHandle& getNodeHandler();
+    ///Path planner ROS service client
+    ros::ServiceClient path_planner_client_;
+    ///Path planner plan sub
+    ros::Subscriber path_planner_sub_;
+    ///When was plan requested
+    ros::Time path_requested_stamp_;
+    ///Last recieved plan
+    ascend_msgs::PointArrayStamped::ConstPtr last_plan_;
+
     struct {
         ros::Time started;
         bool enabled = false;
@@ -18,7 +28,17 @@ class GoToState : public StateInterface {
 
     ///Current command
     EventData cmd_;
+
+    ///Is state active flag
+    bool is_active_ = false;
+
+    ///Obstacle avoidance kicked in flag
+    bool obstacle_avoidance_kicked_in_ = false;
+
+    ///Plan callback 
+    void planCB(ascend_msgs::PointArrayStamped::ConstPtr msg_p);
 
+
     ///Local target to reach
     tf2::Vector3 local_target_;
 public:

include/control/fsm/land_state.hpp

@@ -6,10 +6,13 @@
 class LandState : public StateInterface {
 private:
     EventData cmd_;
+
+    bool obstacle_avoidance_kicked_in_ = false;
 public:
     LandState();
     void handleEvent(ControlFSM& fsm, const EventData& event) override;
     void stateBegin(ControlFSM& fsm, const EventData& event) override;
+    void stateInit(ControlFSM& fsm) override;
     void loopState(ControlFSM& fsm) override;
     std::string getStateName() const override { return "Land"; }
     ascend_msgs::ControlFSMState getStateMsg() const override;

include/control/fsm/position_hold_state.hpp

@@ -12,6 +12,7 @@ class PositionHoldState : public StateInterface {
     PositionHoldState();
     void handleEvent(ControlFSM& fsm, const EventData& event) override;
     void stateBegin(ControlFSM& fsm, const EventData& event) override;
+    void stateInit(ControlFSM& fsm) override;
     std::string getStateName() const override { return "Position hold"; }
     ascend_msgs::ControlFSMState getStateMsg() const override;
     const mavros_msgs::PositionTarget* getSetpointPtr() override;

include/control/fsm/state_interface.hpp

@@ -3,17 +3,17 @@
 
 /** @page fsm_description FSM design
  *  @brief Describes the design pattern
- *  
+ *
  *  All states inherits the StateInterface class, and the
- *  FSM use polymorphism to switch between different states. 
- *  
- *  On transition between states, the StateInterface::stateBegin method will be called in the new state. 
+ *  FSM use polymorphism to switch between different states.
+ *
+ *  On transition between states, the StateInterface::stateBegin method will be called in the new state.
  *  The active states StateInterface::loopState method will always run in a loop.
  *  The StateInterface::stateEnd method will be called before the fsm transition to another state.
- *  
- *  Transitioning between states is done with the ControlFSM::transtionTo method. 
- *  
- *  FSM is not async so do not run any blocking code 
+ *
+ *  Transitioning between states is done with the ControlFSM::transtionTo method.
+ *
+ *  FSM is not async so do not run any blocking code
  *  in any of these methods.
  *  EventData is passed by reference and is NOT guaranteed to remain in scope.
  *  DO NOT store event data by reference.
@@ -34,7 +34,7 @@ class ControlFSM;
 ///Abstract interface class inherited by all states
 /*
 NOTE:
-FSM is not async so do not run any blocking code 
+FSM is not async so do not run any blocking code
 in any of these methods.
 EventData is passed by reference and is NOT guaranteed to remain in scope.
 DO NOT store event data by reference.
@@ -85,22 +85,22 @@ class StateInterface {
 
     ///Handles loss of offboard mode - must be implemented by state
     virtual void handleManual(ControlFSM& fsm) = 0;
-     
+
     ///Runs on current state AFTER transition
     /**stateBegin is only implemented if needed by state.*/
     virtual void stateBegin(ControlFSM& fsm, const EventData& event) {}
 
     ///Runs on current state BEFORE transition
     /**stateEnd is only implemented if needed by state*/
     virtual void stateEnd(ControlFSM& fsm, const EventData& event) {}
-    
+
     ///Runs state specific code
     /**loopState is only implemented if needed by state*/
     virtual void loopState(ControlFSM& fsm) {}
-    
+
     ///Should return name of the state - used for debugging purposes
     virtual std::string getStateName() const = 0;
-    
+
     ///Get state message
     virtual ascend_msgs::ControlFSMState getStateMsg() const = 0; 
 
@@ -110,10 +110,10 @@ class StateInterface {
 
     ///Static interface returning iterator to first state
     static std::list<StateInterface*>::const_iterator cbegin() { return getAllStatesVector()->cbegin(); }
-    
+
     ///Static interface returning iterator to last + 1 state
     static std::list<StateInterface*>::const_iterator cend() { return getAllStatesVector()->cend(); }
-  
+
     ///Returns number of instanciated states
     static size_t getNumStates() { return getAllStatesVector()->size(); }
 };

include/control/planner/node.hpp

@@ -0,0 +1,41 @@
+#ifndef NODE_HPP
+#define NODE_HPP
+
+class Node{
+private:
+    float x;
+    float y;
+
+    // heuristic, use diagonal distance
+    float g;
+    float h;
+    // Sum of g and h
+    float f;
+
+    float parent_x = -1;
+    float parent_y = -1;
+public:
+    Node(){}
+    Node(float x, float y, float g, float h):x(x), y(y), g(g), h(h){f = g+h;}
+
+    float getX() const {return x;}
+    float getY() const {return y;}
+    float getF() const {return f;}
+    float getG() const {return g;}
+    float getParentX() const {return parent_x;}
+    float getParentY() const {return parent_y;}
+
+    void setG(float g) {this->g = g;}
+    void setParentX(float parent_x){this->parent_x = parent_x;}
+    void setParentY(float parent_y){this->parent_y = parent_y;}
+    void updateF(float new_g);
+
+    bool obstacle = false;
+    bool closed = false;
+    bool unsafe = false;
+
+    // Implemented only for the closed list priority queue
+    friend bool operator< (const Node &lhs, const Node &rhs);
+};
+
+#endif // NODE_HPP