コード例 #1
0
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;
}
コード例 #2
0
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();
	}
}
コード例 #3
0
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;
}
コード例 #4
0
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;
}