plugin_demo_interface_namespace::PathPlanner Class Reference

Contain the Interface . More...

#include <path_planner.hpp>

Inheritance diagram for plugin_demo_interface_namespace::PathPlanner:

Public Member Functions
void	initialize (ros::NodeHandle roshandle)
	Initalize the global variables. More...
double	getYawFromQuat (geometry_msgs::Quaternion quat)
	convert a quaternion into yaw More...
void	tfFrameFromPoseStamped (geometry_msgs::PoseStamped *pose, tf::Transform *tf_tf)
	build a tf-frame for a given pose More...
int	setPoints (geometry_msgs::Pose2D *start, geometry_msgs::Pose2D *target)
	replace the current start and target poses and call getPath() More...
int	getPoints (geometry_msgs::Pose2D *start, geometry_msgs::Pose2D *target)
	returns the current start and target poses More...
virtual	~PathPlanner ()
	Intentionally left empty.

Protected Member Functions
	PathPlanner ()
	initialize init
virtual int	onInit (ros::NodeHandle roshandle)
	In this function the plugIn can do some initialisation depending on a ROS NodeHandle. More...
virtual int	getPath ()=0
	calculate current_path from start and target inside a plugIn. More...
void	setPose2d (geometry_msgs::PoseStamped *pose3d, double x, double y, double theta)
	convert a given 2D-Pose into 3D-Stamped-Pose. More...

Protected Attributes
bool	init
	true if initialized. More...
geometry_msgs::PoseStamped	start
	starting pose of the path
geometry_msgs::PoseStamped	target
	last pose of the path
nav_msgs::Path	current_path
	contain the path

Private Member Functions
void	getTargetCallback (const geometry_msgs::PoseStamped event)
	receives a new target pose and call getPath(). More...
void	sendPathCallback (const ros::TimerEvent &event)
	call sendPath() More...
void	getStarttCallback (const geometry_msgs::Pose2D event)
	receives a new start pose and call getPath(). More...
void	sendPath ()
	publish current_path and the start_tf and target_tf frames

Private Attributes
ros::Subscriber	getTargetSub
	listen for a new target pose
ros::Subscriber	getStartSub
	listen for a new start pose
ros::Publisher	pathPub
	send the path calculated by the plugIn
ros::Timer	sendPathTimer
	periodic calling of the path publisher
tf::Transform	start_tf
	the start tf-frame
tf::Transform	target_tf
	the target tf-frame
tf::TransformBroadcaster	tf_br
	send tf-transforms

Detailed Description

Contain the Interface .

Serve some mutual functions for all inherited plugIns.

Definition at line 23 of file path_planner.hpp.

Member Function Documentation

virtual int plugin_demo_interface_namespace::PathPlanner::getPath ( )

protectedpure virtual

calculate current_path from start and target inside a plugIn.

Returns

error code

Implemented in plugin_demo_plugins_namespace::Direct, and plugin_demo_plugins_namespace::DirectOrtho.

int plugin_demo_interface_namespace::PathPlanner::getPoints	(	geometry_msgs::Pose2D *	start,
		geometry_msgs::Pose2D *	target
	)

returns the current start and target poses

Parameters

start	current start pose
target	current target pose

Returns

error code

Definition at line 76 of file path_planner.cpp.

    {
        start->x = this->start.pose.position.x;
        start->y = this->start.pose.position.y;
        start->theta = getYawFromQuat(this->start.pose.orientation);

        target->x = this->target.pose.position.x;
        target->y = this->target.pose.position.y;
        target->theta = getYawFromQuat(this->target.pose.orientation);

        return 0;
    }

void plugin_demo_interface_namespace::PathPlanner::getStarttCallback ( const geometry_msgs::Pose2D  event )

private

receives a new start pose and call getPath().

Parameters

event

a pose message from the main part of this demo

Definition at line 107 of file path_planner.cpp.

    {
        setPose2d(&start, event.x, event.y, event.theta);

        tfFrameFromPoseStamped(&start, &start_tf);
        
        current_path.poses.clear();
        getPath();

        sendPath();
    }

void plugin_demo_interface_namespace::PathPlanner::getTargetCallback ( const geometry_msgs::PoseStamped  event )

private

receives a new target pose and call getPath().

Parameters

event

a pose message from rViz

Definition at line 119 of file path_planner.cpp.

    {
        target = event;

        tfFrameFromPoseStamped(&target, &target_tf);
        
        current_path.poses.clear();
        getPath();

        sendPath();
    }

double plugin_demo_interface_namespace::PathPlanner::getYawFromQuat

(

geometry_msgs::Quaternion

quat

)

convert a quaternion into yaw

Parameters

quat	a Quaternion

Returns

the yaw angle of the quaternion

Definition at line 89 of file path_planner.cpp.

    {
        //geometry_msgs::Quaternion quat = ;
        tf::Quaternion q(quat.x, quat.y, quat.z, quat.w);
        tf::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);

        return yaw;
    }

void plugin_demo_interface_namespace::PathPlanner::initialize

(

ros::NodeHandle

roshandle

)

Initalize the global variables.

Parameters

roshandle

a valid ROS NodeHandle

Definition at line 14 of file path_planner.cpp.

    {
        if (init) {
            ROS_WARN("[interface] plugin was already initialized. Nothing will be done.");
        } else {
            getTargetSub = roshandle.subscribe("set_target", 10, &PathPlanner::getTargetCallback, this);
            getStartSub = roshandle.subscribe("set_start", 10, &PathPlanner::getStarttCallback, this);
            pathPub = roshandle.advertise<nav_msgs::Path>("path", 10);
            sendPathTimer = roshandle.createTimer(ros::Duration(2.0), &PathPlanner::sendPathCallback, this);

            setPose2d(&start, 0, 0, 0);
            target = start;
            target.header.stamp = ros::Time::now();

            tfFrameFromPoseStamped(&start, &start_tf);
            tfFrameFromPoseStamped(&target, &target_tf);

            current_path.header.stamp = ros::Time::now();
            current_path.header.frame_id = "world";
            current_path.poses.push_back(start);
            current_path.poses.push_back(target);
            
            onInit(roshandle);
            init = true;

            sendPath();
        }
    }

int plugin_demo_interface_namespace::PathPlanner::onInit ( ros::NodeHandle  roshandle )

protectedvirtual

In this function the plugIn can do some initialisation depending on a ROS NodeHandle.

Parameters

roshandle

a valid ROS NodeHandle

Returns

error code

Reimplemented in plugin_demo_plugins_namespace::Direct, and plugin_demo_plugins_namespace::DirectOrtho.

Definition at line 43 of file path_planner.cpp.

    {
        ROS_INFO("[interface] plugIn has no onInit function");
    }

void plugin_demo_interface_namespace::PathPlanner::sendPathCallback ( const ros::TimerEvent &  event )

private

call sendPath()

Parameters

event

some time values

Definition at line 131 of file path_planner.cpp.

    {
        sendPath();
    }

int plugin_demo_interface_namespace::PathPlanner::setPoints	(	geometry_msgs::Pose2D *	start,
		geometry_msgs::Pose2D *	target
	)

replace the current start and target poses and call getPath()

Parameters

start	new start pose
target	new target pose

Returns

error code

Definition at line 60 of file path_planner.cpp.

    {
        setPose2d( &(this->start), start->x, start->y, start->theta);
        setPose2d( &(this->target), target->x, target->y, target->theta);

        tfFrameFromPoseStamped(&(this->start), &start_tf);
        tfFrameFromPoseStamped(&(this->target), &target_tf);

        current_path.poses.clear();
        getPath();

        sendPath();

        return 0;
    }

void plugin_demo_interface_namespace::PathPlanner::setPose2d ( geometry_msgs::PoseStamped *  pose3d, double  x, double  y, double  theta  )

protected

convert a given 2D-Pose into 3D-Stamped-Pose.

Parameters

pose3d	pointer to the target variable
x	the x-value of the 2D-Pose
y	the y-value of the 2D-Pose
theta	the theta-value of the 2D-Pose

Definition at line 48 of file path_planner.cpp.

    {
        pose3d->header.stamp = ros::Time::now();
        pose3d->pose.position.x = x;
        pose3d->pose.position.y = y;
        pose3d->pose.position.z = 0;
        pose3d->pose.orientation.x = 0;
        pose3d->pose.orientation.y = 0;
        pose3d->pose.orientation.z = sin(theta * 0.5);          
        pose3d->pose.orientation.w = cos(theta * 0.5);
    }

void plugin_demo_interface_namespace::PathPlanner::tfFrameFromPoseStamped	(	geometry_msgs::PoseStamped *	pose,
		tf::Transform *	tf_tf
	)

build a tf-frame for a given pose

Parameters

pose	the pose for the tf-frame
tf_tf	the resulting tf-frame

Definition at line 100 of file path_planner.cpp.

    {
        tf_tf->setOrigin( tf::Vector3(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z) );
        tf::Quaternion q(pose->pose.orientation.x, pose->pose.orientation.y, pose->pose.orientation.z, pose->pose.orientation.w);
        tf_tf->setRotation(q);
    }

Field Documentation

bool plugin_demo_interface_namespace::PathPlanner::init

protected

true if initialized.

Definition at line 54 of file path_planner.hpp.

---

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

• src/plugin_demo_interface/include/plugin_demo_interface/path_planner.hpp
• src/plugin_demo_interface/src/path_planner.cpp