void heuristic_task_specification_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
specification_t::init(reader, template_reader);
PRX_INFO_S("Trying to create a mapping from: "<<reader->trace());
state_mapping = parameters::create_from_loader<mapping_function_t > ("prx_utilities", reader, "state_mapping", template_reader, "state_mapping");
PRX_INFO_S(state_mapping->mapping_name);
heuristic_specification = dynamic_cast<motion_planning_specification_t*>(parameters::initialize_from_loader<specification_t > ("prx_planning", reader, "heuristic_specification", template_reader, "heuristic_specification"));
real_specification = dynamic_cast<motion_planning_specification_t*>(parameters::initialize_from_loader<specification_t > ("prx_planning", reader, "real_specification", template_reader, "real_specification"));
}
foreach(geometry_info_t body, geoms)
{
// PRX_INFO_S("I am in");
osg::ref_ptr<osg::Group> parent = NULL;
osg::ref_ptr<osg::PositionAttitudeTransform> child = NULL;
if( info_geoms.find(body.full_name) != info_geoms.end() )
{
// PRX_LOG_DEBUG("Delete the geometry : %s", body.full_name.c_str());
// remove from the scene
parent = info_geoms[body.full_name]->getParent(0)->asGroup(); //asTransform()->asPositionAttitudeTransform();
parent->removeChild(info_geoms[body.full_name]);
if( geometry_material_mapping[body.full_name] != NULL )
delete geometry_material_mapping[body.full_name];
}
child = new osg::PositionAttitudeTransform();
child->setName(body.full_name);
PRX_INFO_S("Going to make the new geometry for the system : " << body.full_name << " and type : " << body.type);
// PRX_DEBUG_S(" With color "); body.geometry->get_color().print();
// geometry_t new_geometry(body.type,&body.params);
// PRX_DEBUG_S("From add geometry make geometry with material name " << body.material_name);
osg::ref_ptr<osg::Node> geode = make_geometry(body.material_name, body.geometry, geometry_material_mapping[body.full_name], false, body.uses_geom_color);
child->addChild(geode);
info_geoms[body.full_name] = child;
info_geoms_to_update.push_back(body.full_name);
if( parent )
parent->addChild(child);
else
root->addChild(child);
}
void irs_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
PRX_INFO_S("Initializing IRS motion planner ...");
prm_star_t::init(reader, template_reader);
stretch_factor = parameters::get_attribute_as<double>("stretch_factor", reader, template_reader);
// max_consecutive_failures = reader->get_attribute_as<unsigned int>("max_consecutive_failures", 100);
// delta = reader->get_attribute_as<double>("delta",100);
clock.reset();
}
void replanning_planner_t::resolve_query()
{
if (add_time_interrupt)
{
s_criteria->add_interruption_criterion(t_criterion);
}
s_criteria->reset();
planners[planner_names[0]]->resolve_query();
PRX_INFO_S(mp_query->plan.length()<<" "<<mp_query->path.size());
if(mp_query->plan.length() > this->planning_duration)
mp_query->plan.augment_plan(this->planning_duration);
int index = (int)((mp_query->plan.length() / simulation::simulation_step) + .1);
PRX_INFO_S(mp_query->plan.length()<<" "<<index);
if (mp_query->path.size() != 0)
{
if (index < mp_query->path.size() )
mp_query->plan.copy_end_state(mp_query->path[index]);
else
mp_query->plan.copy_end_state(mp_query->path[mp_query->path.size()-1]);
PRX_INFO_S ("End state is : " << mp_query->state_space->print_point(mp_query->plan.get_end_state(),3));
}
}
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 dprm_task_planner_t::resolve_query()
{
PRX_INFO_S("Resolve query in task planner");
motionPlanner->resolve_query();
}
