3 #include <vigir_footstep_planning_lib/math.h> 5 #include <vigir_footstep_planning_plugins/plugin_aggregators/robot_model.h> 6 #include <vigir_footstep_planning_plugins/plugin_aggregators/world_model.h> 13 : StateGeneratorPlugin(
"reachability_state_generator")
19 if (!StateGeneratorPlugin::loadParams(params))
25 unsigned int jobs_per_thread;
26 result &= params.getParam(
"threads", threads);
27 result &= params.getParam(
"jobs_per_thread", jobs_per_thread);
32 result &= params.getParam(
"max_hash_size", hash_table_size);
33 result &= params.getParam(
"collision_check/cell_size", cell_size);
34 result &= params.getParam(
"collision_check/num_angle_bins", num_angle_bins);
35 double angle_bin_size = 2.0*M_PI /
static_cast<double>(num_angle_bins);
37 int ivMaxStepRangeX, ivMaxStepRangeY, ivMaxStepRangeTheta;
38 int ivMaxInvStepRangeX, ivMaxInvStepRangeY, ivMaxInvStepRangeTheta;
41 std::vector<std::pair<int, int> > step_range;
46 if (params.getParam(
"step_range/x", step_range_x) && params.getParam(
"step_range/y", step_range_y))
50 step_range.reserve(step_range_x.
size());
51 double max_x = (double)step_range_x[0];
52 double max_y = (double)step_range_y[0];
53 double max_inv_x = (double)step_range_x[0];
54 double max_inv_y = (double)step_range_y[0];
55 for (
int i=0; i < step_range_x.
size(); ++i)
57 double x = (double)step_range_x[i];
58 double y = (double)step_range_y[i];
60 max_x = std::max(max_x, x);
61 max_y = std::max(max_y, y);
62 max_inv_x = std::min(max_inv_x, x);
63 max_inv_y = std::min(max_inv_y, y);
65 step_range.push_back(std::pair<int, int>(disc_val(x, cell_size), disc_val(y, cell_size)));
68 double max_step_range_theta;
69 double max_inverse_step_range_theta;
70 result &= params.getParam(
"foot/max/step/theta", max_step_range_theta);
71 result &= params.getParam(
"foot/max/inverse/step/theta", max_inverse_step_range_theta);
73 ivMaxStepRangeX = disc_val(max_x, cell_size);
74 ivMaxStepRangeY = disc_val(max_y, cell_size);
75 ivMaxStepRangeTheta = angle_state_2_cell(max_step_range_theta, angle_bin_size);
76 ivMaxInvStepRangeX = disc_val(max_inv_x, cell_size);
77 ivMaxInvStepRangeY = disc_val(max_inv_y, cell_size);
78 ivMaxInvStepRangeTheta = angle_state_2_cell(max_inverse_step_range_theta, angle_bin_size);
87 expand_states_manager.reset(
new threading::ThreadingManager<threading::ExpandStateJob>(threads, jobs_per_thread));
91 for (
int y = ivMaxInvStepRangeY; y <= ivMaxStepRangeY; y++)
93 for (
int x = ivMaxInvStepRangeX; x <= ivMaxStepRangeX; x++)
95 bool in_step_range = pointWithinPolygon(x, y, step_range);
101 for (
int theta = ivMaxInvStepRangeTheta; theta <= ivMaxStepRangeTheta; theta++)
103 Footstep
f(cont_val(x, cell_size), cont_val(y, cell_size), angle_cell_2_state(theta, angle_bin_size), 0.0, cell_size, num_angle_bins, hash_table_size);
114 std::list<PlanningState::Ptr> result;
116 ROS_ERROR(
"[ReachabilityStateGenerator] generatePredecessor not implemented yet!");
123 std::list<PlanningState::Ptr> result;
126 std::list<threading::ExpandStateJob::Ptr> jobs;
133 for (std::list<threading::ExpandStateJob::Ptr>::iterator itr = jobs.begin(); itr != jobs.end(); itr++)
137 result.push_back(job->next);
144 #include <pluginlib/class_list_macros.h>