void motion_planner_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
planner_t::init(reader, template_reader);
if( has_attribute("serialize_file", reader, template_reader) )
{
PRX_DEBUG_S("Serialization variable set");
serialization_file = get_attribute_as<std::string > ("serialize_file", reader, template_reader);
serialize_flag = true;
}
else
{
PRX_DEBUG_S("Serialization set to false");
serialize_flag = false;
}
if( has_attribute("deserialize_file", reader, template_reader) )
{
PRX_DEBUG_S("Deerialization variable set");
deserialization_file = get_attribute_as<std::string > ("deserialize_file", reader, template_reader);
deserialize_flag = true;
}
else
{
PRX_DEBUG_S("Deserialization set to false");
deserialize_flag = false;
}
}
void simple_controller_t::remove_system(const std::string& path)
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
PRX_DEBUG_S("--- REMOVE system : name : " << name << " subpath : " << subpath);
if( subsystems.find(name) != subsystems.end() )
if( subpath.empty() )
{
PRX_ERROR_S("Trying to remove system from a simple controller. Throwing exception...");
throw simple_add_remove_exception();
}
else
{
try
{
subsystems[name]->remove_system(subpath);
}
catch( removal_exception e )
{
subsystems.erase(name);
}
}
else
throw invalid_path_exception("The system " + name + " does not exist in the path " + pathname);
construct_spaces();
verify();
}
void simple_controller_t::add_system(const std::string& path, system_ptr_t system)
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
PRX_DEBUG_S("+++ ADD system : name : " << name << " subpath: " << subpath);
if( name.empty() )
throw invalid_path_exception("Name is empty");
else if( subpath.empty() )
{
PRX_ERROR_S("Trying to add system to a simple controller. Throwing exception...");
throw simple_add_remove_exception();
}
else if( !subpath.empty() )
{
if( subsystems.find(name) != subsystems.end() )
subsystems[name]->add_system(subpath, system);
}
else
throw invalid_path_exception(path);
construct_spaces();
verify();
}
void disk_t::init(const parameter_reader_t * reader, const parameter_reader_t* template_reader)
{
PRX_DEBUG_S("@@@Init disk_t : " << pathname);
integration_plant_t::init(reader, template_reader);
_z = parameters::get_attribute_as<double>("z", reader, template_reader, 1.5);
max_steps = parameters::get_attribute_as<double>("max_steps", reader, template_reader, PRX_INFINITY);
}
void sliding_rigid_body_t::append_contingency(plan_t& result_plan, double duration)
{
double difference = duration - result_plan.length();
// PRX_DEBUG_S("Difference in append: "<<difference);
// PRX_ASSERT(difference >= 0);
PRX_DEBUG_S("Result plan size is: " << result_plan.size());
if( result_plan.size() == 0 )
{
state_t* temp_state = state_space->alloc_point();
result_plan.copy_onto_back(temp_state, 0.0);
state_space->free_point(temp_state);
result_plan.back().duration += duration;
}
else
{
PRX_DEBUG_S("Difference is: " << difference);
PRX_ASSERT(difference >= 0);
result_plan.back().duration += difference;
}
}
osg::ref_ptr<osg::Node> osg_scene_t::make_geometry(const std::string& material_name, const geometry_t* geom, osg_material_t* material, bool transparent, bool use_color)
{
// The material settings from XML
// PRX_DEBUG_S("Looking for material: " << material_name.c_str() << " for the type : " << geom->get_type());
PRX_ASSERT(template_material_mapping.find(material_name) != template_material_mapping.end());
material = new osg_material_t(*(template_material_mapping[material_name]));
PRX_ASSERT(geom != NULL);
// TODO: Eventually line_thickness could represent other attributes as well (i.e. include transparency and line)
osg::ref_ptr<osg::Node> node = osg_geode_t::setup_geometry(geom, transparent, line_thickness);
// Use diffuse/specular/shininess
if( !transparent && !use_color )
material->setColorMode(osg::Material::OFF);
osg::StateSet* stateSet = node->getOrCreateStateSet();
if( transparent )
{
stateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
// Enable depth test so that an opaque polygon will occlude a transparent one behind it.
stateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
stateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
// Conversely, disable writing to depth buffer so that
// a transparent polygon will allow polygons behind it to shine thru.
// OSG renders transparent polygons after opaque ones.
osg::Depth* depth = new osg::Depth;
depth->setWriteMask(false);
stateSet->setAttributeAndModes(depth, osg::StateAttribute::ON);
// Disable conflicting modes.
stateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
PRX_DEBUG_S("setting transparency");
material->setTransparency(osg::Material::FRONT_AND_BACK, 0.1f);
}
// Apply the material to the node.
if( !use_color )
stateSet->setAttribute(material, osg::StateAttribute::ON);
if( !use_color )
node->setStateSet(stateSet);
return node;
}
void replanning_application_t::process_query_callback(const prx_simulation::query_msg& msg)
{
process_plant_locations_callback(msg.plant_locations);
if (once)
num_queries++;
PRX_INFO_S ("Process query callback!\n Consumer:" << msg.consumer << "\n Goal region radius: " << msg.goal_region_radius);
// Construct the query
model->use_context(consumer_to_space_name_mapping[consumer_path]);
if (!once)
{
root_specifications[0]->link_spaces(state_space, control_space);
root_specifications[0]->setup(model);
task_planner->link_specification(root_specifications[0]);
state_space->set_from_vector(msg.start);
// Set the goal and the distance metric
radial_goal_region_t* new_goal = new radial_goal_region_t(state_space, goal_metric, msg.goal, msg.goal_region_radius);
PRX_DEBUG_S ("Set goal");
query->set_goal(new_goal);
query->link_spaces(state_space, control_space);
query->set_start_from_vector(msg.start);
PRX_DEBUG_S("Link query");
task_planner->link_query(query);
}
task_planner->setup();
task_planner->execute();
task_planner->resolve_query();
if(visualize)
{
task_planner->update_visualization();
((visualization_comm_t*)vis_comm)->send_geometries();
}
((planning_comm_t*)plan_comm)->publish_plan(consumer_path,query->plan );
once = true;
}
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.");
}
}
void osg_ghost_switch_t::switch_mode(scene_mode_t mode)
{
// Cycle to the next mode.
mode = (scene_mode_t)(mode % NUM_MODES);
if(this->mode != mode)
{
PRX_DEBUG_S("Switching to geometry mode " << mode);
osg::Node* root = scene->get_wrapped_root();
switch_visitor_t visitor(mode);
root->accept(static_cast<osg::NodeVisitor&>(visitor));
this->mode = mode;
}
}
void goal_criterion_t::link_goal(goal_t* new_goal)
{
PRX_DEBUG_S("Hey, the goal_criterion has a goal with type " << criterion_type);
linked_goal = new_goal;
}
void print()
{
PRX_DEBUG_S("control_index:" << control_index << " value:" << value << " action:" << action);
}
