diff --git a/.gitignore b/.gitignore index 6844267..e001a26 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,5 @@ cmake-build-debug/ *.out tags .vscode +*.swp diff --git a/CMakeLists.txt b/CMakeLists.txt index fe64d00..593c781 100644 --- a/CMakeLists.txt +++ b/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() + diff --git a/control_fsm_default_config.yaml b/control_fsm_default_config.yaml index ad8286f..204404e 100644 --- a/control_fsm_default_config.yaml +++ b/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,21 +38,23 @@ #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 @@ -60,9 +62,20 @@ #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 diff --git a/include/control/fsm/control_fsm.hpp b/include/control/fsm/control_fsm.hpp index 894b31c..a201ee1 100644 --- a/include/control/fsm/control_fsm.hpp +++ b/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 diff --git a/include/control/fsm/go_to_state.hpp b/include/control/fsm/go_to_state.hpp index 5ea54c9..7d08f52 100644 --- a/include/control/fsm/go_to_state.hpp +++ b/include/control/fsm/go_to_state.hpp @@ -2,14 +2,24 @@ #define GO_TO_STATE_HPP #include "state_interface.hpp" #include -#include +#include #include #include ///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: diff --git a/include/control/fsm/land_state.hpp b/include/control/fsm/land_state.hpp index 74e51d5..215ae11 100644 --- a/include/control/fsm/land_state.hpp +++ b/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; diff --git a/include/control/fsm/position_hold_state.hpp b/include/control/fsm/position_hold_state.hpp index d039523..561932e 100644 --- a/include/control/fsm/position_hold_state.hpp +++ b/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; diff --git a/include/control/fsm/state_interface.hpp b/include/control/fsm/state_interface.hpp index 2d9a421..227b4c3 100644 --- a/include/control/fsm/state_interface.hpp +++ b/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,7 +85,7 @@ 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) {} @@ -93,14 +93,14 @@ class StateInterface { ///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::const_iterator cbegin() { return getAllStatesVector()->cbegin(); } - + ///Static interface returning iterator to last + 1 state static std::list::const_iterator cend() { return getAllStatesVector()->cend(); } - + ///Returns number of instanciated states static size_t getNumStates() { return getAllStatesVector()->size(); } }; diff --git a/include/control/planner/node.hpp b/include/control/planner/node.hpp new file mode 100644 index 0000000..dd48e6b --- /dev/null +++ b/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 diff --git a/include/control/planner/path_planner.hpp b/include/control/planner/path_planner.hpp index d99df34..6767f18 100644 --- a/include/control/planner/path_planner.hpp +++ b/include/control/planner/path_planner.hpp @@ -2,20 +2,122 @@ #define PATH_PLANNER_HPP #include -#include +#include "control/planner/node.hpp" #include "control/tools/control_pose.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace control{ +namespace pathplanner{ +#define FIELD_LENGTH 20.0 + + +struct Obstacle { + float x; + float y; + float dir; + Obstacle(float x, float y, float dir) : x(x), y(y), dir(dir){} +}; + +class PathPlanner; class PathPlanner{ private: - -ascend_msgs::PathPlannerPlan current_plan_; + float obstacle_radius; //meters + float node_distance; //meters + float diagonal_node_distance; //meters + int graph_size; //number of nodes in one direction + + std::vector> graph; + + std::priority_queue open_list; + //std::array, graph_size> closed_list; + std::list plan; + std::list simple_plan; + + std::list obstacles; + + Node end_node; + Node start_node; + + bool no_plan = true; + bool destination_reached = false; + + // Because of the NODE_DISTANCE, the destination found and the one requested + // might not be exactly equal + float destination_found_x; + float destination_found_y; + + PathPlanner() = delete; public: - PathPlanner(); - - - + PathPlanner(float obstacle_radius, float node_distance); + + int coordToIndex(float coord); + + void initializeGraph(); + + void setObstacles(std::list &obstacles); + + // Add a circular obstacle + void addObstacle(float center_x, float center_y); + Obstacle obstacleNextPos(float obstacle_x, float obstacle_y); + void refreshObstacles(); + Obstacle* findBlockingObstacle(float current_x, float current_y); + + // Adds unsafe zone around obstacles, the plan cannot be made here + void addUnsafeZone(float center_x, float center_y); + void refreshUnsafeZones(); + + // Diagonal heuristic - can move in 8 directions from current point + float calculateDiagonalHeuristic(float x, float y); + float calculateManhattanHeuristic(float x, float y); + float calculateEuclidianHeuristic(float x, float y); + + // Print the f-value of each node + //void printGraph(); + void relaxGraph(); + + // + void handleSuccessor(float x, float y, float parent_x, float parent_y, + float distance_to_parent); + // Every point has 8 succsessors + void handleAllSuccessors(float x, float y); + + bool isDestination(float x, float y); + bool isStart(float x, float y); + bool isValidCoordinate(float x, float y); + bool isSafeLine(float x1, float y1, float x2, float y2); + bool canSimplifyLine(float x1, float y1, float x2, float y2); + + // Use A* to calculate the path + void makePlan(float current_x, float current_y, float target_x, float target_y); + // Same plan but with fewer points + void simplifyPlan(); + // Verify the plan from where the drone is to the end point + bool isPlanSafe(float current_x, float current_y); + void resetParameters(); + bool removeOldPoints(float current_x, float current_y); + + // These functions are mainly for the visualization + std::vector> getGraph(){return graph;} + std::list getPlan(){return plan;} + std::list getSimplePlan(){return simple_plan;} + // For the colors in the visualization + float max_f = 0; }; +// Convert between coordinate x or y in metres and index in graph +//int coordToIndex(float k); + +} +} + #endif // PATH_PLANNER_HPP diff --git a/include/control/tools/config.hpp b/include/control/tools/config.hpp index a5ce198..305e468 100644 --- a/include/control/tools/config.hpp +++ b/include/control/tools/config.hpp @@ -83,6 +83,11 @@ class Config { static double safe_hover_altitude; /// \fsmparam Drone safezone static double obstacle_too_close_dist; + /// \fsmparam Clearance osbtacle avoidance + static double obstacle_clearance_max; + static double obstacle_clearance_min; + /// \fsmparam Radius in which obstacle collisions will be checked for + static double obstacle_clearance_checkradius; /// \fsmparam Topic to listen for info about obstacles static std::string lidar_topic; /// \fsmparam Finished drone will require all datastreams to be available @@ -91,6 +96,12 @@ class Config { static bool require_obstacle_detection; /// \fsmparam When is data considered old? static double valid_data_timeout; + /// \fsmparam How long to wait for path planner + static double path_plan_timeout; + /// \fsmparam Topic for planner plan sub + static std::string path_planner_plan_topic; + /// \fsmparam Topic for planner service client + static std::string path_planner_client_topic; /// \fsmparam Frame id for global frame static std::string global_frame_id; /// \fsmparam Frame id for local frame diff --git a/include/control/tools/obstacle_avoidance.hpp b/include/control/tools/obstacle_avoidance.hpp index 5ce4901..9aada42 100644 --- a/include/control/tools/obstacle_avoidance.hpp +++ b/include/control/tools/obstacle_avoidance.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace control { @@ -11,31 +12,65 @@ namespace control { class ObstacleAvoidance; class ObstacleAvoidance { private: - /**Set of callback method pointers registered by states + /**Set of modified callback method pointers registered by states * Pointers are used to allow comparison as std::function can't be compared */ std::set< std::shared_ptr< std::function > > on_modified_cb_set_; + + /**Set of warn callback method pointers registered by states + * Pointers are used to allow comparison as std::function can't be compared + */ + std::set< std::shared_ptr< std::function > > on_warn_cb_set_; ///Notify states the setpoint has been changed void onModified(); + ///Notify states the setpoint should be changed + void onWarn(); + ///Runs obstacle avoidance, and modifies setpoint (and notifies) virtual bool doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint); + ///Nodehandle + ros::NodeHandle nh_; + + ///Publisher for avoid zone information + ros::Publisher zone_pub_; + + ///Responsibility flag + bool has_setpoint_responsibility_ = true; + public: ///Default constructor - ObstacleAvoidance() = default; + ObstacleAvoidance(); ///Default copy constructor ObstacleAvoidance(const ObstacleAvoidance&) = default; - ///Add new callback + + ///Add new on warn callback + void registerOnWarnCBPtr(const std::shared_ptr >& cb_p) { on_warn_cb_set_.insert(cb_p); } + ///Remove on warn callback + void removeOnWarnCBPtr(const std::shared_ptr >& cb_p); + ///Add new on modified callback void registerOnModifiedCBPtr(const std::shared_ptr >& cb_p) { on_modified_cb_set_.insert(cb_p); } - ///Remove a callback + ///Remove on modified callback void removeOnModifiedCBPtr(const std::shared_ptr >& cb_p); + + ///Obstacle avoidance has reduced responsibility. + ///It won't change setpoints and only calls warning level callbacks + void relaxResponsibility() { + ROS_INFO("[obs_avoid] relaxing responsibility"); + has_setpoint_responsibility_ = false; + } + ///Obstacle avoidance takes full responsibility over setpoints and calls all callbacks + void takeResponsibility() { + ROS_INFO("[obs_avoid] taking responsibility"); + has_setpoint_responsibility_ = true; + } + ///Modifies setpoint if obstacle is too close mavros_msgs::PositionTarget run(mavros_msgs::PositionTarget setpoint); ///Returns true when obstacle avoidance is running bool isReady() { return true; } - }; } diff --git a/include/control/tools/obstacle_math.hpp b/include/control/tools/obstacle_math.hpp index d561f2b..7656ada 100644 --- a/include/control/tools/obstacle_math.hpp +++ b/include/control/tools/obstacle_math.hpp @@ -8,8 +8,7 @@ namespace obstacle_math { constexpr double PI{3.14159265358979323846264338327950288419}; // wrap any angle to range [0, 2pi) -template::value, T>::type> -T angleWrapper(const T angle){ +float angleWrapper(const float angle){ return angle - 2*PI*floor(angle/(2*PI)); } @@ -44,20 +43,20 @@ inline double calcDistanceToObstacle(const T& point, const K& obstacle_position) } -template -inline N calcAngleToObstacle(const T& point, const K& obstacle_position, const N obstacle_direction){ +template +inline float calcAngleToObstacle(const T& point, const K& obstacle_position){ T delta_drone_obstacle; delta_drone_obstacle.x = point.x - obstacle_position.x; delta_drone_obstacle.y = point.y - obstacle_position.y; - const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); + //const float angle_to_obstacle = angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x) - obstacle_direction); - return angle_to_obstacle; + return angleWrapper(std::atan2(delta_drone_obstacle.y, delta_drone_obstacle.x)); } // Apply 2d transformation matrix template -inline T rotateXY(const T& point, const float angle){ +T rotateXY(const T& point, const float angle){ T new_point; new_point.x = point.x * std::cos(angle) - point.y * std::sin(angle); new_point.y = point.x * std::sin(angle) + point.y * std::cos(angle); diff --git a/include/control/tools/planner_config.hpp b/include/control/tools/planner_config.hpp new file mode 100644 index 0000000..6fc43d2 --- /dev/null +++ b/include/control/tools/planner_config.hpp @@ -0,0 +1,49 @@ +#ifndef FSM_PLANNER_CONFIG_HPP +#define FSM_PLANNER_CONFIG_HPP +#include +#include +#include + +/** @page fsm_params FSM Params + * @brief Parameter page + * + * Contains all fsm params + */ + + +namespace control { +class PlannerConfig; +class PlannerConfig { +private: + ///Node handler + ros::NodeHandle nh_; + ///Reload service + ros::ServiceServer reload_config_service; + static std::set missing_param_set_; + ///Shared instance ptr + static std::unique_ptr shared_instance_p_; + ///Constructor + PlannerConfig(); + + +public: + /// \fsmparam Topic for recieving obstacle positions + static std::string obstacle_state_topic; + static std::string mavros_setpoint_topic; + static std::string plan_points_topic; + + // for path planner constructor + static double obstacle_radius; + static double node_distance; + + /**Load paramaters + * @throw control::ROSNotInitializedException + */ + static void loadParams(); + + ///Returns set of unloaded params + static const std::set& getMissingParamSet() { return missing_param_set_; } +}; +} + +#endif diff --git a/include/control/tools/setpoint_msg_defines.h b/include/control/tools/setpoint_msg_defines.h index fa16c3f..33ebcf8 100644 --- a/include/control/tools/setpoint_msg_defines.h +++ b/include/control/tools/setpoint_msg_defines.h @@ -23,4 +23,4 @@ constexpr uint16_t default_mask = IGNORE_VX | IGNORE_VY | IGNORE_VZ | IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ | IGNORE_YAW_RATE; -#endif \ No newline at end of file +#endif diff --git a/launch/mist.launch b/launch/mist.launch index 85a9f5b..f416ecf 100644 --- a/launch/mist.launch +++ b/launch/mist.launch @@ -10,4 +10,5 @@ + diff --git a/launch/path_planner.launch b/launch/path_planner.launch new file mode 100644 index 0000000..add34d8 --- /dev/null +++ b/launch/path_planner.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/launch/test_control_fsm_sim_planner.launch b/launch/test_control_fsm_sim_planner.launch new file mode 100644 index 0000000..0b21423 --- /dev/null +++ b/launch/test_control_fsm_sim_planner.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/fsm/blind_hover_state.cpp b/src/fsm/blind_hover_state.cpp index 87e4fe9..51ea40a 100644 --- a/src/fsm/blind_hover_state.cpp +++ b/src/fsm/blind_hover_state.cpp @@ -80,6 +80,10 @@ void BlindHoverState::stateBegin(ControlFSM& fsm, const EventData& event ) { if(event.isValidCMD()) { cmd_ = event; } + + // Obstacle avoidance has no data to go by, so disable it + fsm.obstacle_avoidance_.relaxResponsibility(); + control::handleWarnMsg("No valid pose available, blindhovering!"); } diff --git a/src/fsm/control_fsm.cpp b/src/fsm/control_fsm.cpp index 1f1907e..501ac38 100644 --- a/src/fsm/control_fsm.cpp +++ b/src/fsm/control_fsm.cpp @@ -41,7 +41,8 @@ void ControlFSM::transitionTo(StateInterface& state, StateInterface* caller_p, c } void ControlFSM::releaseCommonStateResources() { - //Add resources here + //Force return of responsibility + obstacle_avoidance_.takeResponsibility(); } //Send external event to current state and to "next" state @@ -129,7 +130,7 @@ bool ControlFSM::isReady() { if (subscribers_.mavros_state_changed_sub.getNumPublishers() <= 0) { control::handleWarnMsg("Preflight Check: No valid mavros state data!"); return false; - } + } //Land detector must be ready if (!LandDetector::isReady()) { control::handleWarnMsg("Preflight Check: No valid land detector data!"); diff --git a/src/fsm/go_to_state.cpp b/src/fsm/go_to_state.cpp index d36c0dd..b1695c0 100644 --- a/src/fsm/go_to_state.cpp +++ b/src/fsm/go_to_state.cpp @@ -1,20 +1,66 @@ #include "control/fsm/go_to_state.hpp" #include "control/fsm/control_fsm.hpp" #include +#include #include #include #include #include "control/tools/config.hpp" #include "control/tools/target_tools.hpp" +#include #include constexpr double PI = 3.14159265359; -constexpr double MAVROS_YAW_CORRECTION_PI_HALF = 3.141592653589793 / 2.0; +constexpr double MAVROS_YAW_CORRECTION_PI_HALF = PI / 2.0; -GoToState::GoToState() : StateInterface::StateInterface() { +//Gurantees construction on first use! +ros::NodeHandle& GoToState::getNodeHandler() { + static ros::NodeHandle n; + return n; +} + +GoToState::GoToState() : StateInterface::StateInterface() { + using ascend_msgs::PointArrayStamped; + last_plan_ = ascend_msgs::PointArrayStamped::ConstPtr(new ascend_msgs::PointArrayStamped); setpoint_.type_mask = default_mask; } +/** + * @brief Returns a yaw that is a multiple of 90 degrees + * @details Drone should fly as straight forward as possible + * , but yaw should be a multiple of 90 degrees. + * This method assumes dx and dy != 0 at the same time + * @param dx difference in x + * @param dy difference in y + * @return Yaw angle in radians - not mavros corrected + */ +double calculatePathYaw(double dx, double dy) { + //Avoid fatal error if dx and dy is too small + //If method is used correctly this should NEVER be a problem + if(std::fabs(dx * dx + dy * dy) < 0.001) { + return 0; + } + /* + angle = acos(([dx, dy] dot [1,0]) / (norm([dx, dy]) * norm([1,0]))) = acos(dx / (norm([dx, dy]) * 1)) + */ + double angle = std::acos(dx / std::sqrt(dx * dx + dy * dy)); + + //Select closest multiple of 90 degrees + if(angle > 3 * PI / 4) { + angle = PI; + } else if(angle > PI / 4) { + angle = PI / 2.0; + } else { + angle = 0; + } + //Invert if dy is negative + if (dy < 0) { + angle = -angle; + } + + return angle; +} + void GoToState::handleEvent(ControlFSM& fsm, const EventData& event) { if(event.isValidRequest()) { if(event.request == RequestType::ABORT) { @@ -93,18 +139,60 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { return; } + //Request new plan + using PathServiceRequest = ascend_msgs::PathPlanner; + PathServiceRequest req; + req.request.cmd = PathServiceRequest::Request::MAKE_PLAN; + req.request.goal_x = static_cast(cmd_.position_goal_local.x); + req.request.goal_y = static_cast(cmd_.position_goal_local.y); + + if(!path_planner_client_.call(req)) { + control::handleErrorMsg("Couldn't request path plan"); + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + return; + } + //Change stamp + path_requested_stamp_ = ros::Time::now(); + + // Set obstacle avoidance flag on entry + obstacle_avoidance_kicked_in_ = false; + // Set setpoint in local frame to target setpoint_.position.x = local_target_.x(); setpoint_.position.y = local_target_.y(); setpoint_.position.z = local_target_.z(); - + try { ///Calculate yaw setpoint using control::pose::quat2yaw; using control::getMavrosCorrectedTargetYaw; using control::DroneHandler; + + auto pose = DroneHandler::getCurrentLocalPose().pose; + //Hold position while waiting for new plan! + setpoint_.position.x = pose.position.x; + setpoint_.position.y = pose.position.y; + setpoint_.position.z = pose.position.z; + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(pose.orientation))); + auto quat = DroneHandler::getCurrentLocalPose().pose.orientation; + //auto pos = DroneHandler::getCurrentPose().pose.position; + + // calculate dx,dy + //auto dx = setpoint_.position.x - pos.x; + //auto dy = setpoint_.position.y - pos.y; + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(quat2yaw(quat))); + /* + if (std::fabs(dx*dx + dy*dy) < 0.01){ + // assume drone is stationary and dont change orientation + } else{ + setpoint_.yaw = static_cast(getMavrosCorrectedTargetYaw(calculatePathYaw(dx, dy))); + } + */ + + } catch(const std::exception& e) { //Critical bug - no recovery //Transition to position hold if no pose available @@ -115,6 +203,13 @@ void GoToState::stateBegin(ControlFSM& fsm, const EventData& event) { } void GoToState::stateEnd(ControlFSM& fsm, const EventData& event) { + using PathServiceRequest = ascend_msgs::PathPlanner; + PathServiceRequest req; + req.request.cmd = PathServiceRequest::Request::ABORT; + if(!path_planner_client_.call(req)) { + control::handleErrorMsg("Failed to call path planner service"); + } + ROS_INFO("Go to state end"); } void GoToState::loopState(ControlFSM& fsm) { @@ -125,13 +220,43 @@ void GoToState::loopState(ControlFSM& fsm) { if(!DroneHandler::isGlobalPoseValid()) { throw control::PoseNotValidException(); } - + + if (obstacle_avoidance_kicked_in_){ + //RequestEvent abort_event(RequestType::ABORT); + //fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + + bool last_plan_timeout = ros::Time::now() - last_plan_->header.stamp > ros::Duration(control::Config::path_plan_timeout); + bool plan_requested_timeout = ros::Time::now() - path_requested_stamp_ > ros::Duration(control::Config::path_plan_timeout); + + //No new valid plan recieved + if(last_plan_timeout || last_plan_->points.empty()) { + if(plan_requested_timeout) { + control::handleErrorMsg("Missing valid path plan!"); + RequestEvent abort_event(RequestType::ABORT); + if(cmd_.isValidCMD()) { + cmd_.eventError("ABORT"); + cmd_ = EventData(); + } + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + return; + } + + auto& target_point = last_plan_->points[0]; + local_target_ = tf2::Vector3(target_point.x, target_point.y, cmd_.position_goal_local.z); + setpoint_.position.x = local_target_.x(); + setpoint_.position.y = local_target_.y(); + setpoint_.position.z = local_target_.z(); + auto tf = DroneHandler::getLocal2GlobalTf(); tf2::Transform tf_matrix; tf2::convert(tf.transform, tf_matrix); //Transform local target back to global using latest transform auto global_target = tf_matrix * local_target_; + + //TODO Should path planner be global or local? //If target becomes invalid, abort if(Config::restrict_arena_boundaries) { @@ -145,7 +270,7 @@ void GoToState::loopState(ControlFSM& fsm) { return; } } - + using control::pose::quat2mavrosyaw; //Get pose auto local_pose = control::DroneHandler::getCurrentLocalPose(); @@ -154,16 +279,17 @@ void GoToState::loopState(ControlFSM& fsm) { //Get reference to orientation in pose auto& quat = local_pose.pose.orientation; //Calculate distance to target - double delta_x = local_position.x - local_target_.x(); - double delta_y = local_position.y - local_target_.y(); - double delta_z = local_position.z - local_target_.z(); + auto final_target = last_plan_->points.back(); + double delta_x = local_position.x - final_target.x; + double delta_y = local_position.y - final_target.y; + double delta_z = local_position.z - final_target.z; double delta_xy_squared = pow(delta_x, 2) + pow(delta_y, 2); double delta_yaw = quat2mavrosyaw(quat) - setpoint_.yaw; //Check if we're close enough using std::pow; using std::fabs; using control::Config; - bool xy_reached = delta_xy_squared <= pow(Config::dest_reached_margin, 2); + bool xy_reached = (delta_xy_squared <= pow(Config::dest_reached_margin, 2)); bool z_reached = (fabs(delta_z) <= Config::altitude_reached_margin); bool yaw_reached = (fabs(delta_yaw) <= Config::yaw_reached_margin); //If destination is reached, begin transition to another state @@ -173,6 +299,7 @@ void GoToState::loopState(ControlFSM& fsm) { delay_transition_.enabled = false; } } catch(const std::exception& e) { + //Exceptions should never occur! control::handleCriticalMsg(e.what()); //Go to PosHold @@ -191,52 +318,31 @@ const mavros_msgs::PositionTarget* GoToState::getSetpointPtr() { return &setpoint_; } +void GoToState::planCB(ascend_msgs::PointArrayStamped::ConstPtr msg_p) { + last_plan_ = msg_p; +} + //Initialize state void GoToState::stateInit(ControlFSM& fsm) { using control::Config; - //TODO Uneccesary variables - Config can be used directly - //Set state variables + using ascend_msgs::PointArrayStamped; + using ascend_msgs::PathPlanner; + auto& pc_topic = Config::path_planner_client_topic; + auto& ps_topic = Config::path_planner_plan_topic; delay_transition_.delayTime = ros::Duration(Config::go_to_hold_dest_time); + path_planner_client_ = getNodeHandler().serviceClient(pc_topic); + path_planner_sub_ = getNodeHandler().subscribe(ps_topic, 1, &GoToState::planCB, this); + + std::function obstacleAvoidanceCB = [this]()->void { + this->obstacle_avoidance_kicked_in_ = true; + }; + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); setStateIsReady(); control::handleInfoMsg("GoTo init completed!"); } -/** - * @brief Returns a yaw that is a multiple of 90 degrees - * @details Drone should fly as straight forward as possible - * , but yaw should be a multiple of 90 degrees. - * This method assumes dx and dy != 0 at the same time - * @param dx difference in x - * @param dy difference in y - * @return Yaw angle in radians - not mavros corrected - */ -double calculatePathYaw(double dx, double dy) { - //Avoid fatal error if dx and dy is too small - //If method is used correctly this should NEVER be a problem - if(std::fabs(dx * dx + dy * dy) < 0.001) { - return 0; - } - /* - angle = acos(([dx, dy] dot [1,0]) / (norm([dx, dy]) * norm([1,0]))) = acos(dx / (norm([dx, dy]) * 1)) - */ - double angle = std::acos(dx / std::sqrt(dx * dx + dy * dy)); - - //Select closest multiple of 90 degrees - if(angle > 3 * PI / 4) { - angle = PI; - } else if(angle > PI / 4) { - angle = PI / 2.0; - } else { - angle = 0; - } - //Invert if dy is negative - if (dy < 0) { - angle = -angle; - } - return angle; -} void GoToState::handleManual(ControlFSM& fsm) { cmd_.eventError("Lost OFFBOARD"); @@ -324,7 +430,6 @@ void GoToState::destinationReached(ControlFSM& fsm, bool z_reached) { } - ascend_msgs::ControlFSMState GoToState::getStateMsg() const { using ascend_msgs::ControlFSMState; ControlFSMState msg; diff --git a/src/fsm/interact_gb_state.cpp b/src/fsm/interact_gb_state.cpp index 7462d4a..e863c7f 100644 --- a/src/fsm/interact_gb_state.cpp +++ b/src/fsm/interact_gb_state.cpp @@ -1,4 +1,5 @@ #include "control/fsm/interact_gb_state.hpp" +#include "control/fsm/control_fsm.hpp" #include "control/tools/setpoint_msg_defines.h" #include @@ -11,7 +12,14 @@ void InteractGBState::handleEvent(ControlFSM& fsm, const EventData& event) { } void InteractGBState::stateBegin(ControlFSM& fsm, const EventData& event) { - //TODO Implement + + if (true /*TODO:check with obstacle avoidance*/){ + fsm.obstacle_avoidance_.relaxResponsibility(); + } else{ + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + return; + } } void InteractGBState::loopState(ControlFSM& fsm) { diff --git a/src/fsm/land_state.cpp b/src/fsm/land_state.cpp index b72cbe3..b501ebd 100644 --- a/src/fsm/land_state.cpp +++ b/src/fsm/land_state.cpp @@ -47,6 +47,7 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { if(!control::DroneHandler::isLocalPoseValid()) { throw control::PoseNotValidException(); } + auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; //Position XY is ignored in typemask, but the values are set as a precaution. @@ -63,6 +64,15 @@ void LandState::stateBegin(ControlFSM& fsm, const EventData& event) { } } + //reset obstacle avoidance flag + obstacle_avoidance_kicked_in_ = false; + + // TODO:Check with obstacle_avoidance if it is ok to land + if (true){ + // assuming obstacle_avoidance doesn't complain, disable it during landing + fsm.obstacle_avoidance_.relaxResponsibility(); + } + //Only land blind when the drone is below a certain altitude if(position.z >= control::Config::min_in_air_alt) { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND; @@ -94,13 +104,18 @@ void LandState::loopState(ControlFSM& fsm) { } else { setpoint_.type_mask = default_mask | SETPOINT_TYPE_LAND | IGNORE_PX | IGNORE_PY; } - + + if (obstacle_avoidance_kicked_in_){ + RequestEvent abort_event(RequestType::ABORT); + fsm.transitionTo(ControlFSM::POSITION_HOLD_STATE, this, abort_event); + } + //Only use distance sensor if required bool low_enough = true; if(control::Config::require_distance_sensor) { low_enough = control::DroneHandler::getCurrentDistance().range < 0.1; } - + //Check landing if(LandDetector::isOnGround() && low_enough) { if(cmd_.isValidCMD()) { @@ -121,6 +136,19 @@ void LandState::loopState(ControlFSM& fsm) { } } + +//Initialize state +void LandState::stateInit(ControlFSM& fsm) { + std::function obstacleAvoidanceCB = [this]()->void { + this->obstacle_avoidance_kicked_in_ = true; + }; + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + setStateIsReady(); + control::handleInfoMsg("Land init completed!"); +} + + const mavros_msgs::PositionTarget* LandState::getSetpointPtr() { setpoint_.header.stamp = ros::Time::now(); return &setpoint_; diff --git a/src/fsm/manual_flight_state.cpp b/src/fsm/manual_flight_state.cpp index 98fa283..8436675 100644 --- a/src/fsm/manual_flight_state.cpp +++ b/src/fsm/manual_flight_state.cpp @@ -51,6 +51,7 @@ void printLandStateOnChange(bool landed) { void ManualFlightState::stateBegin(ControlFSM& fsm, const EventData& event) { printLandState(LandDetector::isOnGround()); + fsm.obstacle_avoidance_.relaxResponsibility(); } void ManualFlightState::loopState(ControlFSM& fsm) { @@ -87,7 +88,6 @@ void ManualFlightState::handleManual(ControlFSM &fsm) { } - ascend_msgs::ControlFSMState ManualFlightState::getStateMsg() const { using ascend_msgs::ControlFSMState; ControlFSMState msg; diff --git a/src/fsm/position_hold_state.cpp b/src/fsm/position_hold_state.cpp index a1ccc38..8d0223a 100644 --- a/src/fsm/position_hold_state.cpp +++ b/src/fsm/position_hold_state.cpp @@ -44,6 +44,24 @@ void PositionHoldState::handleEvent(ControlFSM& fsm, const EventData& event) { } } +void PositionHoldState::stateInit(ControlFSM& fsm) { + std::function obstacleAvoidanceCB = [this]()->void { + //TODO: logic to avoid being pushed around + + // keep new setpoint after obstacle avoidance + auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); + auto& position = pose_stamped.pose.position; + //Set setpoint to current position + setpoint_.position.x = position.x; + setpoint_.position.y = position.y; + }; + + fsm.obstacle_avoidance_.registerOnWarnCBPtr(std::make_shared< std::function >(obstacleAvoidanceCB)); + + setStateIsReady(); + control::handleInfoMsg("PositionHold init completed!"); +} + void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { using control::Config; using control::DroneHandler; @@ -63,6 +81,7 @@ void PositionHoldState::stateBegin(ControlFSM& fsm, const EventData& event) { //Error if pose is not valid throw control::PoseNotValidException(); } + //Get pose - and position auto pose_stamped = control::DroneHandler::getCurrentLocalPose(); auto& position = pose_stamped.pose.position; @@ -133,5 +152,3 @@ ascend_msgs::ControlFSMState PositionHoldState::getStateMsg() const { return msg; } - - diff --git a/src/fsm/state_interface.cpp b/src/fsm/state_interface.cpp index b38b2f7..4d2e377 100644 --- a/src/fsm/state_interface.cpp +++ b/src/fsm/state_interface.cpp @@ -2,7 +2,7 @@ //This should maybe be moved to another file, but which one? -std::list* StateInterface::getAllStatesVector() { +std::list* StateInterface::getAllStatesVector() { static std::list all_states_; return &all_states_; } diff --git a/src/nodes/path_planner.cpp b/src/nodes/path_planner.cpp deleted file mode 100644 index 883e2f6..0000000 --- a/src/nodes/path_planner.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "control/planner/path_planner.hpp" - - -PathPlanner::PathPlanner() -{ - -} diff --git a/src/nodes/path_planner_node.cpp b/src/nodes/path_planner_node.cpp new file mode 100644 index 0000000..64f3d64 --- /dev/null +++ b/src/nodes/path_planner_node.cpp @@ -0,0 +1,175 @@ +#include +#include +#include "control/planner/path_planner.hpp" +#include "control/tools/drone_handler.hpp" +#include +#include +#include +#include "control/tools/planner_config.hpp" +#include "ascend_msgs/PointArrayStamped.h" +#include "ascend_msgs/PathPlanner.h" +#include "control/tools/obstacle_state_handler.hpp" + +using control::pathplanner::PathPlanner; + +struct PlannerState{ + float current_x, current_y, goal_x, goal_y; + bool make_plan = false; + bool new_goal = false; +}; + + + +using Request = ascend_msgs::PathPlanner::Request; +using Response = ascend_msgs::PathPlanner::Response; + +bool newPlanCB(Request &req, Response &res, PlannerState* planner_state){ + + if(req.cmd == Request::ABORT){ + ROS_INFO("Service callback: abort"); + planner_state->make_plan = false; + } + else if(req.cmd == Request::MAKE_PLAN){ + ROS_INFO("Service callback: make new plan"); + + planner_state->make_plan = true; + + //Accept the new goal + planner_state->new_goal = true; + + planner_state->goal_x = req.goal_x; + planner_state->goal_y = req.goal_y; + } + else{ + ROS_INFO("Service callback: not relevant"); + } + return true; +} + + +int main(int argc, char** argv){ + + ros::init(argc, argv, "path_planner_server"); + ros::NodeHandle n; + ros::Rate rate(5); + + ROS_INFO("Path planner node started"); + + control::PlannerConfig::loadParams(); + + PlannerState planner_state; + PathPlanner plan(control::PlannerConfig::obstacle_radius, control::PlannerConfig::node_distance); + std::list simple_plan; + geometry_msgs::Point point; + std::list obstacles; + + ros::Publisher pub_plan = n.advertise(control::PlannerConfig::plan_points_topic, 1); + ros::ServiceServer server = n.advertiseService("path_planner_service", boost::bind(&newPlanCB, _1, _2, &planner_state)); + + ascend_msgs::PointArrayStamped msg; + auto& points_in_plan = msg.points; + // For saving memory + points_in_plan.reserve(20); + + while(!control::DroneHandler::isGlobalPoseValid() && control::ObstacleStateHandler::isInstanceReady() && ros::ok()){ + ros::spinOnce(); + ros::Duration(1.0).sleep(); + } + + while(ros::ok()){ + + ros::spinOnce(); + if(planner_state.make_plan){ + auto obstacles_msg = control::ObstacleStateHandler::getCurrentObstacles(); + obstacles.clear(); + auto current_coord = obstacles_msg.global_robot_position.begin(); + auto current_dir = obstacles_msg.direction.begin(); + while(current_coord != obstacles_msg.global_robot_position.end() && current_dir != obstacles_msg.direction.end()){ + obstacles.push_back(control::pathplanner::Obstacle(current_coord->x, current_coord->y,*current_dir)); + current_coord++; + current_dir++; + } + + plan.setObstacles(obstacles); + plan.refreshObstacles(); + plan.refreshUnsafeZones(); + + // Update planner state + geometry_msgs::PoseStamped current_pose = control::DroneHandler::getCurrentGlobalPose(); + auto& position = current_pose.pose.position; + planner_state.current_x = position.x; + planner_state.current_y = position.y; + } + + // Make new plan as long as a plan is requested and the current one is invalid or the goal is changed + if(planner_state.make_plan && (!plan.isPlanSafe(planner_state.current_x,planner_state.current_y) || planner_state.new_goal)){ + ROS_INFO("Make new plan."); + planner_state.new_goal = false; + plan.makePlan(planner_state.current_x, planner_state.current_y, planner_state.goal_x, planner_state.goal_y); + simple_plan = plan.getSimplePlan(); + + + + // Removing old plan + points_in_plan.clear(); + + if(!simple_plan.empty()){ + + // The first point in the plan is the current point of the drone, so it doesn't need to be sent as part of the plan + std::list::iterator second_point = ++(simple_plan.begin()); + + std::cout << "Published points:\t"; + for(std::list::iterator it = second_point; it != simple_plan.end(); it++){ + + point.x = it->getX(); + point.y = it->getY(); + + points_in_plan.push_back(point); + + std::cout << point.x << ", " << point.y << " "; + + } + std::cout << std::endl; + } + } + + bool is_plan_updated = plan.removeOldPoints(planner_state.current_x, planner_state.current_y); + + // Publish plan as long as a plan is requested + if(planner_state.make_plan){ + if(is_plan_updated){ + // Removing old plan + points_in_plan.clear(); + simple_plan = plan.getSimplePlan(); + if(!simple_plan.empty()){ + + std::cout << "Published points:\t"; + for(std::list::iterator it = simple_plan.begin(); it != simple_plan.end(); it++){ + + point.x = it->getX(); + point.y = it->getY(); + + points_in_plan.push_back(point); + + std::cout << point.x << ", " << point.y << " "; + + } + std::cout << std::endl; + } + } + /*std::cout << "Published points:\t"; + for(auto it = points_in_plan.begin(); it != points_in_plan.end(); it++){ + std::cout << it->x << ", " << it->y << "\t" << std::endl; + }*/ + msg.header.stamp = ros::Time::now(); + pub_plan.publish(msg); + //std::cout << std::endl; + } + + rate.sleep(); + + } + + ros::spin(); + +} diff --git a/src/nodes/test_path_planner_node.cpp b/src/nodes/test_path_planner_node.cpp new file mode 100644 index 0000000..dbad261 --- /dev/null +++ b/src/nodes/test_path_planner_node.cpp @@ -0,0 +1,47 @@ +#include +#include "ascend_msgs/PathPlanner.h" + + +int main(int argc, char** argv){ + + ROS_INFO("Test path planner node started"); + + using Request = ascend_msgs::PathPlanner::Request; + + ros::init(argc, argv, "path_planner_test"); + ros::NodeHandle n; + ros::Rate rate(30.0); + + ros::ServiceClient client = n.serviceClient("path_planner_service"); + + ascend_msgs::PathPlanner clientCall; + + clientCall.request.cmd = Request::MAKE_PLAN; + clientCall.request.goal_x = 1; + clientCall.request.goal_y = 1; + + if(client.call(clientCall)){ + ROS_INFO("Request handled"); + } + else{ + ROS_WARN("Error!"); + } + + /*while(ros::ok()){ + ROS_INFO("Ros is ok"); + ros::spin(); + } + + ros::Duration(3).sleep(); + + clientCall.request.goal_x = 1; + clientCall.request.goal_y = 8; + + if(client.call(clientCall)){ + ROS_INFO("Request handled"); + } + else{ + ROS_WARN("Error!"); + }*/ + +} diff --git a/src/path_planner/node.cpp b/src/path_planner/node.cpp new file mode 100644 index 0000000..b536f9a --- /dev/null +++ b/src/path_planner/node.cpp @@ -0,0 +1,11 @@ +#include "control/planner/node.hpp" + + +bool operator< (const Node &lhs, const Node &rhs){ + return lhs.f > rhs.f; +} + +void Node::updateF(float new_g){ + g = new_g; + f = g+h; +} diff --git a/src/path_planner/path_planner.cpp b/src/path_planner/path_planner.cpp new file mode 100644 index 0000000..d38a76d --- /dev/null +++ b/src/path_planner/path_planner.cpp @@ -0,0 +1,458 @@ +#include "control/planner/path_planner.hpp" +#include "control/tools/obstacle_math.hpp" + + +using namespace control::pathplanner; + +bool debug = false; + + +PathPlanner::PathPlanner(float obstacle_radius, float node_distance) + : obstacle_radius(obstacle_radius), node_distance(node_distance){ + diagonal_node_distance = sqrt(2*node_distance*node_distance); + // plus 1 to include both 0 and the last node + graph_size = ceil(FIELD_LENGTH/node_distance+1); + + graph.resize(graph_size, std::vector(graph_size)); +} + +int PathPlanner::coordToIndex(float coord) { + int index = round(coord/node_distance); + return (index); +} + +void PathPlanner::initializeGraph(){ + if (debug) {std::cout << "Init" << std::endl;} + for (float x = 0; x < FIELD_LENGTH; x += node_distance){ + for (float y = 0; y < FIELD_LENGTH; y += node_distance){ + if(isStart(x,y)){ + graph[coordToIndex(x)][coordToIndex(y)] = start_node; + // Push the start node to the open list so that the search starts from the start + open_list.push(graph[coordToIndex(x)][coordToIndex(y)]); + } + else { + graph[coordToIndex(x)][coordToIndex(y)] = + Node(x,y, std::numeric_limits::infinity(), calculateEuclidianHeuristic(x,y)); + } + } + } +} + +void PathPlanner::setObstacles(std::list &obstacles) { + this->obstacles = obstacles; +} + +void PathPlanner::addObstacle(float center_x, float center_y) { + // Find the y value for each x in the circle + for (float x = center_x-obstacle_radius; x <= center_x+obstacle_radius; x+=node_distance) { + float y = sqrt(obstacle_radius*obstacle_radius-(x-center_x)*(x-center_x)); + // Do this to fill the circle + for(float i = center_y-y; i<=center_y+y; i+= node_distance) { + if (isValidCoordinate(x, i)) { + graph[coordToIndex(x)][coordToIndex(i)].obstacle = true; + } + } + + } + // Do the same as over, just switch x and y + for (float y = center_y-obstacle_radius; y <= center_y+obstacle_radius; y+=node_distance) { + float x = sqrt(obstacle_radius*obstacle_radius-(y-center_y)*(y-center_y)); + for(float i = center_x-x; i<=center_x+x; i+= node_distance) { + if (isValidCoordinate(i, y)) { + graph[coordToIndex(i)][coordToIndex(y)].obstacle = true; + } + } + } +} + +void PathPlanner::refreshObstacles() { + if (debug) {std::cout << "refreshObstacles" << std::endl;} + if(no_plan){ + return; + } + + // delete all old obstacles + for (float x = 0; x < FIELD_LENGTH; x += node_distance){ + for (float y = 0; y < FIELD_LENGTH; y += node_distance){ + graph[coordToIndex(x)][coordToIndex(y)].obstacle = false; + } + } + + for(auto current = obstacles.begin(); current != obstacles.end(); current++){ + addObstacle(current->x, current->y); + Obstacle next_obstacle = obstacleNextPos(current->x,current->y); + addObstacle(next_obstacle.x, next_obstacle.y); + } +} + +Obstacle PathPlanner::obstacleNextPos(float obstacle_x, float obstacle_y) { + float center_x = 10; + float center_y = 10; + float obstacle_angle = atan2(obstacle_y-center_y, obstacle_x-center_x); // radians + float angle_to_next_pos = 0.17; // radians + float radius = sqrt((center_x-obstacle_x)*(center_x-obstacle_x)+(center_y-obstacle_y)*(center_y-obstacle_y)); + float next_x = center_x + radius*cos(obstacle_angle+angle_to_next_pos); + float next_y = center_y + radius*sin(obstacle_angle+angle_to_next_pos); + return Obstacle(next_x,next_y,0); +} + +void PathPlanner::addUnsafeZone(float center_x, float center_y) { + // Same logic as adding obstacles, only difference is the radius + float unsafe_radius = obstacle_radius*2; + // Find the y value for each x in the circle + for (float x = center_x-unsafe_radius; x <= center_x+unsafe_radius; x+=node_distance) { + float y = sqrt(unsafe_radius*unsafe_radius-(x-center_x)*(x-center_x)); + // Do this to fill the circle + for(float i = center_y-y; i<=center_y+y; i+= node_distance) { + if (isValidCoordinate(x, i)) { + graph[coordToIndex(x)][coordToIndex(i)].unsafe = true; + } + } + + } + // Do the same as over, just switch x and y + for (float y = center_y-unsafe_radius; y <= center_y+unsafe_radius; y+=node_distance) { + float x = sqrt(unsafe_radius*unsafe_radius-(y-center_y)*(y-center_y)); + for(float i = center_x-x; i<=center_x+x; i+= node_distance) { + if (isValidCoordinate(i, y)) { + graph[coordToIndex(i)][coordToIndex(y)].unsafe = true; + } + } + } + +} +void PathPlanner::refreshUnsafeZones(){ + if (debug) {std::cout << "refreshUnsafeZones" << std::endl;} + if(no_plan){ + return; + } + // delete all old unsafe zones + for (float x = 0; x < FIELD_LENGTH; x += node_distance){ + for (float y = 0; y < FIELD_LENGTH; y += node_distance){ + graph[coordToIndex(x)][coordToIndex(y)].unsafe = false; + } + } + + + for(auto current = obstacles.begin(); current != obstacles.end(); current++){ + addUnsafeZone(current->x, current->y); + Obstacle next_obstacle = obstacleNextPos(current->x,current->y); + addUnsafeZone(next_obstacle.x, next_obstacle.y); + next_obstacle = obstacleNextPos(current->x,current->y); + addUnsafeZone(next_obstacle.x, next_obstacle.y); + } +} + +float PathPlanner::calculateDiagonalHeuristic(float x, float y) { + return (std::max(abs(x-end_node.getX()), abs(y-end_node.getY()))); +} + +float PathPlanner::calculateManhattanHeuristic(float x, float y){ + return (abs(x-end_node.getX()) + abs(y-end_node.getY())); +} + +float PathPlanner::calculateEuclidianHeuristic(float x, float y) { + return(sqrt((x-end_node.getX())*(x-end_node.getX())+(y-end_node.getY())*(y-end_node.getY()))); +} + +void PathPlanner::relaxGraph(){ + if (debug) {std::cout << "relaxGraph" << std::endl;} + Node current_node; + while(!open_list.empty()){ + // Always search from the node with lowest f value + current_node = open_list.top(); + open_list.pop(); + float x = current_node.getX(); + float y = current_node.getY(); + // Mark node as searched + graph[coordToIndex(x)][coordToIndex(y)].closed = true; + // Search all eight points around current point + handleAllSuccessors(x,y); + // Stop search if destination is reached + if(destination_reached){return;} + } +} + +void PathPlanner::handleSuccessor(float x, float y, float parent_x, float parent_y, float distance_to_parent) { + //std::cout << "Successor = " << x << ", " << y << std::endl; + if(isValidCoordinate(x,y)){ + int x_index = coordToIndex(x); + int y_index = coordToIndex(y); + // Making sure the zero is actually zero and not something like 1.4895e-9 + if(x > 0.0 && x < 0.01){x = 0.0;} + if(y > 0.0 && y < 0.01){y = 0.0;} + if(isDestination(x,y) && !graph[x_index][y_index].closed){ + graph[x_index][y_index].setParentX(parent_x); + graph[x_index][y_index].setParentY(parent_y); + destination_reached = true; + std::cout << "Destination reached: x=" << x << " y=" << y << std::endl; + /*std::cout << "Destination parent: x=" << graph[x_index][y_index].getParentX() + << " y=" << graph[coordToIndex(x)][coordToIndex(y)].getParentY() << std::endl;*/ + } + // If node is not destination and hasn't been searched yet + else if(!graph[x_index][y_index].closed){ + // Calculate distance from start through the parent + float new_g = graph[coordToIndex(parent_x)][coordToIndex(parent_y)].getG() + distance_to_parent; + // Update graph and open list if distance is less than before through the parent + if(graph[x_index][y_index].getG() > new_g) { + // The heuristic values 100 and 50 are just found by testing, and can be further tuned + // These values are to make the cost of moving through the current node larger + if (graph[x_index][y_index].obstacle) { + new_g = new_g * 100; + } else if (graph[x_index][y_index].unsafe) { + new_g = new_g * 50; + } + graph[x_index][y_index].updateF(new_g); + // For visualization + if(max_f < graph[x_index][y_index].getF()){ + max_f = graph[x_index][y_index].getF(); + } + graph[x_index][y_index].setParentX(parent_x); + graph[x_index][y_index].setParentY(parent_y); + open_list.push(graph[x_index][y_index]); + } + } + } +} + +void PathPlanner::handleAllSuccessors(float x, float y) { + if(debug) {std::cout << "q = " << x << ", " << y << std::endl;} + handleSuccessor(x+node_distance,y, x, y, node_distance); + if(destination_reached){return;} + handleSuccessor(x+node_distance, y+node_distance, x, y, diagonal_node_distance); + if(destination_reached){return;} + handleSuccessor(x+node_distance, y-node_distance, x, y, diagonal_node_distance); + if(destination_reached){return;} + handleSuccessor(x, y+node_distance, x, y, node_distance); + if(destination_reached){return;} + handleSuccessor(x, y-node_distance, x, y, node_distance); + if(destination_reached){return;} + handleSuccessor(x-node_distance, y, x, y, node_distance); + if(destination_reached){return;} + handleSuccessor(x-node_distance, y+node_distance, x, y, diagonal_node_distance); + if(destination_reached){return;} + handleSuccessor(x-node_distance, y-node_distance, x, y, diagonal_node_distance); +} + +bool PathPlanner::isDestination(float x, float y) { + return (coordToIndex(x) == coordToIndex(end_node.getX()) && coordToIndex(y) == coordToIndex(end_node.getY())); + +} + +bool PathPlanner::isStart(float x, float y) { + return (coordToIndex(x) == coordToIndex(start_node.getX()) && coordToIndex(y) == coordToIndex(start_node.getY())); + +} + +bool PathPlanner::isValidCoordinate(float x, float y) { + return(x>=0 && x=0 && yx) <= obstacle_radius*2 && abs(current_y-it->y) <= obstacle_radius*2) { + std::cout << "Found blocking obstacle" << std::endl; + return &(*it); + } + } + std::cout << "Could not find blocking obstacle!!" << std::endl; + return nullptr; +} + +void PathPlanner::makePlan(float current_x, float current_y, float target_x, float target_y) { + + if (debug) {std::cout << "Run makePlan" << std::endl;} + + no_plan = false; + + // If start or end point is not valid, a plan is not created + if(!isValidCoordinate(current_x,current_y)){ + ROS_INFO("Current point invalid!"); + return; + } + if(!isValidCoordinate(target_x,target_y)){ + ROS_INFO("Target point invalid!"); + return; + } + + resetParameters(); + + start_node = Node(current_x, current_y, 0, 0); + start_node.setParentX(0); + start_node.setParentY(0); + end_node = Node(target_x,target_y, std::numeric_limits::infinity(), 0); + initializeGraph(); + + refreshObstacles(); + refreshUnsafeZones(); + + float x = end_node.getX(); + float y = end_node.getY(); + + int x_index = coordToIndex(x); + int y_index = coordToIndex(y); + + // Calculate all f values and set the parents + relaxGraph(); + + // Dummy safety for avoiding infinite loop, will remove never + int counter = 0; + // Go through the graph from end to start through the parents of each node + // Add nodes to the plan + + // Actual endpoint + plan.push_front(end_node); + + while(!isStart(x, y)){ + // parent is calculated from the endpoint index, this node might have a different + // coordinate (depending on the grid size) + x = graph.at(x_index).at(y_index).getParentX(); + y = graph.at(x_index).at(y_index).getParentY(); + + + if(!isValidCoordinate(x,y)) { + std::cout << "Invalid parent: " << x << ", " << y << std::endl; + std::cout << "Index: " << x_index << ", " << y_index << std::endl; + } + + x_index = coordToIndex(x); + y_index = coordToIndex(y); + + counter ++; + if(counter >200){break;} + plan.push_front(graph.at(x_index).at(y_index)); + } + + // Delete unnecessary points + simplifyPlan(); +} + +void PathPlanner::simplifyPlan() { + simple_plan = plan; + // Pointing at the three first elements + std::list::iterator first = simple_plan.begin(); + std::list::iterator second = ++(simple_plan.begin()); + std::list::iterator third = ++(simple_plan.begin()); + third++; + + if(first == simple_plan.end() || second == simple_plan.end()){ + return; + } + + float x1, x2, y1, y2; + + while(third != simple_plan.end()){ + // Parameters for finding the straight line between the first and third point + x1 = first->getX(); + x2 = third->getX(); + y1 = first->getY(); + y2 = third->getY(); + + // If an unsafe zone is hit, the points are necessary and the search will start + // at the end of the current search. + if(!canSimplifyLine(x1,y1,x2,y2)){ + first++; + second++; + third++; + } + // If the straight line does not go through an unsafe zone, delete the second point + // (no use in going through it when we can go in a straight line) + // The search continues with same starting point, but the second and third point is changed + else{ + second = simple_plan.erase(second); + third++; + } + } +} + +bool PathPlanner::isPlanSafe(float current_x, float current_y) { + if(simple_plan.empty()){ + return false; + } + + std::list::iterator current = simple_plan.begin(); + std::list::iterator next = ++(simple_plan.begin()); + + // Check if current point to first point in remaining plan is safe + if(!isSafeLine(current_x, current_y, current->getX(), current->getY())){ + return false; + } + + // Check if rest of plan is safe + while(next != simple_plan.end() && current != simple_plan.end()){ + if(!isSafeLine(current->getX(), current->getY(), next->getX(), next->getY())){ + return false; + } + current++; + next++; + } + return true; +} + +void PathPlanner::resetParameters() { + std::cout << "Reset Parameters" << std::endl; + while(!open_list.empty()){ + open_list.pop(); + } + plan.clear(); + simple_plan.clear(); + destination_reached = false; +} + +bool PathPlanner::removeOldPoints(float current_x, float current_y){ + // Check if current point is the first point of the plan + float margin = node_distance; + + if(simple_plan.empty()){ + return false; + } + + // Do not remove end point from plan + if(++simple_plan.begin() == simple_plan.end()){ + return false; + } + + bool removed = false; + if(abs(current_x-simple_plan.begin()->getX()) < margin && abs(current_y-simple_plan.begin()->getY())getX() << ", " + << simple_plan.begin()->getY() << std::endl; + simple_plan.pop_front(); + removed = true; + } + return removed; +} \ No newline at end of file diff --git a/src/tools/config.cpp b/src/tools/config.cpp index 86a5467..045344c 100644 --- a/src/tools/config.cpp +++ b/src/tools/config.cpp @@ -34,6 +34,8 @@ std::string Config::mavros_state_changed_topic = "mavros/state"; std::string Config::land_detector_topic = "/landdetector"; std::string Config::land_detector_type = "landing_gear"; std::string Config::obstacle_state_topic = "/perception_obstacle_states"; +std::string Config::path_planner_plan_topic = "/control/planner/plan"; +std::string Config::path_planner_client_topic = "/control/planner/client"; std::string Config::debug_server_topic = "/control/fsm/debug_server"; std::string Config::action_server_topic = "/control/fsm/action_server"; int Config::fsm_status_buffer_size = 10; @@ -43,10 +45,13 @@ double Config::obstacle_too_close_dist = 2.0; std::string Config::lidar_topic = "perception/obstacles/lidar"; bool Config::require_all_data_streams = true; bool Config::require_obstacle_detection = true; +double Config::path_plan_timeout = 1.0; +double Config::obstacle_clearance_max = 2.0f; +double Config::obstacle_clearance_min = 1.5f; +double Config::obstacle_clearance_checkradius = 4.0f; std::string Config::global_frame_id = "map"; std::string Config::local_frame_id = "odom"; bool Config::use_global_transforms = true; - bool Config::restrict_arena_boundaries = true; double Config::arena_lowest_x = 0.0; double Config::arena_lowest_y = 0.0; @@ -155,6 +160,16 @@ void Config::loadParams() { getStringParam("land_detector_type", land_detector_type); //Obstacles getStringParam("obstacle_state_topic", obstacle_state_topic); + //Path planner + getDoubleParam("path_plan_timeout", path_plan_timeout, 0.0, 5.0); + getStringParam("path_planner_client_topic", path_planner_client_topic); + getStringParam("path_planner_plan_topic", path_planner_plan_topic); + //Obstacle avoidance + getDoubleParam("obstacle_clearance_max", obstacle_clearance_max, 0, 100); + getDoubleParam("obstacle_clearance_min", obstacle_clearance_min, 0, 100); + getDoubleParam("obstacle_clearance_checkradius", obstacle_clearance_checkradius, 0, 100); + //Action server + getStringParam("action_server_topic", action_server_topic); //Frame id getStringParam("global_frame_id", global_frame_id); getStringParam("local_frame_id", local_frame_id); @@ -168,7 +183,6 @@ void Config::loadParams() { getDoubleParam("arena_highest_x", arena_highest_x, -100.0, 100.0); getDoubleParam("arena_highest_y", arena_highest_y, -100.0, 100.0); getBoolParam("require_distance_sensor", require_distance_sensor); - } using Request = ascend_msgs::ReloadConfig::Request; diff --git a/src/tools/obstacle_avoidance.cpp b/src/tools/obstacle_avoidance.cpp index 1620cb1..66e6aa9 100644 --- a/src/tools/obstacle_avoidance.cpp +++ b/src/tools/obstacle_avoidance.cpp @@ -1,10 +1,165 @@ +#include + #include "control/tools/obstacle_avoidance.hpp" +#include "control/tools/obstacle_math.hpp" +#include "control/tools/obstacle_state_handler.hpp" +#include "control/tools/drone_handler.hpp" +#include "control/tools/config.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using obstacle_math::PI; +using obstacle_math::rotateXY; +using obstacle_math::angleWrapper; +using obstacle_math::vectorSum; +using obstacle_math::vectorDifference; +using obstacle_math::calcAngleToObstacle; +using obstacle_math::calcDistanceToObstacle; + +/// Return a vector (x,y,z) which corresponds to the point closest to the obstacle +/// which is allowed, referenced from the obstacles coordinate system. x-axis is positive +/// direction of motion. Obstacle driving in with obstacle_direction=0 would be driving in x-direction +geometry_msgs::Vector3 calcMinimumVector(const float drone_angle_to_obstacle, const float drone_direction, const float drone_speed_ratio, + const float clearance_min, const float clearance_max){ + + geometry_msgs::Vector3 minimum_vector; + + const auto clearance_front = clearance_min + drone_speed_ratio*(clearance_max - clearance_min); + + if (drone_speed_ratio > 0.1){ + const auto worst_direction = PI + drone_direction; + const auto angle = angleWrapper(drone_angle_to_obstacle - worst_direction); + + minimum_vector.x = clearance_front * std::cos(angle); + minimum_vector.y = clearance_min * std::sin(angle); + minimum_vector = rotateXY(minimum_vector, worst_direction); + + } else { + minimum_vector.x = clearance_min * std::cos(drone_angle_to_obstacle); + minimum_vector.y = clearance_min * std::sin(drone_angle_to_obstacle); + } + + minimum_vector.z = 0.0f; + + return minimum_vector; +} + +template // vector types with x,y,z +bool checkAndAvoidSingleObstacle(mavros_msgs::PositionTarget* setpoint, const V1& drone_position, const V2& drone_velocity, const V3& obstacle_global_position, ascend_msgs::PolygonArray& zone_msg){ + using control::Config; + + const auto drone_distance_to_obstacle = calcDistanceToObstacle(drone_position, obstacle_global_position); + const auto setpoint_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); + + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/1.0f, 1.f); // 1.0 m/s is assumed drone max speed + + bool setpoint_modified = false; + if (drone_distance_to_obstacle < Config::obstacle_clearance_checkradius){ + // perform obstacle avoidance + const auto drone_angle_to_obstacle = calcAngleToObstacle(drone_position, obstacle_global_position); + const auto setpoint_angle_to_obstacle = calcAngleToObstacle(setpoint->position, obstacle_global_position); + + // True if setpoint is within a 120 deg cone away from the obstacle + // TODO: turn this angle into a param once complete + const bool setpoint_reachable = fabs(fmod(drone_angle_to_obstacle - setpoint_angle_to_obstacle, 2*PI)) < 3*PI/8; + + const auto minimum_vector = calcMinimumVector(drone_angle_to_obstacle, drone_direction, drone_speed_ratio, Config::obstacle_clearance_min, Config::obstacle_clearance_max); + + const auto minimum_distance = std::sqrt(std::pow(minimum_vector.x, 2) + std::pow(minimum_vector.y, 2)); + if (drone_distance_to_obstacle < minimum_distance){ + if (setpoint_reachable + && setpoint_distance_to_obstacle > drone_distance_to_obstacle + && setpoint_distance_to_obstacle > minimum_distance){ + // no action, maybe logging? + ROS_INFO_THROTTLE(1, "[obstacle avoidance]: Prefer setpoint over avoid algorithm"); + } + else { + if (setpoint_modified){ + // TODO: find out a better solution to this problem + ROS_WARN("[obstacle avoidance]: Two obstacles in range, undefined behaviour!"); + setpoint->position.z = Config::safe_hover_altitude; // hover taller than all obstacles :) + } + // need to avoid obstacle + setpoint_modified = true; + + setpoint->position.x = obstacle_global_position.x + minimum_vector.x; + setpoint->position.y = obstacle_global_position.y + minimum_vector.y; + if (setpoint->position.z < Config::min_in_air_alt){ + setpoint->position.z = Config::min_in_air_alt; + } + } + } + } + + return setpoint_modified; +} bool control::ObstacleAvoidance::doObstacleAvoidance(mavros_msgs::PositionTarget* setpoint) { - //TODO Implement obstacle avoidance - //Return true if setpoint has been modified. - return false; + bool setpoint_modified{false}; + + const auto obstacles = control::ObstacleStateHandler::getCurrentObstacles(); + const auto drone_pose = control::DroneHandler::getCurrentLocalPose(); + const auto drone_velocity = control::DroneHandler::getCurrentTwist().twist.linear; + + ascend_msgs::PolygonArray zone_msg; + + const bool obstacles_valid = control::ObstacleStateHandler::isInstanceReady(); + + if (!obstacles_valid){ + ROS_WARN_THROTTLE(1, "[obstacle_avoidance]: ObstacleStateHandler not ready"); + } + + for (int i = 0; i < obstacles.count && obstacles_valid; i++){ + + if (checkAndAvoidSingleObstacle(setpoint, drone_pose.pose.position, drone_velocity, obstacles.global_robot_position[i], zone_msg)){ + setpoint_modified = true; + } + + // generate points for zone_pub_ + constexpr int N_points{16}; + geometry_msgs::Polygon polygon; + for (int j = 0; j < N_points; j++){ + const float angle = static_cast(j)*2.f*PI/static_cast(N_points); + const auto drone_direction = std::atan2(drone_velocity.y, drone_velocity.x); + const auto drone_speed = std::sqrt(std::pow(drone_velocity.x,2) + std::pow(drone_velocity.y,2)); + const float drone_speed_ratio = std::min((float)drone_speed/2.5f, 1.f); // 2.5 m/s is assumed drone max speed + + const auto local_pos = calcMinimumVector(angle, drone_direction, drone_speed_ratio, control::Config::obstacle_clearance_min, 2.0*control::Config::obstacle_clearance_min); + + geometry_msgs::Point32 global_pos; + global_pos.x = obstacles.global_robot_position[i].x + local_pos.x; + global_pos.y = obstacles.global_robot_position[i].y + local_pos.y; + global_pos.z = obstacles.global_robot_position[i].z + local_pos.z; + + polygon.points.push_back(global_pos); + } + + zone_msg.polygons.push_back(polygon); + + if (setpoint_modified){ + //const auto new_distance_to_obstacle = calcDistanceToObstacle(setpoint->position, obstacle_global_position); + //ROS_INFO_THROTTLE(1, "Distance improvement %.3f to %.3f", drone_distance_to_obstacle, new_distance_to_obstacle); + } + } + + zone_pub_.publish(zone_msg); + + return setpoint_modified; } void control::ObstacleAvoidance::onModified() { @@ -14,21 +169,46 @@ void control::ObstacleAvoidance::onModified() { } } +void control::ObstacleAvoidance::onWarn() { + //Run all callbacks + for(auto& cb_p : on_warn_cb_set_ ) { + (*cb_p)(); + } +} + mavros_msgs::PositionTarget control::ObstacleAvoidance::run(mavros_msgs::PositionTarget setpoint) { - //If obstacle avoidance has altered the setpoint - if(doObstacleAvoidance(&setpoint)) { + mavros_msgs::PositionTarget setpoint_unchanged = setpoint; + + const bool setpoint_modified = doObstacleAvoidance(&setpoint); + + if (setpoint_modified){ //Notify states - onModified(); + onWarn(); + if (has_setpoint_responsibility_){ + onModified(); + return setpoint; + } } - return setpoint; + + return setpoint_unchanged; } void control::ObstacleAvoidance::removeOnModifiedCBPtr(const std::shared_ptr >& cb_p) { - for(auto it = on_modified_cb_set_.begin(); it != on_modified_cb_set_.end(); ++it) { - if(*it == cb_p) { - on_modified_cb_set_.erase(it); - //std::set - no duplicates - return; - } + const auto it = on_modified_cb_set_.find(cb_p); + if (it != on_modified_cb_set_.cend()){ + on_modified_cb_set_.erase(it); + } +} + +void control::ObstacleAvoidance::removeOnWarnCBPtr(const std::shared_ptr >& cb_p) { + const auto it = on_warn_cb_set_.find(cb_p); + if (it != on_warn_cb_set_.cend()) { + on_warn_cb_set_.erase(it); } } + + +control::ObstacleAvoidance::ObstacleAvoidance(){ + zone_pub_ = nh_.advertise("/control/obstacle_avoidance/polygons", 10); +} + diff --git a/src/tools/obstacle_state_handler.cpp b/src/tools/obstacle_state_handler.cpp index d11170d..b6109ca 100644 --- a/src/tools/obstacle_state_handler.cpp +++ b/src/tools/obstacle_state_handler.cpp @@ -41,10 +41,7 @@ bool ObstacleStateHandler::isInstanceReady() { } bool ObstacleStateHandler::isReady() const { - if(last_msg_p_->count == 0) { - control::handleWarnMsg("Obstacle handler: No data available"); - return false; - } else if(control::message::hasTimedOut(*last_msg_p_)) { + if(control::message::hasTimedOut(*last_msg_p_)) { control::handleWarnMsg("Obstacle handler: Using old data"); return false; } diff --git a/src/tools/planner_config.cpp b/src/tools/planner_config.cpp new file mode 100644 index 0000000..27bc8a7 --- /dev/null +++ b/src/tools/planner_config.cpp @@ -0,0 +1,116 @@ +#include "control/tools/planner_config.hpp" +#include +#include +#include + +using control::PlannerConfig; +using control::ROSNotInitializedException; + + +std::set PlannerConfig::missing_param_set_; +std::unique_ptr PlannerConfig::shared_instance_p_ = nullptr; + +//Global PlannerConfig params + +std::string PlannerConfig::obstacle_state_topic = "/perception_obstacle_states"; +std::string PlannerConfig::mavros_setpoint_topic = "/mavros/setpoint_raw/local"; +std::string PlannerConfig::plan_points_topic = "/control/planner/plan"; + +double PlannerConfig::obstacle_radius = 1.0; +double PlannerConfig::node_distance = 0.4; + +void PlannerConfig::loadParams() { + if(!ros::isInitialized()) { + throw ROSNotInitializedException(); + } + if(shared_instance_p_ == nullptr) { + shared_instance_p_ = std::unique_ptr(new PlannerConfig); + } + ros::NodeHandle n("~"); + auto getDoubleParam = [&](const std::string& name, double& var, double min, double max) { + double temp; + if(!n.getParam(name, temp) || temp < min || temp > max) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += std::to_string(var); + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + var = temp; + control::handleInfoMsg("Param " + name + " loaded: " + std::to_string(var)); + } + }; + + auto getStringParam = [&](const std::string& name, std::string& var) { + if(!n.getParam(name, var)) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += var; + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + control::handleInfoMsg("Param " + name + " loaded: " + var); + } + }; + + auto getIntParam = [&](const std::string& name, int& var, int min, int max) { + int temp; + if(!n.getParam(name, temp) || temp < min || temp > max) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += std::to_string(var); + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + var = temp; + control::handleInfoMsg("Param " + name + " loaded: " + std::to_string(var)); + } + }; + + auto getBoolParam = [&](const std::string& name, bool& var) { + if(!n.getParam(name, var)) { + std::string warn_msg = "Load param failed: "; + warn_msg += name; + warn_msg += ", using "; + warn_msg += std::to_string(var); + control::handleWarnMsg(warn_msg); + missing_param_set_.insert(name); + } else { + control::handleInfoMsg("Param " + name + " loaded: " + std::to_string(var)); + } + }; + + //Global params (used by multiple states) + + //Obstacles + getStringParam("obstacle_state_topic", obstacle_state_topic); + getStringParam("mavros_setpoint_topic", mavros_setpoint_topic); + getStringParam("plan_points_topic", plan_points_topic); + + getDoubleParam("obstacle_radius", obstacle_radius,0,3); + getDoubleParam("node_distance", node_distance,0,1); + +} + +using Request = ascend_msgs::ReloadConfig::Request; +using Response = ascend_msgs::ReloadConfig::Response; +bool reloadPlannerConfigCB(Request&, Response& resp) { + control::PlannerConfig::loadParams(); + control::handleInfoMsg("PlannerConfig reloaded by service!"); + + //Missing param set should be empty! + for(auto& s : control::PlannerConfig::getMissingParamSet()) { + resp.missing_params.emplace_back(s); + } + return true; +} + +control::PlannerConfig::PlannerConfig() { + if(!ros::isInitialized()) { + throw control::ROSNotInitializedException(); + } + reload_config_service = nh_.advertiseService("/control_fsm_reload_Config", reloadPlannerConfigCB); +} diff --git a/test/utest.cpp b/test/utest.cpp index 888d712..d87e8d7 100644 --- a/test/utest.cpp +++ b/test/utest.cpp @@ -7,10 +7,15 @@ #define CONTROL_FSM_UNIT_TEST #endif +#include + #include #include #include #include +#include +#include +#include #include #include "gtest/gtest.h" #include @@ -124,9 +129,76 @@ TEST(ControlTest, quatConversionTest) { EXPECT_NEAR(PI, quat2yaw(pi), 0.001); EXPECT_NEAR(PI / 3.0, quat2yaw(pi3), 0.001); EXPECT_NEAR(0.0 - PI_HALF, quat2mavrosyaw(zero), 0.001); +} + +TEST(ControlTest, obstacleAvoidanceHelpers) { + + using obstacle_math::angleWrapper; + using obstacle_math::rotateXY; + // angleWrapper + EXPECT_NEAR(0.f, angleWrapper(0.f), 0.001); + EXPECT_NEAR(2.f, angleWrapper(2.f), 0.001); + EXPECT_NEAR(4.f, angleWrapper(4.f), 0.001); + EXPECT_NEAR(0.7168f, angleWrapper(7.f), 0.001); + EXPECT_NEAR(2.8673f, angleWrapper(28.f), 0.001); + EXPECT_NEAR(5.2832f, angleWrapper(-1.f), 0.001); + EXPECT_NEAR(0.8496f, angleWrapper(-18.f), 0.001); + + geometry_msgs::Vector3 vec = {}; + geometry_msgs::Vector3 zero = {}; + geometry_msgs::Vector3 unitx; unitx.x=1.0; unitx.y=0.0; unitx.z=0.0; + geometry_msgs::Vector3 unity; unity.x=0.0; unity.y=1.0; unity.z=0.0; + + geometry_msgs::Vector3 unitz; unitz.x=0.0; unitz.y=0.0; unitz.z=1.0; + + // rotateXY + geometry_msgs::Vector3 result = rotateXY(unitx, PI/2); + EXPECT_NEAR(unity.x, result.x, 0.001); + EXPECT_NEAR(unity.y, result.y, 0.001); + EXPECT_NEAR(unity.z, result.z, 0.001); + + result = rotateXY(unitx, PI); + EXPECT_NEAR(-unitx.x, result.x, 0.001); + EXPECT_NEAR(-unitx.y, result.y, 0.001); + EXPECT_NEAR(-unitx.z, result.z, 0.001); + + result = rotateXY(unity, 3*PI/2); + EXPECT_NEAR(unitx.x, result.x, 0.001); + EXPECT_NEAR(unitx.y, result.y, 0.001); + EXPECT_NEAR(unitx.z, result.z, 0.001); + + result = rotateXY(unitz, 10.0); + EXPECT_NEAR(unitz.x, result.x, 0.001); + EXPECT_NEAR(unitz.y, result.y, 0.001); + EXPECT_NEAR(unitz.z, result.z, 0.001); + + vec.x=2.0; vec.y=4.0; vec.z=3.0; + result = rotateXY(vec, 12.3); + EXPECT_NEAR(2.9824, result.x, 0.001); + EXPECT_NEAR(3.3325, result.y, 0.001); + EXPECT_NEAR(3.0, result.z, 0.001); + + // calcVectorBetweenPoints + } +TEST(PathPlannerTest, coordToIndexTest) { + control::pathplanner::PathPlanner obj1(1,0.4); + EXPECT_EQ(obj1.coordToIndex(0.0), 0); + EXPECT_EQ(obj1.coordToIndex(0.9), 2); + EXPECT_EQ(obj1.coordToIndex(0.7999), 2); + EXPECT_EQ(obj1.coordToIndex(0.5999), 1); + EXPECT_EQ(obj1.coordToIndex(0.6), 2); + + control::pathplanner::PathPlanner obj2(1, 0.5); + EXPECT_EQ(obj2.coordToIndex(0.0), 0); + EXPECT_EQ(obj2.coordToIndex(0.5), 1); + EXPECT_EQ(obj2.coordToIndex(0.2499), 0); + EXPECT_EQ(obj2.coordToIndex(10), 20); + EXPECT_EQ(obj2.coordToIndex(0.4999), 1); +} + TEST(ControlTest, arenaBoundariesTest) { using control::Config; double valid_x = Config::arena_lowest_x + (Config::arena_highest_x - Config::arena_lowest_x) / 2.0; @@ -149,4 +221,3 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -