#include <ompl_visual_tools.h>

Inheritance diagram for ompl_visual_tools::OmplVisualTools:

Public Member Functions
void	convertPlannerData (const ob::PlannerDataPtr planner_data, og::PathGeometric &path)
	Convert PlannerData to PathGeometric. Assume ordering of verticies is order of path.
bool	convertRobotStatesToTipPoints (const ompl::base::PlannerDataPtr &graph, const std::vector< const robot_model::LinkModel * > &tips, std::vector< std::vector< geometry_msgs::Point > > &vertex_tip_points)
	Convet each vertex in a graph into a list of tip locations, as desired.
double	getCost (const geometry_msgs::Point &point)
	Helper function for converting a point to the correct cost.
double	getCostHeight (const geometry_msgs::Point &point)
	Use bilinear interpolation, if necessary, to find the cost of a point between whole numbers From http://supercomputingblog.com/graphics/coding-bilinear-interpolation/.
bool	getDisable3D ()
	Getter for disable 3d option in Rviz.
bool	interpolateLine (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, visualization_msgs::Marker *marker, const std_msgs::ColorRGBA color)
	Helper Function for Display Graph that makes the exploration lines follow the curvature of the map.
	OmplVisualTools (const std::string &base_link, const std::string &marker_topic=ompl_visual_tools::RVIZ_MARKER_TOPIC, robot_model::RobotModelConstPtr robot_model=robot_model::RobotModelConstPtr())
	Constructor.
bool	publishCostMap (PPMImage *image, bool static_id=true)
	Visualize Results.
bool	publishEdge (const ob::State stateA, const ob::State stateB, const rviz_visual_tools::colors color=rviz_visual_tools::BLUE, const rviz_visual_tools::scales scale=rviz_visual_tools::REGULAR)
bool	publishGraph (ob::PlannerDataPtr planner_data, const rviz_visual_tools::colors &color=rviz_visual_tools::BLUE, const double thickness=0.2, const std::string &ns="space_exploration")
	Display Explored Space.
bool	publishPath (const ob::PlannerDataPtr &planner_data, const rviz_visual_tools::colors color, const double thickness=0.4, const std::string &ns="result_path")
	Display result path from a solver, in the form of a planner_data where the list of states is also the order of the path.
bool	publishPath (const og::PathGeometric &path, const rviz_visual_tools::colors color, const double thickness=0.4, const std::string &ns="result_path")
	Display result path from a solver.
bool	publishRobotGraph (const ompl::base::PlannerDataPtr &graph, const std::vector< const robot_model::LinkModel * > &tips)
	Display resulting graph from a planner, in the form of a planner_data object This uses MoveIt's robot state for inverse kinematics.
bool	publishRobotPath (const ompl::base::PlannerDataPtr &path, robot_model::JointModelGroup joint_model_group, const std::vector< const robot_model::LinkModel > &tips, bool show_trajectory_animated)
	Display resulting path from a solver, in the form of a planner_data where the list of states is also the order of the path. This uses MoveIt's robot state for inverse kinematics.
bool	publishRobotState (const ompl::base::State *state)
	Convert an OMPL state to a MoveIt! robot state and publish it.
bool	publishSampleIDs (const og::PathGeometric &path, const rviz_visual_tools::colors color=rviz_visual_tools::RED, const rviz_visual_tools::scales scale=rviz_visual_tools::SMALL, const std::string &ns="sample_labels")
	Display labels on samples.
bool	publishSampleRegion (const ob::ScopedState<> &state_area, const double &distance)
	Visualize the sampling area in Rviz.
MOVEIT_DEPRECATED bool	publishSamples (const ob::PlannerDataPtr &planner_data, const rviz_visual_tools::colors color=rviz_visual_tools::RED, const rviz_visual_tools::scales scale=rviz_visual_tools::SMALL, const std::string &ns="sample_locations")
	Display Sample Points.
MOVEIT_DEPRECATED bool	publishSamples (const og::PathGeometric &path, const rviz_visual_tools::colors color=rviz_visual_tools::RED, const rviz_visual_tools::scales scale=rviz_visual_tools::SMALL, const std::string &ns="sample_locations")
bool	publishSpheres (const ob::PlannerDataPtr &planner_data, const rviz_visual_tools::colors color=rviz_visual_tools::RED, const rviz_visual_tools::scales scale=rviz_visual_tools::SMALL, const std::string &ns="planner_data_spheres")
	Publish a marker of a series of spheres to rviz.
bool	publishSpheres (const og::PathGeometric &path, const rviz_visual_tools::colors color=rviz_visual_tools::RED, double scale=0.1, const std::string &ns="path_spheres")
bool	publishSpheres (const og::PathGeometric &path, const rviz_visual_tools::colors color=rviz_visual_tools::RED, const rviz_visual_tools::scales scale=rviz_visual_tools::SMALL, const std::string &ns="path_spheres")
bool	publishSpheres (const og::PathGeometric &path, const rviz_visual_tools::colors color, const geometry_msgs::Vector3 &scale, const std::string &ns="path_spheres")
bool	publishStartGoalSpheres (ob::PlannerDataPtr planner_data, const std::string &ns)
	Display Start Goal.
bool	publishState (const ob::ScopedState<> state, const rviz_visual_tools::colors &color, const rviz_visual_tools::scales scale=rviz_visual_tools::REGULAR, const std::string &ns="state_sphere")
	Display the start and goal states on the image map.
bool	publishState (const ob::ScopedState<> state, const rviz_visual_tools::colors &color, double scale=0.1, const std::string &ns="state_sphere")
bool	publishState (const ob::ScopedState<> state, const rviz_visual_tools::colors &color, const geometry_msgs::Vector3 &scale, const std::string &ns="state_sphere")
bool	publishStates (std::vector< const ompl::base::State * > states)
	Display States.
bool	publishText (const geometry_msgs::Point &point, const std::string &text, const rviz_visual_tools::colors &color=rviz_visual_tools::BLACK, bool static_id=true)
	Publish text to rviz at a given location.
bool	publishText (const geometry_msgs::Pose &pose, const std::string &text, const rviz_visual_tools::colors &color=rviz_visual_tools::BLACK, bool static_id=true)
bool	publishTriangle (int x, int y, visualization_msgs::Marker marker, PPMImage image)
	Helper Function to display triangles.
void	setCostMap (intMatrixPtr cost)
	Optional cost map for 2D environments.
void	setDisable3D (bool disable_3d)
	Setter for disable 3d option in Rviz.
void	setSpaceInformation (ompl::base::SpaceInformationPtr si)
void	setStateSpace (ompl::base::StateSpacePtr space)
	Load the OMPL state space or space information pointer.
geometry_msgs::Point	stateToPointMsg (int vertex_id, ob::PlannerDataPtr planner_data)
	Helper Function: gets the x,y coordinates for a given vertex id.
geometry_msgs::Point	stateToPointMsg (const ob::State *state)
geometry_msgs::Point	stateToPointMsg (const ob::ScopedState<> state)
Static Public Member Functions
static int	natRound (double x)
	Nat_Rounding helper function to make readings from cost map more accurate.
Private Attributes
intMatrixPtr	cost_
bool	disable_3d_
ompl::base::SpaceInformationPtr	si_
geometry_msgs::Point	temp_point_

Detailed Description

Definition at line 98 of file ompl_visual_tools.h.

Constructor & Destructor Documentation

ompl_visual_tools::OmplVisualTools::OmplVisualTools	(	const std::string &	base_link,
		const std::string &	marker_topic = `ompl_visual_tools::RVIZ_MARKER_TOPIC`,
		robot_model::RobotModelConstPtr	robot_model = `robot_model::RobotModelConstPtr()`
	)

Constructor.

Definition at line 75 of file ompl_visual_tools.cpp.

Member Function Documentation

void ompl_visual_tools::OmplVisualTools::convertPlannerData	(	const ob::PlannerDataPtr	planner_data,
		og::PathGeometric &	path
	)

Convert PlannerData to PathGeometric. Assume ordering of verticies is order of path.

Parameters:

PlannerData
PathGeometric

Definition at line 551 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::convertRobotStatesToTipPoints	(	const ompl::base::PlannerDataPtr &	graph,
		const std::vector< const robot_model::LinkModel * > &	tips,
		std::vector< std::vector< geometry_msgs::Point > > &	vertex_tip_points
	)

Convet each vertex in a graph into a list of tip locations, as desired.

Parameters:

input	- description
input	- description

Returns:

Definition at line 935 of file ompl_visual_tools.cpp.

double ompl_visual_tools::OmplVisualTools::getCost ( const geometry_msgs::Point & point )

Helper function for converting a point to the correct cost.

Definition at line 112 of file ompl_visual_tools.cpp.

double ompl_visual_tools::OmplVisualTools::getCostHeight ( const geometry_msgs::Point & point )

Use bilinear interpolation, if necessary, to find the cost of a point between whole numbers From http://supercomputingblog.com/graphics/coding-bilinear-interpolation/.

Definition at line 123 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::getDisable3D ( )

Getter for disable 3d option in Rviz.

Definition at line 99 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::interpolateLine	(	const geometry_msgs::Point &	p1,
		const geometry_msgs::Point &	p2,
		visualization_msgs::Marker *	marker,
		const std_msgs::ColorRGBA	color
	)

Helper Function for Display Graph that makes the exploration lines follow the curvature of the map.

Definition at line 287 of file ompl_visual_tools.cpp.

int ompl_visual_tools::OmplVisualTools::natRound ( double x ) [static]

Nat_Rounding helper function to make readings from cost map more accurate.

Parameters:

double

Returns:: rounded down number

Definition at line 877 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishCostMap	(	PPMImage *	image,
		bool	static_id = `true`
	)

Visualize Results.

Definition at line 188 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishEdge	(	const ob::State *	stateA,
		const ob::State *	stateB,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::BLUE`,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::REGULAR`
	)

Parameters:

input	- description
input	- description

Returns:

Definition at line 450 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishGraph	(	ob::PlannerDataPtr	planner_data,
		const rviz_visual_tools::colors &	color = `rviz_visual_tools::BLUE`,
		const double	thickness = `0.2`,
		const std::string &	ns = `"space_exploration"`
	)

Display Explored Space.

Definition at line 386 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishPath	(	const ob::PlannerDataPtr &	planner_data,
		const rviz_visual_tools::colors	color,
		const double	thickness = `0.4`,
		const std::string &	ns = `"result_path"`
	)

Display result path from a solver, in the form of a planner_data where the list of states is also the order of the path.

Returns:: true on success

Definition at line 763 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishPath	(	const og::PathGeometric &	path,
		const rviz_visual_tools::colors	color,
		const double	thickness = `0.4`,
		const std::string &	ns = `"result_path"`
	)

Display result path from a solver.

Returns:: true on success

Definition at line 772 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishRobotGraph	(	const ompl::base::PlannerDataPtr &	graph,
		const std::vector< const robot_model::LinkModel * > &	tips
	)

Display resulting graph from a planner, in the form of a planner_data object This uses MoveIt's robot state for inverse kinematics.

Returns:: true on success

Definition at line 708 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishRobotPath	(	const ompl::base::PlannerDataPtr &	path,
		robot_model::JointModelGroup *	joint_model_group,
		const std::vector< const robot_model::LinkModel * > &	tips,
		bool	show_trajectory_animated
	)

Display resulting path from a solver, in the form of a planner_data where the list of states is also the order of the path. This uses MoveIt's robot state for inverse kinematics.

Returns:: true on success

Definition at line 631 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishRobotState ( const ompl::base::State * state )

Convert an OMPL state to a MoveIt! robot state and publish it.

Parameters:

OMPL	format of a robot state

Returns:: true on success

Definition at line 615 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSampleIDs	(	const og::PathGeometric &	path,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::RED`,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::SMALL`,
		const std::string &	ns = `"sample_labels"`
	)

Display labels on samples.

Definition at line 458 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSampleRegion	(	const ob::ScopedState<> &	state_area,
		const double &	distance
	)

Visualize the sampling area in Rviz.

Parameters:

state_area	- the center point of the uniform sampler
distance	- the radius around the center for sampling

Definition at line 897 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSamples	(	const ob::PlannerDataPtr &	planner_data,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::RED`,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::SMALL`,
		const std::string &	ns = `"sample_locations"`
	)

Display Sample Points.

Definition at line 487 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSamples	(	const og::PathGeometric &	path,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::RED`,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::SMALL`,
		const std::string &	ns = `"sample_locations"`
	)

Definition at line 497 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSpheres	(	const ob::PlannerDataPtr &	planner_data,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::RED`,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::SMALL`,
		const std::string &	ns = `"planner_data_spheres"`
	)

Publish a marker of a series of spheres to rviz.

Parameters:

spheres	- where to publish them
color	- an enum pre-defined name of a color
scale	- an enum pre-defined name of a size
ns	- namespace of marker

Returns:: true on success

Definition at line 513 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSpheres	(	const og::PathGeometric &	path,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::RED`,
		double	scale = `0.1`,
		const std::string &	ns = `"path_spheres"`
	)

Definition at line 522 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSpheres	(	const og::PathGeometric &	path,
		const rviz_visual_tools::colors	color = `rviz_visual_tools::RED`,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::SMALL`,
		const std::string &	ns = `"path_spheres"`
	)

Definition at line 532 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishSpheres	(	const og::PathGeometric &	path,
		const rviz_visual_tools::colors	color,
		const geometry_msgs::Vector3 &	scale,
		const std::string &	ns = `"path_spheres"`
	)

Definition at line 538 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishStartGoalSpheres	(	ob::PlannerDataPtr	planner_data,
		const std::string &	ns
	)

Display Start Goal.

Definition at line 372 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishState	(	const ob::ScopedState<>	state,
		const rviz_visual_tools::colors &	color,
		const rviz_visual_tools::scales	scale = `rviz_visual_tools::REGULAR`,
		const std::string &	ns = `"state_sphere"`
	)

Display the start and goal states on the image map.

Parameters:

start	state
color

Definition at line 882 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishState	(	const ob::ScopedState<>	state,
		const rviz_visual_tools::colors &	color,
		double	scale = `0.1`,
		const std::string &	ns = `"state_sphere"`
	)

Definition at line 887 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishState	(	const ob::ScopedState<>	state,
		const rviz_visual_tools::colors &	color,
		const geometry_msgs::Vector3 &	scale,
		const std::string &	ns = `"state_sphere"`
	)

Definition at line 892 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishStates ( std::vector< const ompl::base::State * > states )

Display States.

Returns:: true on success

Definition at line 560 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishText	(	const geometry_msgs::Point &	point,
		const std::string &	text,
		const rviz_visual_tools::colors &	color = `rviz_visual_tools::BLACK`,
		bool	static_id = `true`
	)

Publish text to rviz at a given location.

Definition at line 908 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishText	(	const geometry_msgs::Pose &	pose,
		const std::string &	text,
		const rviz_visual_tools::colors &	color = `rviz_visual_tools::BLACK`,
		bool	static_id = `true`
	)

Definition at line 917 of file ompl_visual_tools.cpp.

bool ompl_visual_tools::OmplVisualTools::publishTriangle	(	int	x,
		int	y,
		visualization_msgs::Marker *	marker,
		PPMImage *	image
	)

Helper Function to display triangles.

Definition at line 264 of file ompl_visual_tools.cpp.

void ompl_visual_tools::OmplVisualTools::setCostMap ( intMatrixPtr cost )

Optional cost map for 2D environments.

Definition at line 91 of file ompl_visual_tools.cpp.

void ompl_visual_tools::OmplVisualTools::setDisable3D ( bool disable_3d )

Setter for disable 3d option in Rviz.

Definition at line 107 of file ompl_visual_tools.cpp.

void ompl_visual_tools::OmplVisualTools::setSpaceInformation ( ompl::base::SpaceInformationPtr si )

Definition at line 86 of file ompl_visual_tools.cpp.

void ompl_visual_tools::OmplVisualTools::setStateSpace ( ompl::base::StateSpacePtr space )

Load the OMPL state space or space information pointer.

Definition at line 81 of file ompl_visual_tools.cpp.

geometry_msgs::Point ompl_visual_tools::OmplVisualTools::stateToPointMsg	(	int	vertex_id,
		ob::PlannerDataPtr	planner_data
	)

Helper Function: gets the x,y coordinates for a given vertex id.

Parameters:

id	of a vertex
result	from an OMPL planner

Returns:: geometry point msg with no z value filled in

Definition at line 841 of file ompl_visual_tools.cpp.

geometry_msgs::Point ompl_visual_tools::OmplVisualTools::stateToPointMsg ( const ob::State * state )

Definition at line 849 of file ompl_visual_tools.cpp.

geometry_msgs::Point ompl_visual_tools::OmplVisualTools::stateToPointMsg ( const ob::ScopedState<> state )

Definition at line 868 of file ompl_visual_tools.cpp.

Member Data Documentation

intMatrixPtr ompl_visual_tools::OmplVisualTools::cost_ [private]

Definition at line 103 of file ompl_visual_tools.h.

bool ompl_visual_tools::OmplVisualTools::disable_3d_ [private]

Definition at line 112 of file ompl_visual_tools.h.

ompl::base::SpaceInformationPtr ompl_visual_tools::OmplVisualTools::si_ [private]

Definition at line 106 of file ompl_visual_tools.h.

geometry_msgs::Point ompl_visual_tools::OmplVisualTools::temp_point_ [private]

Definition at line 109 of file ompl_visual_tools.h.

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

Public Member Functions

Static Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation