#include <mesh_controller.h>

Inheritance diagram for mesh_controller::MeshController:

Public Types
typedef boost::shared_ptr< mesh_controller::MeshController >	Ptr
	shared pointer typedef to simplify pointer access of the mesh controller More...

Public Types inherited from mbf_mesh_core::MeshController
typedef boost::shared_ptr< mbf_mesh_core::MeshController >	Ptr

Public Types inherited from mbf_abstract_core::AbstractController
typedef boost::shared_ptr< ::mbf_abstract_core::AbstractController >	Ptr

Public Member Functions
virtual bool	cancel ()
	Requests the planner to cancel, e.g. if it takes too much time. More...

virtual uint32_t	computeVelocityCommands (const geometry_msgs::PoseStamped &pose, const geometry_msgs::TwistStamped &velocity, geometry_msgs::TwistStamped &cmd_vel, std::string &message)
	Given the current position, orientation, and velocity of the robot, compute the next velocity commands to move the robot towards the goal. More...

float	gaussValue (const float &sigma_squared, const float &value)

virtual bool	initialize (const std::string &plugin_name, const boost::shared_ptr< tf2_ros::Buffer > &tf_ptr, const boost::shared_ptr< mesh_map::MeshMap > &mesh_map_ptr)
	Initializes the controller plugin with a name, a tf pointer and a mesh map pointer. More...

virtual bool	isGoalReached (double dist_tolerance, double angle_tolerance)
	Checks if the robot reached to goal pose. More...

	MeshController ()
	Constructor. More...

std::array< float, 2 >	naiveControl (const mesh_map::Vector &robot_pos, const mesh_map::Normal &robot_dir, const mesh_map::Vector &mesh_dir, const mesh_map::Normal &mesh_normal, const float &mesh_cost)

mesh_map::Normal	poseToDirectionVector (const geometry_msgs::PoseStamped &pose, const tf2::Vector3 &axis=tf2::Vector3(1, 0, 0))

mesh_map::Vector	poseToPositionVector (const geometry_msgs::PoseStamped &pose)

void	reconfigureCallback (mesh_controller::MeshControllerConfig &cfg, uint32_t level)
	reconfigure callback function which is called if a dynamic reconfiguration were triggered. More...

virtual bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &plan)
	Sets the current plan to follow, it also sets the vector field. More...

virtual	~MeshController ()
	Destructor. More...

Public Member Functions inherited from mbf_abstract_core::AbstractController
virtual	~AbstractController ()

Private Attributes
ros::Publisher	angle_pub
	publishes the angle between the robots orientation and the goal vector field for debug purposes More...

std::atomic_bool	cancel_requested
	flag to handle cancel requests More...

MeshControllerConfig	config
	current mesh controller configuration More...

dynamic_reconfigure::Server< mesh_controller::MeshControllerConfig >::CallbackType	config_callback
	dynamic reconfigure callback function binding More...

lvr2::OptionalFaceHandle	current_face
	The triangle on which the robot is located. More...

vector< geometry_msgs::PoseStamped >	current_plan
	the current set plan More...

mesh_map::Normal	goal_dir
	the goal's and robot's orientation More...

mesh_map::Vector	goal_pos
	the goal and robot pose More...

boost::shared_ptr< mesh_map::MeshMap >	map_ptr
	shared pointer to the used mesh map More...

boost::shared_ptr< dynamic_reconfigure::Server< mesh_controller::MeshControllerConfig > >	reconfigure_server_ptr
	shared pointer to dynamic reconfigure server More...

mesh_map::Normal	robot_dir

mesh_map::Vector	robot_pos

lvr2::DenseVertexMap< mesh_map::Vector >	vector_map
	The vector field to the goal. More...

Additional Inherited Members
Protected Member Functions inherited from mbf_mesh_core::MeshController
	MeshController ()

Protected Member Functions inherited from mbf_abstract_core::AbstractController
	AbstractController ()

Detailed Description

Definition at line 46 of file mesh_controller.h.

Member Typedef Documentation

◆ Ptr

typedef boost::shared_ptr<mesh_controller::MeshController> mesh_controller::MeshController::Ptr

shared pointer typedef to simplify pointer access of the mesh controller

Definition at line 51 of file mesh_controller.h.

Constructor & Destructor Documentation

◆ MeshController()

mesh_controller::MeshController::MeshController ( )

Constructor.

Definition at line 61 of file mesh_controller.cpp.

◆ ~MeshController()

mesh_controller::MeshController::~MeshController ( )

virtual

Destructor.

Reimplemented from mbf_mesh_core::MeshController.

Definition at line 65 of file mesh_controller.cpp.

Member Function Documentation

◆ cancel()

bool mesh_controller::MeshController::cancel ( )

virtual

Requests the planner to cancel, e.g. if it takes too much time.

Returns: True if cancel has been successfully requested, false otherwise

Implements mbf_mesh_core::MeshController.

Definition at line 197 of file mesh_controller.cpp.

◆ computeVelocityCommands()

uint32_t mesh_controller::MeshController::computeVelocityCommands	(	const geometry_msgs::PoseStamped &	pose,
		const geometry_msgs::TwistStamped &	velocity,
		geometry_msgs::TwistStamped &	cmd_vel,
		std::string &	message
	)

virtual

Given the current position, orientation, and velocity of the robot, compute the next velocity commands to move the robot towards the goal.

Parameters

pose	The current pose of the robot
velocity	The current velocity of the robot
cmd_vel	Computed velocity command
message	Detailed outcome as string message

Returns: An mbf_msgs/ExePathResult outcome code

Implements mbf_mesh_core::MeshController.

Definition at line 69 of file mesh_controller.cpp.

◆ gaussValue()

float mesh_controller::MeshController::gaussValue	(	const float &	sigma_squared,
		const float &	value
	)

A normal distribution / gaussian function

Parameters

sigma_squared	The squared sigma / variance
value	The value to be evaluated

Returns: the gauss faction value

Definition at line 217 of file mesh_controller.cpp.

◆ initialize()

bool mesh_controller::MeshController::initialize	(	const std::string &	plugin_name,
		const boost::shared_ptr< tf2_ros::Buffer > &	tf_ptr,
		const boost::shared_ptr< mesh_map::MeshMap > &	mesh_map_ptr
	)

virtual

Initializes the controller plugin with a name, a tf pointer and a mesh map pointer.

Parameters

plugin_name	The controller plugin name, defined by the user. It defines the controller namespace
tf_ptr	A shared pointer to a transformation buffer
mesh_map_ptr	A shared pointer to the mesh map

Returns: true if the plugin has been initialized successfully

Implements mbf_mesh_core::MeshController.

Definition at line 246 of file mesh_controller.cpp.

◆ isGoalReached()

bool mesh_controller::MeshController::isGoalReached	(	double	dist_tolerance,
		double	angle_tolerance
	)

virtual

Checks if the robot reached to goal pose.

Parameters

pose	The current pose of the robot
angle_tolerance	The angle tolerance in which the current pose will be partly accepted as reached goal
dist_tolerance	The distance tolerance in which the current pose will be partly accepted as reached goal

Returns: true if the goal is reached

Implements mbf_mesh_core::MeshController.

Definition at line 174 of file mesh_controller.cpp.

◆ naiveControl()

std::array< float, 2 > mesh_controller::MeshController::naiveControl	(	const mesh_map::Vector &	robot_pos,
		const mesh_map::Normal &	robot_dir,
		const mesh_map::Vector &	mesh_dir,
		const mesh_map::Normal &	mesh_normal,
		const float &	mesh_cost
	)

Computes the angular and linear velocity

Parameters

robot_pos	robot position
robot_dir	robot orientation
mesh_dir	supposed orientation
mesh_cost	cost value at robot position

Returns: angular and linear velocity

Definition at line 222 of file mesh_controller.cpp.

◆ poseToDirectionVector()

mesh_map::Normal mesh_controller::MeshController::poseToDirectionVector	(	const geometry_msgs::PoseStamped &	pose,
		const tf2::Vector3 &	axis = `tf2::Vector3(1,0,0)`
	)

Converts the orientation of a geometry_msgs/PoseStamped message to a direction vector

Parameters

pose	the pose to convert

Returns: direction normal vector

Definition at line 204 of file mesh_controller.cpp.

◆ poseToPositionVector()

mesh_map::Vector mesh_controller::MeshController::poseToPositionVector ( const geometry_msgs::PoseStamped & pose )

Converts the position of a geometry_msgs/PoseStamped message to a position vector

Parameters

pose	the pose to convert

Returns: position vector

Definition at line 212 of file mesh_controller.cpp.

◆ reconfigureCallback()

void mesh_controller::MeshController::reconfigureCallback	(	mesh_controller::MeshControllerConfig &	cfg,
		uint32_t	level
	)

reconfigure callback function which is called if a dynamic reconfiguration were triggered.

Definition at line 241 of file mesh_controller.cpp.

◆ setPlan()

bool mesh_controller::MeshController::setPlan ( const std::vector< geometry_msgs::PoseStamped > & plan )

virtual

Sets the current plan to follow, it also sets the vector field.

Parameters

plan	The plan to follow

Returns: true if the plan was set successfully, false otherwise

Implements mbf_mesh_core::MeshController.

Definition at line 181 of file mesh_controller.cpp.

Member Data Documentation

◆ angle_pub

ros::Publisher mesh_controller::MeshController::angle_pub

private

publishes the angle between the robots orientation and the goal vector field for debug purposes

Definition at line 186 of file mesh_controller.h.

◆ cancel_requested

std::atomic_bool mesh_controller::MeshController::cancel_requested

private

flag to handle cancel requests

Definition at line 189 of file mesh_controller.h.

◆ config

MeshControllerConfig mesh_controller::MeshController::config

private

current mesh controller configuration

Definition at line 183 of file mesh_controller.h.

◆ config_callback

dynamic_reconfigure::Server<mesh_controller::MeshControllerConfig>::CallbackType mesh_controller::MeshController::config_callback

private

dynamic reconfigure callback function binding

Definition at line 180 of file mesh_controller.h.

◆ current_face

lvr2::OptionalFaceHandle mesh_controller::MeshController::current_face

private

The triangle on which the robot is located.

Definition at line 171 of file mesh_controller.h.

◆ current_plan

vector<geometry_msgs::PoseStamped> mesh_controller::MeshController::current_plan

private

the current set plan

Definition at line 162 of file mesh_controller.h.

◆ goal_dir

mesh_map::Normal mesh_controller::MeshController::goal_dir

private

the goal's and robot's orientation

Definition at line 168 of file mesh_controller.h.

◆ goal_pos

mesh_map::Vector mesh_controller::MeshController::goal_pos

private

the goal and robot pose

Definition at line 165 of file mesh_controller.h.

◆ map_ptr

boost::shared_ptr<mesh_map::MeshMap> mesh_controller::MeshController::map_ptr

private

shared pointer to the used mesh map

Definition at line 159 of file mesh_controller.h.

◆ reconfigure_server_ptr

boost::shared_ptr<dynamic_reconfigure::Server<mesh_controller::MeshControllerConfig> > mesh_controller::MeshController::reconfigure_server_ptr

private

shared pointer to dynamic reconfigure server

Definition at line 177 of file mesh_controller.h.

◆ robot_dir

mesh_map::Normal mesh_controller::MeshController::robot_dir

private

Definition at line 168 of file mesh_controller.h.

◆ robot_pos

mesh_map::Vector mesh_controller::MeshController::robot_pos

private

Definition at line 165 of file mesh_controller.h.

◆ vector_map

lvr2::DenseVertexMap<mesh_map::Vector> mesh_controller::MeshController::vector_map

private

The vector field to the goal.

Definition at line 174 of file mesh_controller.h.

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

Public Types

Public Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Ptr

Constructor & Destructor Documentation

◆ MeshController()

◆ ~MeshController()

Member Function Documentation

◆ cancel()

◆ computeVelocityCommands()

◆ gaussValue()

◆ initialize()

◆ isGoalReached()

◆ naiveControl()

◆ poseToDirectionVector()

◆ poseToPositionVector()

◆ reconfigureCallback()

◆ setPlan()

Member Data Documentation

◆ angle_pub

◆ cancel_requested

◆ config

◆ config_callback

◆ current_face

◆ current_plan

◆ goal_dir

◆ goal_pos

◆ map_ptr

◆ reconfigure_server_ptr

◆ robot_dir

◆ robot_pos

◆ vector_map