#include "tfd_modules/module_api/pddlModuleTypes.h"
#include "planner_modules_pr2/module_param_cache.h"
#include <map>
#include <string>
#include <utility>
#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
Go to the source code of this file.
Functions | |
double | callPlanningService (nav_msgs::GetPlan &srv, const string &startLocationName, const string &goalLocationName, bool &callFailure) |
Return the cost of the plan. | |
string | computePathCacheKey (const string &startLocation, const string &goalLocation, const geometry_msgs::Pose &startPose, const geometry_msgs::Pose &goalPose) |
bool | fillPathRequest (const modules::ParameterList ¶meterList, modules::numericalFluentCallbackType numericalFluentCallback, nav_msgs::GetPlan::Request &request) |
double | getPlanCost (const std::vector< geometry_msgs::PoseStamped > &plan) |
void | navstack_init (int argc, char **argv) |
double | pathCost (const modules::ParameterList ¶meterList, modules::predicateCallbackType predicateCallback, modules::numericalFluentCallbackType numericalFluentCallback, int relaxed) |
Variables | |
static const bool | g_Debug = false |
ros::ServiceClient | g_GetPlan |
double | g_GoalTolerance |
ros::NodeHandle * | g_NodeHandle |
ModuleParamCacheDouble | g_PathCostCache |
double | g_RotSpeed |
double | g_TransSpeed |
speed values for computing the estimated time as cost of a path | |
std::string | g_WorldFrame |
Plan requests are issued using this frame - so the poses from the planner are given in this frame (e.g. map) |
double callPlanningService | ( | nav_msgs::GetPlan & | srv, |
const string & | startLocationName, | ||
const string & | goalLocationName, | ||
bool & | callFailure | ||
) |
Return the cost of the plan.
[out] | callFailure | is set to true, if there was some failure during the call, i.e. the resulting INFINITE_COST does not represent necessarily the impossibility of a path. |
Definition at line 222 of file navstack_module.cpp.
string computePathCacheKey | ( | const string & | startLocation, |
const string & | goalLocation, | ||
const geometry_msgs::Pose & | startPose, | ||
const geometry_msgs::Pose & | goalPose | ||
) |
Definition at line 40 of file navstack_module.cpp.
bool fillPathRequest | ( | const modules::ParameterList & | parameterList, |
modules::numericalFluentCallbackType | numericalFluentCallback, | ||
nav_msgs::GetPlan::Request & | request | ||
) |
Definition at line 145 of file navstack_module.cpp.
double getPlanCost | ( | const std::vector< geometry_msgs::PoseStamped > & | plan | ) |
Definition at line 199 of file navstack_module.cpp.
void navstack_init | ( | int | argc, |
char ** | argv | ||
) |
Definition at line 56 of file navstack_module.cpp.
double pathCost | ( | const modules::ParameterList & | parameterList, |
modules::predicateCallbackType | predicateCallback, | ||
modules::numericalFluentCallbackType | numericalFluentCallback, | ||
int | relaxed | ||
) |
Definition at line 285 of file navstack_module.cpp.
Simple module implementation for ROS navigation stack.
Directly queries the move_base_node/make_plan service for each cost request by the planner.
Definition at line 19 of file navstack_module.h.
Definition at line 26 of file navstack_module.cpp.
double g_GoalTolerance |
Definition at line 31 of file navstack_module.cpp.
Definition at line 25 of file navstack_module.cpp.
Definition at line 39 of file navstack_module.cpp.
double g_RotSpeed |
Definition at line 34 of file navstack_module.cpp.
double g_TransSpeed |
speed values for computing the estimated time as cost of a path
Definition at line 33 of file navstack_module.cpp.
std::string g_WorldFrame |
Plan requests are issued using this frame - so the poses from the planner are given in this frame (e.g. map)
Definition at line 29 of file navstack_module.cpp.