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(); }