parameter_reader_t::parameter_reader_t(const std::string& path) : path(path)
{
if(global_reader==NULL)
{
XmlRpc::XmlRpcValue global_root;
if( !ros::param::get("", global_root) )
{
PRX_FATAL_S("Can't create global parameter_reader_t");
}
global_reader = new parameter_reader_t(global_root,"");
}
char last = this->path[this->path.length() - 1];
if( last == '/' )
{
this->path = this->path.substr(0,this->path.length()-1);
}
last = this->path[0];
if( last == '/' )
{
this->path = this->path.substr(1,this->path.length()-1);
}
std::auto_ptr<const parameter_reader_t> subreader = global_reader->get_child(this->path);
root = XmlRpc::XmlRpcValue(subreader->root);
// if( !ros::param::get(path, root) )
// PRX_FATAL_S("Can't create root parameter_reader_t at " << path);
if( root.getType() != XmlRpc::XmlRpcValue::TypeStruct )
PRX_FATAL_S("Subreader constructed from nondictionary at " << path);
}
void motion_planner_t::link_specification(specification_t* new_spec)
{
input_specification = (motion_planning_specification_t*)new_spec;
state_space = input_specification->state_space;
control_space = input_specification->control_space;
if( input_specification->local_planner != NULL )
local_planner = input_specification->local_planner;
else
PRX_FATAL_S("No local planner has been specified in motion planner " << path);
if( input_specification->validity_checker != NULL )
validity_checker = input_specification->validity_checker;
else
PRX_FATAL_S("No validity checker has been specified in motion planner " << path);
if( input_specification->sampler != NULL )
sampler = input_specification->sampler;
else
PRX_FATAL_S("No sampler has been specified in motion planner " << path);
if( input_specification->metric != NULL )
metric = input_specification->metric;
else
PRX_FATAL_S("No distance metric has been specified in motion planner " << path);
}
std::vector<const parameter_reader_t*> parameter_reader_t::get_subreaders(const std::string& key) const
{
// if( !has_key(key) )
// PRX_WARN_S("No element with name " << path << " " << key);
std::vector<const parameter_reader_t*> sub_readers;
XmlRpc::XmlRpcValue sub_trees;
try
{
if( key.empty() )
sub_trees = root;
else
sub_trees = root[key];
}
catch( XmlRpc::XmlRpcException e )
{
PRX_FATAL_S("Exception (" << e.getMessage() << ")"
<< " reading ROS parameter server list: " << path << key);
}
if( sub_trees.getType() != XmlRpc::XmlRpcValue::TypeArray )
PRX_FATAL_S("Element with name \"" << path << "\"" << key << "\" is not a list.");
sub_readers.reserve(sub_trees.size());
for( int i = 0; i < sub_trees.size(); ++i )
{
XmlRpc::XmlRpcValue& sub_tree = sub_trees[i];
sub_readers.push_back(new parameter_reader_t(sub_tree, path + key + '/'));
}
return sub_readers;
}
planner_context_info_t(std::string planning_context_name, plan::planner_t* planner, plan::query_t* query)
{
this->planning_context_name = planning_context_name;
this->planner = dynamic_cast<plan::motion_planner_t*>(planner);
planner_name = planner->get_name();
if(this->planner == NULL)
PRX_FATAL_S("Manipulation task planner can only work with motion planners!");
this->query = dynamic_cast<plan::motion_planning_query_t*>(query);
if(this->query == NULL)
PRX_FATAL_S("Manipulation task planner van have only motion planning queries as output queries.");
}
void mapping_function_t::verify() const
{
if(domain!=0 && domain != preimage_interval.second-preimage_interval.first-1)
{
PRX_FATAL_S("The domain of a mapping doesn't match the interval");
}
if(range!=0 && range != image_interval.second-image_interval.first-1)
{
PRX_FATAL_S("The range of a mapping doesn't match the interval");
}
}
std::auto_ptr<const parameter_reader_t> parameter_reader_t::get_child(const std::string& path) const
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
if( path.empty() )
{
PRX_FATAL_S("Calling get child with empty path");
}
else if( subpath.empty() ) // No slash found
{
return std::auto_ptr<const parameter_reader_t > (get_subreader(path));
}
else
{
if( !has_element(name) )
PRX_WARN_S("Can't follow path \"" << path << "\" from " << trace());
const parameter_reader_t* subreader = get_subreader(name);
std::auto_ptr<const parameter_reader_t> child_reader = subreader->get_child(subpath);
delete subreader;
return child_reader;
}
}
const std::string parameter_reader_t::get_value(const std::string& key) const
{
std::string value;
try
{
if( root.getType() != XmlRpc::XmlRpcValue::TypeStruct )
{
if( key.empty() )
value = get_xmlrpc_as_string(root);
else
PRX_WARN_S("Expected dictionary in " << trace() << " for key " << key);
}
else
{
if( has_key(key) )
value = get_xmlrpc_as_string(root[key]);
else
PRX_WARN_S("Expected key " << key << " in " << trace());
}
}
catch( XmlRpc::XmlRpcException e )
{
PRX_FATAL_S("Exception (" << e.getMessage() << ")"
<< " reading ROS parameter server value: "
<< path << '/' << key);
}
return value;
}
void radial_goal_region_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
PRX_DEBUG_COLOR("Inside radial goal region init!", PRX_TEXT_BLUE);
goal_state_t::init(reader, template_reader);
if( parameters::has_attribute("radius", reader, template_reader) )
{
PRX_DEBUG_S("Reading in goal radius");
radius = parameters::get_attribute_as< double >("radius", reader, template_reader);
PRX_DEBUG_COLOR("Radius is: " << radius, PRX_TEXT_CYAN);
}
else
{
PRX_FATAL_S("Missing radius attribute in input files for the radial_goal_region.");
}
}
std::map<const std::string, const parameter_reader_t*> parameter_reader_t::get_subreaders_map() const
{
std::map<const std::string, const parameter_reader_t*> sub_readers_map;
if( root.getType() != XmlRpc::XmlRpcValue::TypeStruct )
PRX_FATAL_S("Element with name " << path << " is not a map.");
const MyXmlRpcValue& my_rpc_value = static_cast<const MyXmlRpcValue&>(root);
const XmlRpc::XmlRpcValue::ValueStruct& value_map = my_rpc_value.get_map();
typedef XmlRpc::XmlRpcValue::ValueStruct::value_type key_value_t;
foreach(key_value_t key_value, value_map)
{
const std::string& key = key_value.first;
const XmlRpc::XmlRpcValue& value = key_value.second;
sub_readers_map[key] = new parameter_reader_t(value, path + key + '/');
}
return sub_readers_map;
}
const parameter_reader_t* parameter_reader_t::get_subreader(const std::string& key) const
{
// if( !has_key(key) )
// PRX_WARN_S("No element with name \"" << path <<", "<< key << '"');
XmlRpc::XmlRpcValue sub_reader;
try
{
sub_reader = root[key];
}
catch( XmlRpc::XmlRpcException e )
{
PRX_FATAL_S("Exception (" << e.getMessage() << ")"
<< " reading ROS parameter server list: " << path <<" "<< key);
}
if(key[key.length()-1]!='/')
return new parameter_reader_t(sub_reader, path + key + '/');
return new parameter_reader_t(sub_reader, path + key);
}
void replanning_application_t::init(const parameter_reader_t* reader)
{
planning_application_t::init(reader);
model->use_context(consumer_to_space_name_mapping[consumer_path]);
state_space = model->get_state_space();
control_space = model->get_control_space();
planning_duration = reader->get_attribute_as<double>("planning_duration");
//TODO: Query just needs the state and control space, I believe...
query->state_space = state_space;
query->control_space = control_space;
task_planner = this->root_task;
//task_planner = dynamic_cast<task_planner_t*>(this->root_task);
if (task_planner == NULL || query == NULL)
{
PRX_FATAL_S ("Something did not initialize correctly in the replanning application init: query or task planner is NULL.");
}
}
void pno_criterion_t::link_motion_planner(motion_planner_t* mp)
{
PRX_DEBUG_COLOR("Linking the motion planner...", PRX_TEXT_CYAN );
criterion_t::link_motion_planner( mp );
//I think we need to store a pointer explicitly to a prm* planner
prm_motion_planner = dynamic_cast<prm_star_t*>( mp );
if( prm_motion_planner == NULL )
PRX_FATAL_S("PNO Criterion requires the linked motion planner be a PRM*");
linked_space = mp->state_space;
total_volume = linked_space->space_size();
d = linked_space->get_dimension();
PRX_PRINT("Working in a " << d << " dimensional space, of size: " << total_volume, PRX_TEXT_LIGHTGRAY );
free_volume = total_volume;
//compute_limit();
//Open up an appropriate file for output...
char* w = std::getenv("PRACSYS_PATH");
std::string fname(w);
fname += ("/prx_output/pno_data/");
//fname += "dim_" + int_to_str( d ) + "_" + int_to_str( time(NULL) ) + ".txt";
fname += "dim_" + int_to_str( d ) + "_" + ros::this_node::getName().substr(1,ros::this_node::getName().length()) + ".txt";
stat_out.open( fname.c_str() );
}
void consumer_controller_t::read_in_goals(const parameter_reader_t * reader, const parameter_reader_t* template_reader)
{
std::vector<const parameter_reader_t*> goal_readers, template_goal_readers;
if( reader->has_attribute("goals") )
goal_readers = reader->get_list("goals");
else
{
PRX_FATAL_S("No goals to read!");
}
std::string template_name;
parameter_reader_t* child_template_reader = NULL;
foreach(const parameter_reader_t* r, goal_readers)
{
PRX_DEBUG_COLOR("Goal reading!", PRX_TEXT_CYAN);
radial_goal_region_t* goal_region = new radial_goal_region_t();
if( r->has_attribute("goal/template") )
{
template_name = r->get_attribute("goal/template");
// TODO: Find a way to load templates under namespaces more cleanly.
child_template_reader = new parameter_reader_t(ros::this_node::getName() + "/" + template_name);
}
goal_region->init(r->get_child("goal").get(), child_template_reader);
goal_region->link_space(child_state_space);
goal_states.push_back(goal_region);
if( child_template_reader == NULL )
{
delete child_template_reader;
child_template_reader = NULL;
}
}
bool apc_sampler_t::sample_near_object(state_t* result_point, const state_t* target_point)
{
PRX_FATAL_S("sample_near object for apc sampler is not implemented YET!");
return false;
}
bool stopping_criteria_t::satisfied()
{
bool ret, check;
satisfied_criterion.clear();
// First check the stopping criteria for satisfaction
switch (criteria_check_type)
{
case ALL_CRITERIA:
ret = 1;
for (unsigned i = 0; i < stopping_criteria.size(); i++)
{
check = stopping_criteria[i]->criterion_check();
// We stored any satisfied criterion
if (check)
satisfied_criterion.push_back(stopping_criteria[i]);
ret *= check;
}
break;
case SINGLE_CRITERION:
ret = 0;
for (unsigned i = 0; i < stopping_criteria.size(); i++)
{
check = stopping_criteria[i]->criterion_check();
// We stored any satisfied criterion
if (check)
{
satisfied_criterion.push_back(stopping_criteria[i]);
}
ret += check;
}
break;
default:
PRX_FATAL_S ("Criteria check type "<<criteria_check_type<<" not supported in stopping criterion");
break;
}
if (stopping_criteria.empty())
ret = 0;
// If the stopping criterion is satisfied, based on its type, an exception is thrown
if (ret)
throw stopping_criteria_t::stopping_criteria_satisfied(" Stopping criteria is satisfied ");
// The first interruption criterion that is satisfied will throw the exception
for (unsigned i = 0; i < interruption_criteria.size(); i++)
{
if (interruption_criteria[i]->criterion_check())
{
// Since an interruption has been met, clear the vector
interruption_criteria.clear();
// Throw an interruption
throw stopping_criteria_t::interruption_criteria_satisfied(" Interruption criteria is satisfied");
}
}
return false;
}
void apc_sampler_t::sample_near(const space_t* space, space_point_t* near_point, std::vector<bounds_t*>& bounds, space_point_t* point)
{
PRX_FATAL_S("sample_near for manipulation sampler is not implemented!");
}
void geometry_t::set_params( const std::vector<double>* params )
{
switch (type)
{
case PRX_SPHERE:
PRX_ASSERT(params->size() == 1);
set_sphere((*params)[0]);
break;
case PRX_BOX:
PRX_ASSERT(params->size() == 3);
set_box((*params)[0], (*params)[1], (*params)[2]);
break;
case PRX_CONE:
PRX_ASSERT(params->size() == 2);
set_cone((*params)[0], (*params)[1]);
break;
case PRX_CYLINDER:
PRX_ASSERT(params->size() == 2);
set_cylinder((*params)[0], (*params)[1]);
break;
case PRX_OPEN_CYLINDER:
PRX_ASSERT(params->size() == 2);
set_open_cylinder((*params)[0], (*params)[1]);
break;
case PRX_CAPSULE:
PRX_ASSERT(params->size() == 2);
set_capsule((*params)[0], (*params)[1]);
break;
case PRX_LINES:
// Must have at least two points.
PRX_ASSERT(params->size() >= 6);
// Must have three params per point.
PRX_ASSERT(params->size() % 3 == 0);
// Must have two points per line.
PRX_ASSERT(params->size() % 6 == 0);
this->type = type;
info = *params; // Copy the info directly
break;
case PRX_LINESTRIP:
// Must have at least two points.
PRX_ASSERT(params->size() >= 6);
// Must have three params per point.
PRX_ASSERT(params->size() % 3 == 0);
this->type = type;
info = *params; // Copy the info directly
break;
case PRX_CLOUD:
// Must have three params per point.
PRX_ASSERT(params->size() % 3 == 0);
info = *params; // Copy the info directly
break;
default:
PRX_FATAL_S("Invalid geometry type: " << type);
break;
}
}
void motion_planner_t::set_param(const std::string& path, const std::string& parameter_name, const boost::any& value)
{
if( !path.empty() )
PRX_FATAL_S("A path leading to motion planner is invalid. Passed path is " << path << " while this motion planner has path " << this->path);
this->set_param(parameter_name, value);
}
void geometry_t::init(const parameter_reader_t* const reader)
{
const std::string geom_type = reader->get_attribute("type");
// PRX_DEBUG_COLOR("MAKING GEOMETRY " << geom_type.c_str(), PRX_TEXT_CYAN);
if (geom_type == "box")
{
const vector_t dims = reader->get_attribute_as<vector_t>("dims");
if (dims.get_dim() != 3)
PRX_FATAL_S("Box must have three-dimensional dims attribute.")
set_box(dims[0], dims[1], dims[2]);
}
else if (geom_type == "sphere")
set_sphere(reader->get_attribute_as<double>("radius"));
else if (geom_type == "cone")
set_cone(reader->get_attribute_as<double>("radius"),
reader->get_attribute_as<double>("height"));
else if (geom_type == "cylinder")
set_cylinder(reader->get_attribute_as<double>("radius"),
reader->get_attribute_as<double>("height"));
else if (geom_type == "open_cylinder")
set_open_cylinder(reader->get_attribute_as<double>("radius"),
reader->get_attribute_as<double>("height"));
else if (geom_type == "capsule")
set_capsule(reader->get_attribute_as<double>("radius"),
reader->get_attribute_as<double>("height"));
else if (geom_type == "mesh")
set_mesh(reader->get_attribute_as<std::string>("filename"));
else if (geom_type == "point_cloud")
set_point_cloud();
else if (geom_type == "heightmap")
set_heightmap(reader->get_attribute_as<std::string>("filename"));
else if (geom_type == "polygon")
{
const std::vector< double > triangles = reader->get_attribute_as< std::vector<double> >("triangles");
set_polygon( triangles );
}
else
{
const std::string trace = reader->trace();
PRX_FATAL_S("Unrecognized geometry type (" << geom_type.c_str() << ") in " << trace.c_str());
}
if ( reader->has_attribute("scale"))
{
scale = reader->get_attribute_as<vector_t>("scale");
if (scale.get_dim() != 3)
PRX_FATAL_S("Scale must have 3 elements at " << reader->trace() );
}
else
set_scale(1.0,1.0,1.0);
if( reader->has_attribute("material" ) )
{
std::string color_string = reader->get_attribute("material");
if( color_string == "yellow" )
{ color[0] = 1; color[1] = 1; color[2] = 0; color[3] = 1.0; }
else if( color_string == "blue" )
{ color[0] = 0; color[1] = 0; color[2] = 1; color[3] = 1.0; }
else if( color_string == "dark_grey" )
{ color[0] = 0.25; color[1] = 0.25; color[2] = 0.25; color[3] = 1.0; }
else if( color_string == "black" )
{ color[0] = 0; color[1] = 0; color[2] = 0; color[3] = 1.0; }
else if( color_string == "green" )
{ color[0] = 0; color[1] = 1; color[2] = 0; color[3] = 1.0; }
else if( color_string == "red" )
{ color[0] = 1; color[1] = 0; color[2] = 0; color[3] = 1.0; }
else if( color_string == "silver" )
{ color[0] = 0.75; color[1] = 0.75; color[2] = 0.75; color[3] = 1.0; }
else if( color_string == "cyan" )
{ color[0] = 0.7; color[1] = 1; color[2] = 1; color[3] = 1.0; }
else if( color_string == "orange" )
{ color[0] = 1; color[1] = 0.6; color[2] = 0.05; color[3] = 1.0; }
else if( color_string == "brown" )
{ color[0] = 0.4; color[1] = 0.25; color[2] = 0.0; color[3] = 1.0;}
else if( color_string == "glass" )
{ color[0] = 0.5; color[1] = 0.5; color[2] = 0.55; color[3] = 0.18;}
else
{ color[0] = 1; color[1] = 1; color[2] = 1; color[3] = 1.0; }
}
else
{ color[0] = 1; color[1] = 1; color[2] = 1; color[3] = 1.0; }
}
