PathCreator
A path creator tool for teleoperated navigation.
path_creator::PathCreatorCore Class Reference

#include <PathCreatorCore.hpp>

Public Member Functions

 PathCreatorCore ()
 
 ~PathCreatorCore ()=default
 
void fixNodeOrientation (const int index, const bool fix)
 
void fixNodePose (const int index, const bool fix)
 
void fixNodePosition (const int index, const bool fix)
 
nav_msgs::Path getEdges () const
 
std_msgs::ColorRGBA getLocalGuidanceModeColor (const std::string mode) const
 
std::map< std::string, std_msgs::ColorRGBA > getLocalGuidanceModeColorMap () const
 
std::vector< std::string > getLocalGuidanceModes () const
 
const std::pair< std::string, double > getMeshInfo (const std::string localGuidanceMode) const
 
PathCreatorNode getNode (const int nodeId) const
 
bool getNodePassThrough (const int nodeId)
 
int getNumNodes () const
 
std::vector< std::string > getPathConfig (const std::string config) const
 
std::string getRobotFrameId () const
 
void setInitialMenuConfig (const menuHandler_t config)
 
void setLocalGuidanceMode (const std::string mode)
 
void setLocalGuidanceModeByIndex (const int index, const std::string mode)
 
void setNodeMenuConfig (const int index, const menuHandler_t menuConfig)
 
void setNodePassThrough (const int index, const bool passThrough)
 
void setNodePose (const int index, const geometry_msgs::Pose &pose)
 
void setRobotFrameId (const std::string frameId)
 
void addEdge (const std::string frameId, const geometry_msgs::Point position)
 
bool addInitialNodePair (const std::string frameId)
 
bool addNode (const std::string frameId, const geometry_msgs::Point &position, const bool updateConfig)
 
bool autoOrientNodes ()
 
std::string convertEdgeToString (PathCreatorNode &pathNode)
 
std::string convertNodeToString (PathCreatorNode &pathNode)
 
bool insertNumNewNodes (const int indexToAppendTo, const int numNewNodes)
 
void loadNodeConfig (const std::string &configVersion)
 
void moveMultipleNodes (const int movedNodeIndex, const geometry_msgs::Pose &newPose)
 
bool pathFromTextFile (std::string &filePath)
 
bool pathToTextFile (std::string &filePath)
 
void readParameters (const ros::NodeHandle &nh)
 
void removeAllNodes (const bool updateConfig)
 
bool removeNodeByIndex (const int nodeId)
 
bool resetNodeHeight ()
 
void storePathConfig ()
 
bool transformVector (const std::string sourceFrame, const std::string targetFrame, geometry_msgs::Point &sourceVec, geometry_msgs::Point &targetVec)
 
bool transformVector (const std::string sourceFrame, const std::string targetFrame, Eigen::Vector3d &sourceVec, geometry_msgs::Point &targetVec)
 
void updateNodeColor (const int index)
 

Private Attributes

tf::TransformListener tfListener_
 
int lastNodeIndex_
 
std::string robotFrameId_
 
std::vector< PathCreatorNodenodes_
 
nav_msgs::Path edges_
 
std::vector< std::string > colors_
 
std::unordered_map< std::string, std_msgs::ColorRGBA > colorDefinitions_
 
std::map< std::string, std_msgs::ColorRGBA > localGuidanceModeToColorMap_
 
std::vector< std::string > localGuidanceModes_
 
std::vector< std::string > lastPathConfiguration_
 
std::vector< std::string > previousPathConfiguration_
 
menuHandler_t initialMenuConfig_
 
std::map< std::string, std::pair< std::string, double > > localGuidanceModeCustomMeshes_
 

Constructor & Destructor Documentation

path_creator::PathCreatorCore::PathCreatorCore ( )
path_creator::PathCreatorCore::~PathCreatorCore ( )
default

Member Function Documentation

void path_creator::PathCreatorCore::addEdge ( const std::string  frameId,
const geometry_msgs::Point  position 
)

Adds points to the edges_ object.

Parameters
[in]frameIdThe frame of the point.
[in]positionThe position of the point.
bool path_creator::PathCreatorCore::addInitialNodePair ( const std::string  frameId)

Adds a node to the existing node vector.

Parameters
[in]frameIdThe frame id of the node.
bool path_creator::PathCreatorCore::addNode ( const std::string  frameId,
const geometry_msgs::Point &  position,
const bool  updateConfig 
)

Adds a node to the existing node vector.

Parameters
[in]frameIdThe frame id of the node.
[in]positionThe position of the node.
[in]updateConfigWhether or not the change should be registered and the last config updated.
bool path_creator::PathCreatorCore::autoOrientNodes ( )

Auto-orient nodes, to face towards the next node on the path.

Returns
True if the nodes' orientation was adapted, false otherwise.
std::string path_creator::PathCreatorCore::convertEdgeToString ( PathCreatorNode pathNode)

Converts the interactive marker info to a path edge string.

Parameters
[in]pathNodeThe node from which the info is read.
Returns
Pose graph edge information in a pose graph manager readable format.
std::string path_creator::PathCreatorCore::convertNodeToString ( PathCreatorNode pathNode)

Converts the interactive marker info to a path node string.

Parameters
[in]pathNodeThe node from which the info is read.
Returns
Pose graph node information in a pose graph manager readable format.
void path_creator::PathCreatorCore::fixNodeOrientation ( const int  index,
const bool  fix 
)

Fixate a node's orientation.

Parameters
[in]indexThe index of the node.
[in]fixWhether the orientation should be fixed or not.
void path_creator::PathCreatorCore::fixNodePose ( const int  index,
const bool  fix 
)

Fixate a node's pose (i.e., position and orientation).

Parameters
[in]indexThe index of the node.
[in]fixWhether the pose should be fixed or not.
void path_creator::PathCreatorCore::fixNodePosition ( const int  index,
const bool  fix 
)

Fixate a node's position.

Parameters
[in]indexThe index of the node.
[in]fixWhether the position should be fixed or not.
nav_msgs::Path path_creator::PathCreatorCore::getEdges ( ) const

Get the path's edges.

Returns
The path's edges.
std_msgs::ColorRGBA path_creator::PathCreatorCore::getLocalGuidanceModeColor ( const std::string  mode) const

Get the color associated to a Local Guidance Mode. Note: if the color is not specified, returns gray (i.e., rgba 0.7, 0.7, 0.7, 1.0) by default).

Parameters
[in]modeThe Local Guidance Mode, whose color is needed.
Returns
The color.
std::map<std::string, std_msgs::ColorRGBA> path_creator::PathCreatorCore::getLocalGuidanceModeColorMap ( ) const

Get a map, associating Local Guidance Modes (key) to colors (value).

Returns
The map.
std::vector<std::string> path_creator::PathCreatorCore::getLocalGuidanceModes ( ) const

Get the available Local Guidance Modes.

Returns
The Local Guidance Modes.
const std::pair<std::string, double> path_creator::PathCreatorCore::getMeshInfo ( const std::string  localGuidanceMode) const

Get the mesh file info of the associated Local Guidance Mode.

Parameters
[in]localGuidanceModeThe Local Guidance Mode, whose associated mesh info is desired.
PathCreatorNode path_creator::PathCreatorCore::getNode ( const int  nodeId) const

Get a node.

Parameters
[in]nodeIdThe Id of the desired node.
Returns
The node with Id nodeId.
bool path_creator::PathCreatorCore::getNodePassThrough ( const int  nodeId)

Get a node's pass-through value.

Parameters
nodeIdThe Id of the desired node.
Returns
Whether the node must be passed through or not.
int path_creator::PathCreatorCore::getNumNodes ( ) const

Get the amount of available nodes.

Returns
The amount of nodes.
std::vector<std::string> path_creator::PathCreatorCore::getPathConfig ( const std::string  config) const

Get the last path configuration.

Parameters
[in]configThe desired configuration: either 'last' or 'previous'.
Returns
The last path configuration.
std::string path_creator::PathCreatorCore::getRobotFrameId ( ) const

Get the robot frame id.

Returns
The frame id.
bool path_creator::PathCreatorCore::insertNumNewNodes ( const int  indexToAppendTo,
const int  numNewNodes 
)

Inserts a number of nodes in-between (or at the end) of the path.

Parameters
[in]indexToAppendToThe index of the node, to which to append new ones.
[in]numNewNodesThe amount of nodes to append.
void path_creator::PathCreatorCore::loadNodeConfig ( const std::string &  configVersion)

Load a previous path configuration.

Parameters
[in]configVersionLoads the 'previous' or 'last' path configuration.
void path_creator::PathCreatorCore::moveMultipleNodes ( const int  movedNodeIndex,
const geometry_msgs::Pose &  newPose 
)

Calculate marker poses, if multiple markers are moved at once.

Parameters
[in]movedNodeIndexThe index of the node actually being moved by the user.
[in]newPoseThe pose, where the user placed the node.
bool path_creator::PathCreatorCore::pathFromTextFile ( std::string &  filePath)

Load a path from a .txt file.

Parameters
[in]filePathThe (aboslute) file path including the filename (e.g., /home/user/path.txt).
bool path_creator::PathCreatorCore::pathToTextFile ( std::string &  filePath)

Export the current path to a .txt file.

Parameters
[in]filePathThe (aboslute) file path including the filename (e.g., /home/user/path.txt).
void path_creator::PathCreatorCore::readParameters ( const ros::NodeHandle &  nh)

Reads ROS parameters from the parameter server.

Parameters
[in]nhThe ROS NodeHandle.
void path_creator::PathCreatorCore::removeAllNodes ( const bool  updateConfig)

Removes all existing nodes and updates configuration depending on input.

Parameters
[in]updateConfigWhether or not the change should be registered and the last config updated.
bool path_creator::PathCreatorCore::removeNodeByIndex ( const int  nodeId)

Removes a single node of the path.

Parameters
[in]nodeIdThe Id of the node to be removed.
Returns
True, if the node was successfully removed, false otherwise.
bool path_creator::PathCreatorCore::resetNodeHeight ( )

Resets the height of all nodes to the default value.

Returns
True, if the height was reset, false otherwise.
void path_creator::PathCreatorCore::setInitialMenuConfig ( const menuHandler_t  config)

Set the initial menu configuration.

Parameters
[in]configThe initial menu configuration.
void path_creator::PathCreatorCore::setLocalGuidanceMode ( const std::string  mode)

Set all nodes' Local Guidance Mode.

Parameters
[in]modeThe Local Guidane Mode to be set.
void path_creator::PathCreatorCore::setLocalGuidanceModeByIndex ( const int  index,
const std::string  mode 
)

Set a single node's Local Guidance Mode.

Parameters
[in]indexThe Id of the node, whose Local Guidance Mode is set.
[in]modeThe Local Guidane Mode to be set.
void path_creator::PathCreatorCore::setNodeMenuConfig ( const int  index,
const menuHandler_t  menuConfig 
)

Set a node's menu configuration.

Parameters
[in]indexThe index of the node, whose menu configuration is set.
[in]menuConfigThe menu configuration to be set.
void path_creator::PathCreatorCore::setNodePassThrough ( const int  index,
const bool  passThrough 
)

Set a node's pass-through value.

Parameters
indexThe index of the node.
passThroughTrue, if the node must be passed through, false otherwise.
void path_creator::PathCreatorCore::setNodePose ( const int  index,
const geometry_msgs::Pose &  pose 
)

Set a node's pose.

Parameters
[in]indexThe index of the node.
[in]poseThe new pose.
void path_creator::PathCreatorCore::setRobotFrameId ( const std::string  frameId)

Set the robot frame id.

Parameters
[in]frameIdThe name of the robot frame.
void path_creator::PathCreatorCore::storePathConfig ( )

Stores the last node configuration in pose graph style.

bool path_creator::PathCreatorCore::transformVector ( const std::string  sourceFrame,
const std::string  targetFrame,
geometry_msgs::Point &  sourceVec,
geometry_msgs::Point &  targetVec 
)

Transforms a vector from source to target frame.

Parameters
[in]sourceFrameThe source frame.
[in]targetFrameThe target frame.
[in]sourceVecThe vector to be transformed.
[out]targetVecThe transformed vector.
Returns
True if the vector was successfully transformed, false otherwise.
bool path_creator::PathCreatorCore::transformVector ( const std::string  sourceFrame,
const std::string  targetFrame,
Eigen::Vector3d &  sourceVec,
geometry_msgs::Point &  targetVec 
)

Transforms a vector from source to target frame.

Parameters
[in]sourceFrameThe source frame.
[in]targetFrameThe target frame.
[in]sourceVecThe vector to be transformed.
[out]targetVecThe transformed vector.
Returns
True if the vector was successfully transformed, false otherwise.
void path_creator::PathCreatorCore::updateNodeColor ( const int  index)

Updates the node's color according to its Local Guidance Mode.

Parameters
[in]indexThe index of the node, whose color is updated.

Member Data Documentation

std::unordered_map<std::string, std_msgs::ColorRGBA> path_creator::PathCreatorCore::colorDefinitions_
private

A map to refer color names (key) to their respective definitions, i.e., RGBA (value).

std::vector<std::string> path_creator::PathCreatorCore::colors_
private
nav_msgs::Path path_creator::PathCreatorCore::edges_
private

The path's edges. NOTE: these are only stored for visualization purposes.

menuHandler_t path_creator::PathCreatorCore::initialMenuConfig_
private

The initial menu configuration.

int path_creator::PathCreatorCore::lastNodeIndex_
private

The index of the last node (i.e., node N of the path with length N).

std::vector<std::string> path_creator::PathCreatorCore::lastPathConfiguration_
private

The last path configuration known to the Path Creator.

std::map<std::string, std::pair<std::string, double> > path_creator::PathCreatorCore::localGuidanceModeCustomMeshes_
private

A map, associating Local Guidance Modes (key) to a custom mesh (value) Note: the mesh pair contains the file name (first) and the scaling (second) information.

std::vector<std::string> path_creator::PathCreatorCore::localGuidanceModes_
private

Available Local Guidance Modes.

std::map<std::string, std_msgs::ColorRGBA> path_creator::PathCreatorCore::localGuidanceModeToColorMap_
private

A map to refer Local Guidance Modes (key) to colors (value).

std::vector<PathCreatorNode> path_creator::PathCreatorCore::nodes_
private

The path's nodes.

std::vector<std::string> path_creator::PathCreatorCore::previousPathConfiguration_
private

The second last path configuration known to the Path Creator.

std::string path_creator::PathCreatorCore::robotFrameId_
private

The robot's frame id.

tf::TransformListener path_creator::PathCreatorCore::tfListener_
private

The tf transform listener.


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