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