int MessageControl::carmen_planner_ackerman_update_robot(carmen_ackerman_traj_point_p new_position, carmen_robot_ackerman_config_t *robot_conf) { static carmen_ackerman_traj_point_t lastPosition; static int first_time = 1; robot_conf_g = robot_conf; if (!carmen_planner_map) return false; if (!requested_goal) return false; if ((new_position->x - carmen_planner_map->config.x_origin) < 0 || (new_position->y - carmen_planner_map->config.y_origin) < 0 || (new_position->x - carmen_planner_map->config.x_origin) > carmen_planner_map->config.resolution * carmen_planner_map->config.x_size || (new_position->y - carmen_planner_map->config.y_origin) > carmen_planner_map->config.resolution*carmen_planner_map->config.y_size) return 0; robot = *new_position; if (!first_time && carmen_distance_ackerman_traj(new_position, &lastPosition) < carmen_planner_map->config.resolution) { return false; } lastPosition = *new_position; carmen_planner_ackerman_regenerate_trajectory(); return 1; }
void MessageControl::carmen_planner_ackerman_set_cost_map(carmen_map_t *new_map) { carmen_planner_map = new_map; carmen_conventional_set_costs(new_map); if (this->requested_goal) { plan(); carmen_planner_ackerman_regenerate_trajectory(); } }
int MessageControl::carmen_planner_ackerman_update_goal(carmen_ackerman_traj_point_t *goal) { if (!carmen_planner_map) return false; requested_goal = goal; printf("teste: %d\n", requested_goal->x); plan(); carmen_planner_ackerman_regenerate_trajectory(); return true; }
int MessageControl::carmen_planner_ackerman_setDistanceMap(carmen_obstacle_distance_mapper_message *newDistanceMap) { this->astarAckeman.distanceMap = newDistanceMap; //carmen_conventional_set_costs(new_map); if (this->requested_goal) { //plan(); carmen_planner_ackerman_regenerate_trajectory(); } return true; }