|
MFCPP
1.0
|
Corentin Chauvin-Hameau – 2019-2020
|
|
Coverage Path Planning for an underwater robot surveying a marine farm
|
|
Base class for a node managing high level planning logic. More...
#include <planning_logic.hpp>
Public Member Functions | |
| PlanningLogic () | |
| ~PlanningLogic () | |
| void | run_node () |
| Runs the node. More... | |
Private Member Functions | |
| void | init_node () |
| Initialises the node and its parameters. More... | |
| void | load_path (std::string file_name, float resolution, nav_msgs::Path &path) |
| Loads a path from a text file. More... | |
| void | load_transition_pts (std::string file_name) |
| Loads the points delimiting the transitions. More... | |
| geometry_msgs::Pose | find_closest (const std::vector< geometry_msgs::PoseStamped > &path, const geometry_msgs::Pose &pose) |
| Finds the closest pose in a path. More... | |
| void | odom_cb (const nav_msgs::Odometry msg) |
| Callback for the odometry. More... | |
Private Attributes | |
| ros::NodeHandle | nh_ |
| Node handler. More... | |
| ros::Subscriber | odom_sub_ |
| Subscriber for robot's odometry. More... | |
| ros::Publisher | path_pub_ |
| Publisher for a path. More... | |
| ros::ServiceClient | disable_planner_client_ |
| Client for disabling the path planner. More... | |
| ros::ServiceClient | enable_planner_client_ |
| Client for enabling the path planner. More... | |
| nav_msgs::Odometry | odom_ |
| Odometry of the robot. More... | |
| nav_msgs::Path | path_ |
| Path to publish. More... | |
| bool | odom_received_ |
| Whether the odometry has been received. More... | |
| bool | path_loaded_ |
| Whether the path was loaded with success. More... | |
| bool | transition_pts_loaded_ |
| Whether the transition points have been loaded. More... | |
| bool | transition_ |
| Whether the robot is following a transition path. More... | |
| std::vector< Eigen::Vector3f > | transition_pts_ |
| Points marking start and end of transition paths. More... | |
ROS parameters | |
| float | main_freq_ |
Base class for a node managing high level planning logic.
The planner alternates between hard-coded path for transitions between lines, and informative path planner to survey a wall.
Definition at line 31 of file planning_logic.hpp.
| mfcpp::PlanningLogic::PlanningLogic | ( | ) |
Definition at line 35 of file planning_logic.cpp.
| mfcpp::PlanningLogic::~PlanningLogic | ( | ) |
Definition at line 42 of file planning_logic.cpp.
|
private |
Finds the closest pose in a path.
| [in] | path | List of poses |
| [in] | pose | Current pose |
pose in path Definition at line 231 of file planning_logic.cpp.
|
private |
Initialises the node and its parameters.
Definition at line 48 of file planning_logic.cpp.
|
private |
Loads a path from a text file.
The text file contains waypoints that are then interpolated by a spline.
| [in] | file_name | Relative path of the file containing the waypoints |
| [in] | resolution | Spatial resolution of the path |
| [out] | path | Loaded path |
Definition at line 139 of file planning_logic.cpp.
|
private |
Loads the points delimiting the transitions.
| file_name | Relative path of the file containing the points |
Definition at line 198 of file planning_logic.cpp.
|
private |
Callback for the odometry.
Definition at line 254 of file planning_logic.cpp.
| void mfcpp::PlanningLogic::run_node | ( | ) |
Runs the node.
Definition at line 93 of file planning_logic.cpp.
|
private |
Client for disabling the path planner.
Definition at line 46 of file planning_logic.hpp.
|
private |
Client for enabling the path planner.
Definition at line 47 of file planning_logic.hpp.
|
private |
Main frequency
Definition at line 59 of file planning_logic.hpp.
|
private |
Node handler.
Definition at line 43 of file planning_logic.hpp.
|
private |
Odometry of the robot.
Definition at line 49 of file planning_logic.hpp.
|
private |
Whether the odometry has been received.
Definition at line 51 of file planning_logic.hpp.
|
private |
Subscriber for robot's odometry.
Definition at line 44 of file planning_logic.hpp.
|
private |
Path to publish.
Definition at line 50 of file planning_logic.hpp.
|
private |
Whether the path was loaded with success.
Definition at line 52 of file planning_logic.hpp.
|
private |
Publisher for a path.
Definition at line 45 of file planning_logic.hpp.
|
private |
Whether the robot is following a transition path.
Definition at line 54 of file planning_logic.hpp.
|
private |
Points marking start and end of transition paths.
Definition at line 55 of file planning_logic.hpp.
|
private |
Whether the transition points have been loaded.
Definition at line 53 of file planning_logic.hpp.
1.8.11