static void
build_points_vector_handler()
{
static int firstime = 1;
if (firstime)
{
points_vector[0].x = globalpos.globalpos.x;
points_vector[0].y = globalpos.globalpos.y;
points_vector[0].theta = globalpos.globalpos.theta;
points_vector_size = 1;
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(10), 3.0, &car_config);
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(-10), 3.0, &car_config);
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(0), 3.0, &car_config);
printf("enviando %d, pose %f %f %f\n", points_vector_size, globalpos.globalpos.x, globalpos.globalpos.y, globalpos.globalpos.theta);
carmen_motion_planner_publish_path_message(points_vector, points_vector_size, 0);
// publish_navigator_ackerman_plan_message(points_vector, points_vector_size);
// send_base_ackerman_command(points_vector, points_vector_size);
carmen_ipc_sleep(0.5);
firstime = 0;
}
}
void
behavior_selector_update_rddf(carmen_rddf_road_profile_message *rddf_msg)
{
copy_rddf_message(rddf_msg);
if ((get_current_algorithm() == CARMEN_BEHAVIOR_SELECTOR_RDDF) && (last_rddf_message) && (last_rddf_message->number_of_poses > 0))
carmen_motion_planner_publish_path_message(last_rddf_message->poses, last_rddf_message->number_of_poses, CARMEN_BEHAVIOR_SELECTOR_RDDF);
if ((!robot_initialized) || (current_goal_source == CARMEN_BEHAVIOR_SELECTOR_USER_GOAL))
return;
if (((goal_list_size - goal_list_index) > 1) &&
(rddf_msg->annotations[rddf_msg->number_of_poses - 1] != RDDF_ANNOTATION_END_POINT_AREA || (rddf_msg->annotations[rddf_msg->number_of_poses - 1] == RDDF_ANNOTATION_END_POINT_AREA &&
carmen_distance_ackerman_traj(&goal_list[goal_list_size - 1], &rddf_msg->poses[rddf_msg->number_of_poses - 1]) < 0.1)))
return;
goal_list_index = 0;
goal_list_size = 0;
fill_goal_list(rddf_msg, robot_pose);
change_goal();
}
static void
localize_globalpos_handler(carmen_localize_ackerman_globalpos_message *msg)
{
// if (current_algorithm != CARMEN_BEHAVIOR_SELECTOR_A_STAR)
// return;
IPC_RETURN_TYPE err = IPC_OK;
static int roundValue = 2;
static carmen_ackerman_traj_point_t robot_position;
robot_position.x = round(msg->globalpos.x * roundValue) / roundValue;
robot_position.y = round(msg->globalpos.y * roundValue) / roundValue;
robot_position.theta = round(msg->globalpos.theta * roundValue) / roundValue;
robot_position.v = msg->v;
robot_position.phi = msg->phi;
if (messageControl.carmen_planner_ackerman_update_robot(&robot_position, &robot_config) == 0)
return;
//publica caminho
carmen_planner_status_t status;
messageControl.carmen_planner_ackerman_get_status(&status);
//Mensagem que faz andar
if (status.path.length > 0)
{
carmen_motion_planner_publish_path_message(status.path.points, status.path.length, current_algorithm);
IPC_RETURN_TYPE err;
static int firsttime = 1;
static carmen_navigator_ackerman_astar_goal_list_message goal_list_msg;
if (firsttime)
{
err = IPC_defineMsg(CARMEN_ASTAR_GOAL_LIST_NAME, IPC_VARIABLE_LENGTH, CARMEN_ASTAR_GOAL_LIST_FMT);
carmen_test_ipc_exit(err, "Could not define message", CARMEN_ASTAR_GOAL_LIST_NAME);
goal_list_msg.host = carmen_get_host();
firsttime = 0;
}
goal_list_msg.goal_list = status.path.points;
goal_list_msg.size = status.path.length;
goal_list_msg.timestamp = carmen_get_time();
err = IPC_publishData(CARMEN_ASTAR_GOAL_LIST_NAME, &goal_list_msg);//todo valgrind encontra problema de valor nao inicializado aqui
carmen_test_ipc(err, "Could not publish", CARMEN_ASTAR_GOAL_LIST_NAME);
}
//Mensagem que exibe o caminho na tela
if (status.path.length > 0)
{
static carmen_navigator_ackerman_plan_tree_message plan_tree_msg;
static bool firstTime = true;
if (firstTime == true)
{
err = IPC_defineMsg(CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME, IPC_VARIABLE_LENGTH,
CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_FMT);
carmen_test_ipc_exit(err, "Could not define", CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME);
plan_tree_msg.host = carmen_get_host();
firstTime = false;
}
if (status.path.length > 100)
{ // Ver tipo carmen_navigator_ackerman_plan_tree_message
printf("Error: status.path.length > 100\n");
return;
}
plan_tree_msg.num_path = 1;
memcpy(plan_tree_msg.paths[0], status.path.points, sizeof(carmen_ackerman_traj_point_t) * status.path.length);
plan_tree_msg.path_size[0] = status.path.length;
err = IPC_publishData(CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME, &plan_tree_msg);
carmen_test_ipc(err, "Could not publish", CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME);
}
}
