#include <ompl_rviz_viewer.h>
Public Member Functions | |
void | addPoint (int x, int y, visualization_msgs::Marker *marker, PPMImage *image, bnu::matrix< int > &cost) |
std::vector< std::pair< double, double > > | convertSolutionToVector (og::PathGeometric &path) |
After running the algorithm, this converts the results to a vector of coordinates. | |
void | displayGraph (bnu::matrix< int > &cost, ob::PlannerDataPtr planner_data) |
void | displayResult (og::PathGeometric &path, std_msgs::ColorRGBA *color, bnu::matrix< int > &cost) |
void | displaySamples (bnu::matrix< int > &cost, ob::PlannerDataPtr planner_data) |
void | displayTriangles (PPMImage *image, bnu::matrix< int > &cost) |
Visualize Results. | |
std::pair< double, double > | getCoordinates (int vertex_id, ob::PlannerDataPtr planner_data) |
void | interpolateLine (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, visualization_msgs::Marker *marker, std_msgs::ColorRGBA &color, bnu::matrix< int > &cost) |
OmplRvizViewer (bool verbose) | |
Constructor. | |
void | publishSphere (const geometry_msgs::Point &point, const std_msgs::ColorRGBA &color) |
~OmplRvizViewer () | |
Destructor. | |
Private Attributes | |
ros::Publisher | marker_pub_ |
ros::NodeHandle | nh_ |
bool | verbose_ |
Definition at line 137 of file ompl_rviz_viewer.h.
ompl_rviz_viewer::OmplRvizViewer::OmplRvizViewer | ( | bool | verbose | ) | [inline] |
Constructor.
verbose | - run in debug mode |
Definition at line 156 of file ompl_rviz_viewer.h.
ompl_rviz_viewer::OmplRvizViewer::~OmplRvizViewer | ( | ) | [inline] |
Destructor.
Definition at line 169 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::addPoint | ( | int | x, |
int | y, | ||
visualization_msgs::Marker * | marker, | ||
PPMImage * | image, | ||
bnu::matrix< int > & | cost | ||
) | [inline] |
Definition at line 274 of file ompl_rviz_viewer.h.
std::vector<std::pair<double, double> > ompl_rviz_viewer::OmplRvizViewer::convertSolutionToVector | ( | og::PathGeometric & | path | ) | [inline] |
After running the algorithm, this converts the results to a vector of coordinates.
Definition at line 177 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::displayGraph | ( | bnu::matrix< int > & | cost, |
ob::PlannerDataPtr | planner_data | ||
) | [inline] |
Definition at line 392 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::displayResult | ( | og::PathGeometric & | path, |
std_msgs::ColorRGBA * | color, | ||
bnu::matrix< int > & | cost | ||
) | [inline] |
Definition at line 599 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::displaySamples | ( | bnu::matrix< int > & | cost, |
ob::PlannerDataPtr | planner_data | ||
) | [inline] |
Definition at line 480 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::displayTriangles | ( | PPMImage * | image, |
bnu::matrix< int > & | cost | ||
) | [inline] |
Visualize Results.
Definition at line 209 of file ompl_rviz_viewer.h.
std::pair<double, double> ompl_rviz_viewer::OmplRvizViewer::getCoordinates | ( | int | vertex_id, |
ob::PlannerDataPtr | planner_data | ||
) | [inline] |
Definition at line 686 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::interpolateLine | ( | const geometry_msgs::Point & | p1, |
const geometry_msgs::Point & | p2, | ||
visualization_msgs::Marker * | marker, | ||
std_msgs::ColorRGBA & | color, | ||
bnu::matrix< int > & | cost | ||
) | [inline] |
Definition at line 295 of file ompl_rviz_viewer.h.
void ompl_rviz_viewer::OmplRvizViewer::publishSphere | ( | const geometry_msgs::Point & | point, |
const std_msgs::ColorRGBA & | color | ||
) | [inline] |
Definition at line 549 of file ompl_rviz_viewer.h.
Definition at line 148 of file ompl_rviz_viewer.h.
Definition at line 142 of file ompl_rviz_viewer.h.
bool ompl_rviz_viewer::OmplRvizViewer::verbose_ [private] |
Definition at line 145 of file ompl_rviz_viewer.h.