47 #include <boost/math/constants/constants.hpp> 50 static const std::string
LOGNAME =
"generate_state_database";
76 nh.
param<std::string>(
"state_space_parameterization",
"JointModel");
78 output_folder = nh.
param<std::string>(
"output_folder",
"constraint_approximations_database");
91 "Could not find valid constraint description in parameter '" 94 "Please upload correct correct constraint description or remap the parameter.");
119 scene->getCurrentStateNonConst().update();
137 "Successfully generated Joint Space Constraint Approximation Database for constraint:\n" 139 ROS_INFO_STREAM_NAMED(
LOGNAME,
"The database has been saved in your local folder '" << params.
output_folder <<
"'");
142 int main(
int argc,
char** argv)
bool isEmpty(const moveit_msgs::Constraints &constr)
#define ROS_INFO_NAMED(name,...)
std::string state_space_parameterization
void computeDB(const planning_scene::PlanningScenePtr &scene, struct GenerateStateDatabaseParameters ¶ms)
moveit_msgs::Constraints constraints
std::string resolveName(const std::string &name, bool remap=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const robot_model::RobotModelConstPtr & getRobotModel() const
std::string output_folder
#define ROS_INFO_STREAM_NAMED(name, args)
The MoveIt! interface to OMPL.
const planning_scene::PlanningScenePtr & getPlanningScene()
static const std::string ROBOT_DESCRIPTION
ompl_interface::ConstraintApproximationConstructionOptions construction_opts
#define ROS_FATAL_STREAM_NAMED(name, args)
static const std::string CONSTRAINT_PARAMETER
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
bool setFromHandle(ros::NodeHandle &nh)
unsigned int max_explicit_points
double explicit_points_resolution
bool constructConstraints(XmlRpc::XmlRpcValue ¶ms, moveit_msgs::Constraints &constraints)
static const std::string LOGNAME
unsigned int edges_per_sample
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
int main(int argc, char **argv)
ConstraintApproximationPtr approx
std::string planning_group