#include <move_base_topo.h>
Public Types | |
typedef actionlib::SimpleActionClient < move_base_msgs::MoveBaseAction > | MoveBaseClient |
Public Member Functions | |
void | executeCB (const topological_nav_msgs::MoveBaseTopoGoal::ConstPtr &goal) |
void | localizationCB (const geometry_msgs::PoseStamped &l) |
MoveBaseTopo () | |
geometry_msgs::PoseStamped | nodePoseOnGrid (unsigned n, unsigned g) const |
void | roadmapCB (topological_nav_msgs::TopologicalRoadmap::ConstPtr roadmap) |
double | shortestPathDistance (const geometry_msgs::Pose &p1) |
Private Member Functions | |
unsigned | bestGrid (const WaypointVec &path) const |
void | checkEdgeTimeouts (const ros::TimerEvent &e) |
void | cleanup () |
nav_msgs::OccupancyGrid::ConstPtr | getGrid (const unsigned g) const |
void | graphCB (topological_nav_msgs::TopologicalGraph::ConstPtr m) |
void | gridUpdateCB (const warehouse::UpdateNotification &m) |
unsigned | lastWaypointOnGridIndex (const WaypointVec &plan, const unsigned g) const |
unsigned | lastWaypointOnGridIndex (const Path &plan, const std::string &global_frame) |
bool | makeRoadmapPlan (const topological_nav_msgs::MoveBaseTopoGoal::ConstPtr &goal, boost::optional< Path > &plan, std::string &error_string) |
void | removeBlockedEdges (const std::list< BlockedEdge > &blocked_edges, Roadmap &roadmap) |
bool | robotWithinTolerance (double tolerance_dist, unsigned node_id) |
double | squareDist (const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2) |
Private Attributes | |
actionlib::SimpleActionServer < topological_nav_msgs::MoveBaseTopoAction > | as_ |
double | blocked_edge_timeout_ |
std::list< BlockedEdge > | blocked_edges_ |
costmap_2d::Costmap2DROS | costmap_ |
boost::optional< unsigned > | current_grid_ |
warehouse::WarehouseClient | db_ |
ros::Timer | edge_watchdog_ |
bool | grid_switch_ |
ros::Subscriber | grid_update_sub_ |
warehouse::Collection < nav_msgs::OccupancyGrid > | grids_ |
ros::Time | last_valid_plan_ |
ros::Subscriber | localization_sub_ |
ros::ServiceClient | make_plan_client_ |
MoveBaseClient | move_base_client_ |
boost::mutex | mutex_ |
double | next_waypoint_distance_ |
ros::Publisher | path_pub_ |
navfn::NavfnROS | planner_ |
double | planner_patience_ |
double | progress_check_frequency_ |
Roadmap | roadmap_ |
ros::Subscriber | roadmap_sub_ |
ros::ServiceClient | switch_grid_client_ |
tf::TransformListener | tf_ |
std::string | tf_prefix_ |
topological_map_2d::TopologicalMap | tmap_ |
ros::Subscriber | tmap_sub_ |
Definition at line 79 of file move_base_topo.h.
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> topological_roadmap::MoveBaseTopo::MoveBaseClient |
Definition at line 89 of file move_base_topo.h.
topological_roadmap::MoveBaseTopo::MoveBaseTopo | ( | ) |
Definition at line 61 of file move_base_topo.cpp.
unsigned topological_roadmap::MoveBaseTopo::bestGrid | ( | const WaypointVec & | path | ) | const [private] |
void topological_roadmap::MoveBaseTopo::checkEdgeTimeouts | ( | const ros::TimerEvent & | e | ) | [private] |
Definition at line 254 of file move_base_topo.cpp.
void topological_roadmap::MoveBaseTopo::cleanup | ( | ) | [private] |
Definition at line 109 of file move_base_topo.cpp.
void topological_roadmap::MoveBaseTopo::executeCB | ( | const topological_nav_msgs::MoveBaseTopoGoal::ConstPtr & | goal | ) |
nm::OccupancyGrid::ConstPtr topological_roadmap::MoveBaseTopo::getGrid | ( | const unsigned | g | ) | const [private] |
Definition at line 572 of file move_base_topo.cpp.
void topological_roadmap::MoveBaseTopo::graphCB | ( | topological_nav_msgs::TopologicalGraph::ConstPtr | m | ) | [private] |
void topological_roadmap::MoveBaseTopo::gridUpdateCB | ( | const warehouse::UpdateNotification & | m | ) | [private] |
unsigned topological_roadmap::MoveBaseTopo::lastWaypointOnGridIndex | ( | const WaypointVec & | plan, | |
const unsigned | g | |||
) | const [private] |
unsigned topological_roadmap::MoveBaseTopo::lastWaypointOnGridIndex | ( | const Path & | plan, | |
const std::string & | global_frame | |||
) | [private] |
void topological_roadmap::MoveBaseTopo::localizationCB | ( | const geometry_msgs::PoseStamped & | l | ) |
bool topological_roadmap::MoveBaseTopo::makeRoadmapPlan | ( | const topological_nav_msgs::MoveBaseTopoGoal::ConstPtr & | goal, | |
boost::optional< Path > & | plan, | |||
std::string & | error_string | |||
) | [private] |
gm::PoseStamped topological_roadmap::MoveBaseTopo::nodePoseOnGrid | ( | unsigned | n, | |
unsigned | g | |||
) | const |
Definition at line 117 of file move_base_topo.cpp.
void topological_roadmap::MoveBaseTopo::removeBlockedEdges | ( | const std::list< BlockedEdge > & | blocked_edges, | |
Roadmap & | roadmap | |||
) | [private] |
void topological_roadmap::MoveBaseTopo::roadmapCB | ( | topological_nav_msgs::TopologicalRoadmap::ConstPtr | roadmap | ) |
bool topological_roadmap::MoveBaseTopo::robotWithinTolerance | ( | double | tolerance_dist, | |
unsigned | node_id | |||
) | [private] |
Definition at line 126 of file move_base_topo.cpp.
double topological_roadmap::MoveBaseTopo::shortestPathDistance | ( | const geometry_msgs::Pose & | p1 | ) |
double topological_roadmap::MoveBaseTopo::squareDist | ( | const geometry_msgs::PoseStamped & | p1, | |
const geometry_msgs::PoseStamped & | p2 | |||
) | [inline, private] |
Definition at line 105 of file move_base_topo.h.
actionlib::SimpleActionServer<topological_nav_msgs::MoveBaseTopoAction> topological_roadmap::MoveBaseTopo::as_ [private] |
Definition at line 119 of file move_base_topo.h.
double topological_roadmap::MoveBaseTopo::blocked_edge_timeout_ [private] |
Definition at line 131 of file move_base_topo.h.
std::list<BlockedEdge> topological_roadmap::MoveBaseTopo::blocked_edges_ [private] |
Definition at line 129 of file move_base_topo.h.
costmap_2d::Costmap2DROS topological_roadmap::MoveBaseTopo::costmap_ [private] |
Definition at line 121 of file move_base_topo.h.
boost::optional<unsigned> topological_roadmap::MoveBaseTopo::current_grid_ [private] |
Definition at line 120 of file move_base_topo.h.
warehouse::WarehouseClient topological_roadmap::MoveBaseTopo::db_ [private] |
Definition at line 111 of file move_base_topo.h.
ros::Timer topological_roadmap::MoveBaseTopo::edge_watchdog_ [private] |
Definition at line 130 of file move_base_topo.h.
bool topological_roadmap::MoveBaseTopo::grid_switch_ [private] |
Definition at line 125 of file move_base_topo.h.
ros::Subscriber topological_roadmap::MoveBaseTopo::grid_update_sub_ [private] |
Definition at line 113 of file move_base_topo.h.
warehouse::Collection<nav_msgs::OccupancyGrid> topological_roadmap::MoveBaseTopo::grids_ [private] |
Definition at line 112 of file move_base_topo.h.
ros::Time topological_roadmap::MoveBaseTopo::last_valid_plan_ [private] |
Definition at line 127 of file move_base_topo.h.
ros::Subscriber topological_roadmap::MoveBaseTopo::localization_sub_ [private] |
Definition at line 113 of file move_base_topo.h.
ros::ServiceClient topological_roadmap::MoveBaseTopo::make_plan_client_ [private] |
Definition at line 132 of file move_base_topo.h.
Definition at line 123 of file move_base_topo.h.
boost::mutex topological_roadmap::MoveBaseTopo::mutex_ [mutable, private] |
Definition at line 116 of file move_base_topo.h.
double topological_roadmap::MoveBaseTopo::next_waypoint_distance_ [private] |
Definition at line 124 of file move_base_topo.h.
ros::Publisher topological_roadmap::MoveBaseTopo::path_pub_ [private] |
Definition at line 114 of file move_base_topo.h.
navfn::NavfnROS topological_roadmap::MoveBaseTopo::planner_ [mutable, private] |
Definition at line 122 of file move_base_topo.h.
double topological_roadmap::MoveBaseTopo::planner_patience_ [private] |
Definition at line 128 of file move_base_topo.h.
double topological_roadmap::MoveBaseTopo::progress_check_frequency_ [private] |
Definition at line 124 of file move_base_topo.h.
Definition at line 117 of file move_base_topo.h.
ros::Subscriber topological_roadmap::MoveBaseTopo::roadmap_sub_ [private] |
Definition at line 113 of file move_base_topo.h.
ros::ServiceClient topological_roadmap::MoveBaseTopo::switch_grid_client_ [private] |
Definition at line 115 of file move_base_topo.h.
tf::TransformListener topological_roadmap::MoveBaseTopo::tf_ [private] |
Definition at line 110 of file move_base_topo.h.
std::string topological_roadmap::MoveBaseTopo::tf_prefix_ [private] |
Definition at line 126 of file move_base_topo.h.
topological_map_2d::TopologicalMap topological_roadmap::MoveBaseTopo::tmap_ [private] |
Definition at line 118 of file move_base_topo.h.
ros::Subscriber topological_roadmap::MoveBaseTopo::tmap_sub_ [private] |
Definition at line 113 of file move_base_topo.h.