Nodelet for path planning of an underwater robot surveying a marine farm. More...

#include <planning_nodelet.hpp>

Inheritance diagram for mfcpp::PlanningNodelet:

Collaboration diagram for mfcpp::PlanningNodelet:

Public Member Functions
	PlanningNodelet ()

	~PlanningNodelet ()

virtual void	onInit ()
	Function called at beginning of nodelet execution. More...

Private Member Functions
void	main_cb (const ros::TimerEvent &timer_event)
	Main callback which is called by a timer. More...

bool	check_planning_needed ()
	Checks whether planning is needed or not. More...

std::vector< mf_common::Float32Array >	vector2D_to_array (const std::vector< std::vector< float >> &vector2D)
	Converts a 2D std vector to a custom ROS array. More...

std::vector< std::vector< float > >	array_to_vector2D (const std::vector< mf_common::Float32Array > &array)
	Converts a custom ROS array to a 2D std vector. More...

std::vector< std::vector< float > >	array_to_vector2D (const mf_common::Array2D &array)
	Converts a custom ROS array to a 2D std vector. More...

Eigen::Vector3f	get_wall_orientation (float &yaw_wall)
	Gets horizontal unit vector along the wall with same orientation as the robot. More...

Eigen::Vector3f	get_wall_orientation ()
	Gets horizontal unit vector along the wall with same orientation as the robot. More...

void	generate_cart_lattice (float max_lat_angle, float max_elev_angle, float horizon, float resolution, Lattice &lattice)
	Fills a cartesian lattice of possible waypoints. More...

void	generate_lattice (Lattice &lattice)
	Fills a lattice of possible waypoints based on motion model. More...

void	generate_lattices (const RobotModel::state_type &init_state, std::vector< Lattice > &lattices)
	Generates several lattices along the wall. More...

void	filter_lattice (Lattice &lattice_in, Lattice &lattice_out)
	Filters out waypoints that are not in given bounds. More...

void	transform_lattices (std::vector< Lattice > &lattices, const geometry_msgs::TransformStamped &transform)
	Apply a TF transform to all nodes of a set of lattices. More...

void	add_node (const RobotModel::state_type &state, const geometry_msgs::TransformStamped &frame_ocean_tf, Lattice &lattice)
	Adds a node given by its state to a lattice. More...

void	connect_lattices (const RobotModel::state_type &init_state, std::vector< Lattice > &lattices_in, std::vector< Lattice > &lattices_out)
	Connects lattices and removes viewpoints not dynamically reachable. More...

bool	compute_lattice_gp (Lattice &lattice, ros::Time stamp)
	Computes the diagonal of the covariance for each viewpoint of the lattice. More...

void	compute_info_gain (Lattice &lattice)
	Computes information gain over a lattice of viewpoint. More...

std::vector< LatticeNode * >	select_viewpoints (const std::vector< Lattice > &lattices)
	Selects a sequence of viewpoints maximising information gain over a list of lattices. More...

void	select_viewpoints (LatticeNode node, float &info_gain, std::vector< LatticeNode > &selected_vp)
	Selects recursively a list of nodes viewpoints maximising information gain. More...

void	fill_display_obj (const std::vector< Lattice > &lattices, const std::vector< LatticeNode * > &selected_vp)
	Fills display objects. More...

void	generate_path (const std::vector< geometry_msgs::Pose > &selected_vp, nav_msgs::Path &path, std::vector< geometry_msgs::Pose > &waypoints)
	Generates a spline trajectory between the selected points. More...

nav_msgs::Path	straight_line_path (const geometry_msgs::Pose &start, const geometry_msgs::Pose &end, float resolution)
	Interpolates a straight line path between two poses. More...

nav_msgs::Path	spline_path (const std::vector< geometry_msgs::Pose > &waypoints, float resolution, float speed)
	Interpolates a spline path between a list of poses. More...

bool	plan_trajectory ()
	Main function to compute a trajectory plan. More...

void	pub_lattice_markers ()
	Publishes the waypoints lattice markers. More...

bool	get_tf ()
	Gets tf transforms. More...

void	gp_mean_cb (const mf_common::Float32ArrayConstPtr msg)
	Callback for Gaussian Process mean. More...

void	gp_cov_cb (const mf_common::Array2DConstPtr msg)
	Callback for Gaussian Process covariance. More...

void	state_cb (const mf_common::Float32Array msg)
	Callback for the state of the robot. More...

bool	disable_cb (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	Service server callback to disable planning. More...

bool	enable_cb (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	Service server callback to enable planning. More...

geometry_msgs::Pose	tf_to_pose (const geometry_msgs::TransformStamped &transf)
	Converts a tf transform to a pose, assuming the frames correspond. More...

Static Private Member Functions
static void	sigint_handler (int s)
	SINGINT (Ctrl+C) callback to stop the nodelet properly. More...

Private Attributes
ros::NodeHandle	nh_
	Node handler (for topics and services) More...

ros::NodeHandle	private_nh_
	Private node handler (for parameters) More...

ros::ServiceClient	ray_multi_client_
	Service client for raycasting at several camera poses. More...

ros::ServiceClient	update_gp_client_
	Service client for updating the Gaussian Process. More...

ros::ServiceServer	disable_serv_
	Service server to disable planning. More...

ros::ServiceServer	enable_serv_
	Service server to enable planning. More...

ros::Publisher	lattice_pub_
	Publisher for the waypoints lattice. More...

ros::Publisher	lattice_pose_pub_
	Publisher for the waypoints lattice poses. More...

ros::Publisher	path_pub_
	Publisher for the path. More...

ros::Subscriber	gp_mean_sub_
	Subscriber for the mean of the Gaussian Process. More...

ros::Subscriber	gp_cov_sub_
	Subscriber for the covariance of the Gaussian Process. More...

ros::Subscriber	state_sub_
	Subscriber for the state of the robot. More...

tf2_ros::Buffer	tf_buffer_
	Buffer for tf2. More...

tf2_ros::TransformListener	tf_listener_
	Transform listener for tf2. More...

bool	planner_enabled_
	Whether the planner should run. More...

RobotModel	robot_model_
	Robot model. More...

RobotModel::state_type	state_
	State of the robot (in ocean frame) More...

bool	state_received_
	Whether the state of the robot has been received. More...

geometry_msgs::TransformStamped	wall_robot_tf_
	Transform from wall to robot frames. More...

geometry_msgs::TransformStamped	robot_wall_tf_
	Transform from robot to wall frames. More...

geometry_msgs::TransformStamped	ocean_robot_tf_
	Transform from ocean to robot frames. More...

geometry_msgs::TransformStamped	robot_ocean_tf_
	Transform from robot to ocean frames. More...

geometry_msgs::TransformStamped	ocean_wall_tf_
	Transform from ocean to wall frames. More...

geometry_msgs::TransformStamped	wall_ocean_tf_
	Transform from wall to ocean frames. More...

geometry_msgs::TransformStamped	ocean_camera_tf_
	Transform from ocean to camera frames. More...

std::vector< geometry_msgs::Pose >	lattice_
	Lattice of possible waypoints in ocean frame. More...

std::vector< geometry_msgs::Pose >	selected_vp_
	Selected view points in the lattice (in ocean frame) More...

std::vector< float >	x_hit_pt_sel_
	X coordinates of the hit points for the selected viewpoint (in ocean frame) More...

std::vector< float >	y_hit_pt_sel_
	Y coordinates of the hit points for the selected viewpoint (in ocean frame) More...

std::vector< float >	z_hit_pt_sel_
	Z coordinates of the hit points for the selected viewpoint (in ocean frame) More...

std::vector< float >	last_gp_mean_
	Last mean of the Gaussian Process. More...

std::vector< std::vector< float > >	last_gp_cov_
	Last covariance of the Gaussian Process. More...

std::vector< geometry_msgs::Pose >	waypoints_
	Waypoints to follow (in ocean frame) More...

nav_msgs::Path	path_
	Interpolated path between the waypoints (in ocean frame) More...

General ROS parameters
float	main_freq_
	Frequency of the main loop. More...

std::string	ocean_frame_
	Ocean frame. More...

std::string	wall_frame_
	Wall frame. More...

std::string	robot_frame_
	Robot frame. More...

std::string	camera_frame_
	Camera frame. More...

float	length_wall_

ROS parameters for lattice of viewpoint generation
float	max_lat_rudder_
	Maximum angle of the lateral rudder. More...

float	max_elev_rudder_
	Maximum angle of the elevation rudder. More...

bool	horiz_motion_
	Whether to allow motion in the horizontal plane. More...

bool	vert_motion_
	Whether to allow motion in the vertical plane. More...

bool	mult_lattices_
	Whether to use multi-lattices planning. More...

bool	replan_
	Whether to replan from the current robot pose, or to plan from the last planned pose. More...

int	nbr_lattices_
	Number of lattices for multi-lattices planning. More...

bool	cart_lattice_
	Whether to create a cartesian lattice, or use motion model instead. More...

float	plan_speed_
	Planned speed (m/s) of the robot. More...

float	plan_horizon_
	Horizon (m) of the planning (for each lattice) More...

int	lattice_size_horiz_
	Size of the lattice in the horizontal direction (half size when single-lattice planning) More...

int	lattice_size_vert_
	Size of the lattice in the vertical direction (half size when single-lattice planning) More...

float	wall_orientation_
	Orientation of the wall (absolute value) More...

float	lattice_res_
	Resolution (m) of the waypoints lattice. More...

std::vector< float >	bnd_wall_dist_
	Bounds on the distance to the wall for waypoint selection. More...

std::vector< float >	bnd_depth_
	Bounds on the depth (in wall frame) for waypoint selection. More...

float	bnd_pitch_

ROS parameters for viewpoint selection
int	camera_height_
	Number of pixels of the camera along height (-1 for actual camera size) More...

int	camera_width_
	Number of pixels of the camera along width (-1 for actual camera size) More...

float	gp_threshold_

ROS parameters for path generation
bool	linear_path_
	Whether to create a linear path, or a spline one. More...

float	path_res_
	Spatial resolution (m) of the planned trajectory. More...

float	min_wall_dist_

Static Private Attributes
static sig_atomic_t volatile	b_sigint_ = 0
	Whether SIGINT signal has been received. More...

static ros::Timer	main_timer_ = ros::Timer()
	Timer callback for the main function. More...

Detailed Description

Nodelet for path planning of an underwater robot surveying a marine farm.

Definition at line 34 of file planning_nodelet.hpp.

Constructor & Destructor Documentation

mfcpp::PlanningNodelet::PlanningNodelet ( )

Definition at line 43 of file planning_nodelet.cpp.

mfcpp::PlanningNodelet::~PlanningNodelet ( )

Definition at line 50 of file planning_nodelet.cpp.

Member Function Documentation

void mfcpp::PlanningNodelet::add_node	(	const RobotModel::state_type &	state,
		const geometry_msgs::TransformStamped &	frame_ocean_tf,
		Lattice &	lattice
	)

private

Adds a node given by its state to a lattice.

Parameters

[in]	state	State of the node (in ocean frame)
[in]	frame_ocean_tf	Transform between "frame" and "ocean" frames
[in,out]	lattice	Lattice with an added node transformed in "frame" frame

Definition at line 343 of file planning.cpp.

std::vector<std::vector<float> > mfcpp::PlanningNodelet::array_to_vector2D ( const std::vector< mf_common::Float32Array > & array )

private

Converts a custom ROS array to a 2D std vector.

Parameters

The	array to convert

Returns: Converted 2D vector

vector< vector< float > > mfcpp::PlanningNodelet::array_to_vector2D ( const mf_common::Array2D & array )

private

Converts a custom ROS array to a 2D std vector.

Parameters

The	array to convert

Returns: Converted 2D vector

Definition at line 61 of file planning.cpp.

bool mfcpp::PlanningNodelet::check_planning_needed ( )

private

Checks whether planning is needed or not.

If the last planned waypoint is at the end of the wall, it is not required to plan more.

Returns: Whether to plan more

Definition at line 192 of file planning_nodelet.cpp.

void mfcpp::PlanningNodelet::compute_info_gain ( Lattice & lattice )

private

Computes information gain over a lattice of viewpoint.

Parameters

[in,out] lattice Lattice of viewpoint

Definition at line 575 of file planning.cpp.

bool mfcpp::PlanningNodelet::compute_lattice_gp	(	Lattice &	lattice,
		ros::Time	stamp
	)

private

Computes the diagonal of the covariance for each viewpoint of the lattice.

Will also store the camera hit points for each viewpoint

Parameters

[in,out]	lattice	Lattice of viewpoints
[in]	stamp	Time at which to fetch the ROS transforms

Definition at line 487 of file planning.cpp.

void mfcpp::PlanningNodelet::connect_lattices	(	const RobotModel::state_type &	init_state,
		std::vector< Lattice > &	lattices_in,
		std::vector< Lattice > &	lattices_out
	)

private

Connects lattices and removes viewpoints not dynamically reachable.

Propagates the motion model from all the viewpoints to determine which viewpoints are reachable in the next lattice. Viewpoints not reachable from any viewpoint of the previous lattice (or from the initial state) are removed.

Warning: The input lattice won't be usable anymore (pointers losing ownership)

Todo:: Remove nodes of the first lattice that doesn't lead to the right tree depth.

Parameters

[in]	init_state	Initial state of the robot
[in]	lattices_in	Input lattices
[out]	lattices_out	Output lattices

Definition at line 362 of file planning.cpp.

bool mfcpp::PlanningNodelet::disable_cb	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

private

Service server callback to disable planning.

Definition at line 339 of file planning_nodelet.cpp.

bool mfcpp::PlanningNodelet::enable_cb	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

private

Service server callback to enable planning.

Definition at line 353 of file planning_nodelet.cpp.

void mfcpp::PlanningNodelet::fill_display_obj	(	const std::vector< Lattice > &	lattices,
		const std::vector< LatticeNode * > &	selected_vp
	)

private

Fills display objects.

Parameters

[in]	lattices	Lattices of viewpoints
[in]	selected_vp	Selected viewpoints

Definition at line 651 of file planning.cpp.

void mfcpp::PlanningNodelet::filter_lattice	(	Lattice &	lattice_in,
		Lattice &	lattice_out
	)

private

Filters out waypoints that are not in given bounds.

Bounds are on the position with respect to the wall and the pitch angle. Waypoints going backwards are also discarded.

Warning: The input lattice won't be usable anymore (pointers losing ownership)

Parameters

Definition at line 282 of file planning.cpp.

void mfcpp::PlanningNodelet::generate_cart_lattice	(	float	max_lat_angle,
		float	max_elev_angle,
		float	horizon,
		float	resolution,
		Lattice &	lattice
	)

private

Fills a cartesian lattice of possible waypoints.

Generates a lattice of possible waypoints in robot frame. These waypoints will be aligned on a cartesian grid, within specified limit angles and horizon distance.

Parameters

[in]	max_lat_angle	Maximum lateral angle
[in]	max_elev_angle	Maximum elevation angle
[in]	horizon	Maximum distance
[in]	resolution	Spatial resolution of the lattice
[out]	lattice	Lattice to fill

Definition at line 100 of file planning.cpp.

void mfcpp::PlanningNodelet::generate_lattice ( Lattice & lattice )

private

Fills a lattice of possible waypoints based on motion model.

Generate the lattice by picking different control inputs and applying it during a fixed time. The speed and horizontal angle commands will be constant. The horizontal command will be interpolated to guaranty that the final pose is parallel to the algae wall.

Parameters

[out] lattice Lattice to fill

Definition at line 152 of file planning.cpp.

void mfcpp::PlanningNodelet::generate_lattices	(	const RobotModel::state_type &	init_state,
		std::vector< Lattice > &	lattices
	)

private

Generates several lattices along the wall.

The lattices are simple cartesian grids along the wall. The viewpoints are expressed in robot frame.

Parameters

[in]	init_state	State at beginning of planning
[out]	lattices	Lattices of viewpoints (assumed to be already sized)

Definition at line 205 of file planning.cpp.

void mfcpp::PlanningNodelet::generate_path	(	const std::vector< geometry_msgs::Pose > &	selected_vp,
		nav_msgs::Path &	path,
		std::vector< geometry_msgs::Pose > &	waypoints
	)

private

Generates a spline trajectory between the selected points.

Everything is in ocean frame.

Parameters

[in]	selected_vp	Selected viewpoints
[out]	new_path	New path to append to the previous one
[out]	waypoints	New waypoints to append to the previous ones

Definition at line 702 of file planning.cpp.

bool mfcpp::PlanningNodelet::get_tf ( )

private

Gets tf transforms.

Returns: Whether it could retrieve a transform

Definition at line 291 of file planning_nodelet.cpp.

Eigen::Vector3f mfcpp::PlanningNodelet::get_wall_orientation ( float & yaw_wall )

private

Gets horizontal unit vector along the wall with same orientation as the robot.

Parameters

[out] Yaw of the wall

Returns: Wall orientation vector in ocean frame

Definition at line 75 of file planning.cpp.

Eigen::Vector3f mfcpp::PlanningNodelet::get_wall_orientation ( )

private

Gets horizontal unit vector along the wall with same orientation as the robot.

Returns: Wall orientation in ocean frame

Definition at line 93 of file planning.cpp.

void mfcpp::PlanningNodelet::gp_cov_cb ( const mf_common::Array2DConstPtr msg )

private

Callback for Gaussian Process covariance.

Definition at line 326 of file planning_nodelet.cpp.

void mfcpp::PlanningNodelet::gp_mean_cb ( const mf_common::Float32ArrayConstPtr msg )

private

Callback for Gaussian Process mean.

Definition at line 320 of file planning_nodelet.cpp.

void mfcpp::PlanningNodelet::main_cb ( const ros::TimerEvent & timer_event )

private

Main callback which is called by a timer.

Parameters

timer_event Timer event information

Definition at line 155 of file planning_nodelet.cpp.

void mfcpp::PlanningNodelet::onInit ( )

virtual

Function called at beginning of nodelet execution.

Definition at line 53 of file planning_nodelet.cpp.

bool mfcpp::PlanningNodelet::plan_trajectory ( )

private

Main function to compute a trajectory plan.

Definition at line 812 of file planning.cpp.

void mfcpp::PlanningNodelet::pub_lattice_markers ( )

private

Publishes the waypoints lattice markers.

Definition at line 212 of file planning_nodelet.cpp.

std::vector< LatticeNode * > mfcpp::PlanningNodelet::select_viewpoints ( const std::vector< Lattice > & lattices )

private

Selects a sequence of viewpoints maximising information gain over a list of lattices.

Parameters

[in] lattices List of lattices of viewpoints

Returns: Sequence of viewpoints

Definition at line 595 of file planning.cpp.

void mfcpp::PlanningNodelet::select_viewpoints	(	LatticeNode *	node,
		float &	info_gain,
		std::vector< LatticeNode * > &	selected_vp
	)

private

Selects recursively a list of nodes viewpoints maximising information gain.

Parameters

[in]	node	Current viewpoint node
[out]	info_gain	Cumulated information gain of this node and the best next ones
[out]	selected_vp	List of this viewpoint, and the next best ones

Definition at line 616 of file planning.cpp.

void mfcpp::PlanningNodelet::sigint_handler ( int s )

staticprivate

SINGINT (Ctrl+C) callback to stop the nodelet properly.

Definition at line 184 of file planning_nodelet.cpp.

nav_msgs::Path mfcpp::PlanningNodelet::spline_path	(	const std::vector< geometry_msgs::Pose > &	waypoints,
		float	resolution,
		float	speed
	)

private

Interpolates a spline path between a list of poses.

Parameters

waypoints	List of pose
resolution	Spatial resolution of the path
speed	Desired speed along the path

Returns: Generated path

Definition at line 766 of file planning.cpp.

void mfcpp::PlanningNodelet::state_cb ( const mf_common::Float32Array msg )

private

Callback for the state of the robot.

Definition at line 332 of file planning_nodelet.cpp.

nav_msgs::Path mfcpp::PlanningNodelet::straight_line_path	(	const geometry_msgs::Pose &	start,
		const geometry_msgs::Pose &	end,
		float	resolution
	)

private

Interpolates a straight line path between two poses.

Parameters

p1	Start of the path
p2	End of the path
resolution	Spatial resolution of the path

Returns: Generated path

Definition at line 732 of file planning.cpp.

geometry_msgs::Pose mfcpp::PlanningNodelet::tf_to_pose ( const geometry_msgs::TransformStamped & transf )

inlineprivate

Converts a tf transform to a pose, assuming the frames correspond.

Parameters

transf Transform to convert Converted pose

Definition at line 450 of file planning_nodelet.hpp.

void mfcpp::PlanningNodelet::transform_lattices	(	std::vector< Lattice > &	lattices,
		const geometry_msgs::TransformStamped &	transform
	)

private

Apply a TF transform to all nodes of a set of lattices.

Definition at line 329 of file planning.cpp.

vector< mf_common::Float32Array > mfcpp::PlanningNodelet::vector2D_to_array ( const std::vector< std::vector< float >> & vector2D )

private

Converts a 2D std vector to a custom ROS array.

Parameters

2D	vector to convert

Returns: Converted array

Definition at line 33 of file planning.cpp.

Member Data Documentation

sig_atomic_t volatile mfcpp::PlanningNodelet::b_sigint_ = 0

staticprivate

Whether SIGINT signal has been received.

Definition at line 47 of file planning_nodelet.hpp.

std::vector<float> mfcpp::PlanningNodelet::bnd_depth_

private

Bounds on the depth (in wall frame) for waypoint selection.

Definition at line 115 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::bnd_pitch_

private

Bound on the pith (in radian) (provided in degree as ROS param)

Definition at line 116 of file planning_nodelet.hpp.

std::vector<float> mfcpp::PlanningNodelet::bnd_wall_dist_

private

Bounds on the distance to the wall for waypoint selection.

Definition at line 114 of file planning_nodelet.hpp.

std::string mfcpp::PlanningNodelet::camera_frame_

private

Camera frame.

Definition at line 93 of file planning_nodelet.hpp.

int mfcpp::PlanningNodelet::camera_height_

private

Number of pixels of the camera along height (-1 for actual camera size)

Definition at line 121 of file planning_nodelet.hpp.

int mfcpp::PlanningNodelet::camera_width_

private

Number of pixels of the camera along width (-1 for actual camera size)

Definition at line 122 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::cart_lattice_

private

Whether to create a cartesian lattice, or use motion model instead.

Definition at line 107 of file planning_nodelet.hpp.

ros::ServiceServer mfcpp::PlanningNodelet::disable_serv_

private

Service server to disable planning.

Definition at line 55 of file planning_nodelet.hpp.

ros::ServiceServer mfcpp::PlanningNodelet::enable_serv_

private

Service server to enable planning.

Definition at line 56 of file planning_nodelet.hpp.

ros::Subscriber mfcpp::PlanningNodelet::gp_cov_sub_

private

Subscriber for the covariance of the Gaussian Process.

Definition at line 61 of file planning_nodelet.hpp.

ros::Subscriber mfcpp::PlanningNodelet::gp_mean_sub_

private

Subscriber for the mean of the Gaussian Process.

Definition at line 60 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::gp_threshold_

private

Threshold to consider a GP component in information gain computation

Definition at line 123 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::horiz_motion_

private

Whether to allow motion in the horizontal plane.

Definition at line 102 of file planning_nodelet.hpp.

std::vector<std::vector<float> > mfcpp::PlanningNodelet::last_gp_cov_

private

Last covariance of the Gaussian Process.

Definition at line 83 of file planning_nodelet.hpp.

std::vector<float> mfcpp::PlanningNodelet::last_gp_mean_

private

Last mean of the Gaussian Process.

Definition at line 82 of file planning_nodelet.hpp.

std::vector<geometry_msgs::Pose> mfcpp::PlanningNodelet::lattice_

private

Lattice of possible waypoints in ocean frame.

Definition at line 77 of file planning_nodelet.hpp.

ros::Publisher mfcpp::PlanningNodelet::lattice_pose_pub_

private

Publisher for the waypoints lattice poses.

Definition at line 58 of file planning_nodelet.hpp.

ros::Publisher mfcpp::PlanningNodelet::lattice_pub_

private

Publisher for the waypoints lattice.

Definition at line 57 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::lattice_res_

private

Resolution (m) of the waypoints lattice.

Definition at line 113 of file planning_nodelet.hpp.

int mfcpp::PlanningNodelet::lattice_size_horiz_

private

Size of the lattice in the horizontal direction (half size when single-lattice planning)

Definition at line 110 of file planning_nodelet.hpp.

int mfcpp::PlanningNodelet::lattice_size_vert_

private

Size of the lattice in the vertical direction (half size when single-lattice planning)

Definition at line 111 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::length_wall_

private

Length of the algae wall

Definition at line 94 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::linear_path_

private

Whether to create a linear path, or a spline one.

Definition at line 128 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::main_freq_

private

Frequency of the main loop.

Definition at line 89 of file planning_nodelet.hpp.

ros::Timer mfcpp::PlanningNodelet::main_timer_ = ros::Timer()

staticprivate

Timer callback for the main function.

Definition at line 48 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::max_elev_rudder_

private

Maximum angle of the elevation rudder.

Definition at line 100 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::max_lat_rudder_

private

Maximum angle of the lateral rudder.

Definition at line 99 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::min_wall_dist_

private

Minimum distance to the wall that can be planned

Definition at line 130 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::mult_lattices_

private

Whether to use multi-lattices planning.

Definition at line 104 of file planning_nodelet.hpp.

int mfcpp::PlanningNodelet::nbr_lattices_

private

Number of lattices for multi-lattices planning.

Definition at line 106 of file planning_nodelet.hpp.

ros::NodeHandle mfcpp::PlanningNodelet::nh_

private

Node handler (for topics and services)

Definition at line 51 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::ocean_camera_tf_

private

Transform from ocean to camera frames.

Definition at line 76 of file planning_nodelet.hpp.

std::string mfcpp::PlanningNodelet::ocean_frame_

private

Ocean frame.

Definition at line 90 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::ocean_robot_tf_

private

Transform from ocean to robot frames.

Definition at line 72 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::ocean_wall_tf_

private

Transform from ocean to wall frames.

Definition at line 74 of file planning_nodelet.hpp.

nav_msgs::Path mfcpp::PlanningNodelet::path_

private

Interpolated path between the waypoints (in ocean frame)

Definition at line 85 of file planning_nodelet.hpp.

ros::Publisher mfcpp::PlanningNodelet::path_pub_

private

Publisher for the path.

Definition at line 59 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::path_res_

private

Spatial resolution (m) of the planned trajectory.

Definition at line 129 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::plan_horizon_

private

Horizon (m) of the planning (for each lattice)

Definition at line 109 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::plan_speed_

private

Planned speed (m/s) of the robot.

Definition at line 108 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::planner_enabled_

private

Whether the planner should run.

Definition at line 66 of file planning_nodelet.hpp.

ros::NodeHandle mfcpp::PlanningNodelet::private_nh_

private

Private node handler (for parameters)

Definition at line 52 of file planning_nodelet.hpp.

ros::ServiceClient mfcpp::PlanningNodelet::ray_multi_client_

private

Service client for raycasting at several camera poses.

Definition at line 53 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::replan_

private

Whether to replan from the current robot pose, or to plan from the last planned pose.

Definition at line 105 of file planning_nodelet.hpp.

std::string mfcpp::PlanningNodelet::robot_frame_

private

Robot frame.

Definition at line 92 of file planning_nodelet.hpp.

RobotModel mfcpp::PlanningNodelet::robot_model_

private

Robot model.

Definition at line 67 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::robot_ocean_tf_

private

Transform from robot to ocean frames.

Definition at line 73 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::robot_wall_tf_

private

Transform from robot to wall frames.

Definition at line 71 of file planning_nodelet.hpp.

std::vector<geometry_msgs::Pose> mfcpp::PlanningNodelet::selected_vp_

private

Selected view points in the lattice (in ocean frame)

Definition at line 78 of file planning_nodelet.hpp.

RobotModel::state_type mfcpp::PlanningNodelet::state_

private

State of the robot (in ocean frame)

Definition at line 68 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::state_received_

private

Whether the state of the robot has been received.

Definition at line 69 of file planning_nodelet.hpp.

ros::Subscriber mfcpp::PlanningNodelet::state_sub_

private

Subscriber for the state of the robot.

Definition at line 62 of file planning_nodelet.hpp.

tf2_ros::Buffer mfcpp::PlanningNodelet::tf_buffer_

private

Buffer for tf2.

Definition at line 63 of file planning_nodelet.hpp.

tf2_ros::TransformListener mfcpp::PlanningNodelet::tf_listener_

private

Transform listener for tf2.

Definition at line 64 of file planning_nodelet.hpp.

ros::ServiceClient mfcpp::PlanningNodelet::update_gp_client_

private

Service client for updating the Gaussian Process.

Definition at line 54 of file planning_nodelet.hpp.

bool mfcpp::PlanningNodelet::vert_motion_

private

Whether to allow motion in the vertical plane.

Definition at line 103 of file planning_nodelet.hpp.

std::string mfcpp::PlanningNodelet::wall_frame_

private

Wall frame.

Definition at line 91 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::wall_ocean_tf_

private

Transform from wall to ocean frames.

Definition at line 75 of file planning_nodelet.hpp.

float mfcpp::PlanningNodelet::wall_orientation_

private

Orientation of the wall (absolute value)

Definition at line 112 of file planning_nodelet.hpp.

geometry_msgs::TransformStamped mfcpp::PlanningNodelet::wall_robot_tf_

private

Transform from wall to robot frames.

Definition at line 70 of file planning_nodelet.hpp.

std::vector<geometry_msgs::Pose> mfcpp::PlanningNodelet::waypoints_

private

Waypoints to follow (in ocean frame)

Definition at line 84 of file planning_nodelet.hpp.

std::vector<float> mfcpp::PlanningNodelet::x_hit_pt_sel_

private

X coordinates of the hit points for the selected viewpoint (in ocean frame)

Definition at line 79 of file planning_nodelet.hpp.

std::vector<float> mfcpp::PlanningNodelet::y_hit_pt_sel_

private

Y coordinates of the hit points for the selected viewpoint (in ocean frame)

Definition at line 80 of file planning_nodelet.hpp.

std::vector<float> mfcpp::PlanningNodelet::z_hit_pt_sel_

private

Z coordinates of the hit points for the selected viewpoint (in ocean frame)

Definition at line 81 of file planning_nodelet.hpp.

The documentation for this class was generated from the following files:

/home/corentin/thesis_ws/src/marine-farm-cpp/mf_navigation/include/mf_navigation/planning_nodelet.hpp
/home/corentin/thesis_ws/src/marine-farm-cpp/mf_navigation/src/mf_navigation/planning.cpp
/home/corentin/thesis_ws/src/marine-farm-cpp/mf_navigation/src/mf_navigation/planning_nodelet.cpp

MFCPP 1.0	Corentin Chauvin-Hameau – 2019-2020
Coverage Path Planning for an underwater robot surveying a marine farm

Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation