int calculate_turn_score() { double score = 0; //pre_hit_table_state && table_state; if (table_state.white_ball.z < POOL_TABLE_HEIGHT) { score += WHITE_BALL_POTTED; std::cout << "[V-REP_ROS_POOL_SIM] White potted\n"; } for (unsigned int i = 0; i < BALL_COUNT; i++) { if ((table_state.balls[i].z < POOL_TABLE_HEIGHT) && (pre_hit_table_state.balls[i].z >= POOL_TABLE_HEIGHT)) { //if ball is potted score += RED_BALL_POTTED; std::cout << "[V-REP_ROS_POOL_SIM] RED potted\n"; } else if (!closeTo(table_state.balls[i], pre_hit_table_state.balls[i], RED_BALL_MOVEMENT)) { //ball has be hit and moved! score += RED_BALL_HIT; std::cout << "[V-REP_ROS_POOL_SIM] RED hit\n"; } } return ceil(score); }
XmlOutput& XmlOutput::operator<<(const xml_output& o) { switch(o.xo_type) { case tNothing: break; case tRaw: addRaw(o.xo_text); break; case tDeclaration: addDeclaration(o.xo_text, o.xo_value); break; case tTag: newTagOpen(o.xo_text); break; case tCloseTag: if (o.xo_value.count()) closeAll(); else if (o.xo_text.count()) closeTo(o.xo_text); else closeTag(); break; case tAttribute: addAttribute(o.xo_text, o.xo_value); break; case tData: { // Special case to be able to close tag in normal // way ("</tag>", not "/>") without using addRaw().. if (!o.xo_text.count()) { closeOpen(); break; } QString output = doConversion(o.xo_text); output.replace('\n', "\n" + currentIndent); addRaw(QString("\n%1%2").arg(currentIndent).arg(output)); } break; case tComment: { QString output("<!--%1-->"); addRaw(output.arg(o.xo_text)); } break; case tCDATA: { QString output("<![CDATA[\n%1\n]]>"); addRaw(output.arg(o.xo_text)); } break; } return *this; }
void XmlOutput::closeAll() { if (!tagStack.count()) return; closeTo(QString()); }
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages): VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData) { // This is called quite often. Just watch out for messages/events you want to handle // Keep following 5 lines at the beginning and unchanged: simLockInterface(1); int errorModeSaved; simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved); simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore); void* retVal=NULL; // Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here: if (message==sim_message_eventcallback_instancepass) { // This message is sent each time the scene was rendered (well, shortly after) (very often) // When a simulation is not running, but you still need to execute some commands, then put some code here } if (message==sim_message_eventcallback_mainscriptabouttobecalled) { // Main script is about to be run (only called while a simulation is running (and not paused!)) // // This is a good location to execute commands (e.g. commands needed to generate ROS messages) // // e.g. to read all joint positions: simLockInterface(1); ros::spinOnce(); sensor_msgs::Image image_msg; // // Execute player speceific functions // if ((player_turn_state == PLAYER_NONE) && new_player_turn) { //ROS_INFO("New player turn received: %f %f", player_turn.power, player_turn.angle); ROS_INFO("[PLAYER_NONE] Player turn ended.\n"); new_player_turn = false; } else if (player_turn_state == PLAYER_ROTATE_MOVE) { tip_position[0] = table_state.white_ball.x - TIP_OFFSET_DISTANCE * sin(player_turn.angle); tip_position[1] = table_state.white_ball.y + TIP_OFFSET_DISTANCE * cos(player_turn.angle); tip_orientation[2] = player_turn.angle; std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Orientation success: " << simSetObjectOrientation(tip_handle, WORLD_FRAME, tip_orientation) << std::endl; std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Position success: " << simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl; player_turn_state = PLAYER_CUE_DOWN; } else if (player_turn_state == PLAYER_CUE_DOWN) { tip_position[2] = CUE_ON_TABLE_HEIGHT; std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_CUE_DOWN] Position success: " << simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl; player_turn_state = PLAYER_EXECUTE_SHOT; } else if (player_turn_state == PLAYER_EXECUTE_SHOT) { double dist = player_turn.power * POWER_DISTANCE_RATIO + MIN_POWER_DISTANCE; tip_position[0] += sin(player_turn.angle) * dist; tip_position[1] -= cos(player_turn.angle) * dist; std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_EXECUTE_SHOT] Position success: " << simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl; player_turn_state = PLAYER_RAISE_CUE; } else if (player_turn_state == PLAYER_RAISE_CUE) { tip_position[2] = CUE_RAISED_HEIGHT; std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_RAISE_CUE] Position success: " << simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl; player_turn_state = PLAYER_NONE; } generate_image_message(image_msg); // // Transfer to old table state and get new state // old_table_state = table_state; get_ball_positions(); get_cue_position_orientation(); // //Check if table is in steady state // steady_state = (table_state.white_ball.z < POOL_TABLE_HEIGHT)? (true):(closeTo(old_table_state.white_ball, table_state.white_ball, STEADY_STATE_DRIFT)); for (unsigned int i = 0; (i < BALL_COUNT) && steady_state; i++) { if (!closeTo(old_table_state.balls[i], table_state.balls[i], STEADY_STATE_DRIFT)) { steady_state = false; } } // // Publish information // if (steady_state && (player_turn_state == PLAYER_NONE && new_player_turn == false)) { ROS_INFO("Table is in steady state!\n"); //calculate turn score and publish it. int score = calculate_turn_score(); ROS_INFO("The score for the last turn is %d", score); std_msgs::Int32 score_msg; score_msg.data = score; score_pub.publish(score_msg); usleep(50 * 1000); //50ms table_state_pub.publish(table_state); // publish a message only if nothing is moving } // // Move white ball if potted // if (steady_state && (table_state.white_ball.z < POOL_TABLE_HEIGHT)&& (player_turn_state == PLAYER_NONE && new_player_turn == false)) { std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " "; std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " << simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl; table_state.white_ball.x = white_ball_initial_position[0]; table_state.white_ball.y = white_ball_initial_position[1]; table_state.white_ball.z = white_ball_initial_position[2]; } // // Move all red balls is all potted // if (steady_state) { int balls_in = 0; for (unsigned int i = 0; i < BALL_COUNT; i++) { if (table_state.balls[i].z < POOL_TABLE_HEIGHT) { balls_in++; } } if (balls_in == BALL_COUNT) { //Move all balls to the top! std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " "; std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " << simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl; for (unsigned int i = 0; i < BALL_COUNT; i++) { std::cout << "[V-REP_ROS_POOL_SIM] Ball " << i << std::endl; float p[3]; p[0] = initial_table_state.balls[i].x; p[1] = initial_table_state.balls[i].y; p[2] = initial_table_state.balls[i].z; std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(balls_handle[i]) << std::endl; std::cout << "[V-REP_ROS_POOL_SIM] Moving ball position success: " << simSetObjectPosition(balls_handle[i], WORLD_FRAME, p) << std::endl; } table_state = initial_table_state; games_played++; ROS_INFO("The are %d games played so far.", games_played); } } simLockInterface(0); //Publish messages img_pub.publish(image_msg); // The best would be to start ROS activity when a simulation is running, and stop it when a simulation ended // If you allow ROS activity while a simulation is not running, then it becomes a little bit tricky when a scene // was switched for example (e.g. the clients would need a way to detect that) } if (message==sim_message_eventcallback_simulationabouttostart) { // Simulation is about to start // Here you could launch the ROS node (if using just one node) // If more than one node should be supported (e.g. with different services per node), // then it is better to start a node via a custom Lua function white_ball_handle = simGetObjectHandle("white_ball"); std::cout << "[V-REP POOL PLUGIN] White ball handle: " << white_ball_handle << std::endl; tip_handle = simGetObjectHandle("tip"); for (int i=0; i < BALL_COUNT; i++) { std::stringstream ball_name; ball_name << "ball"; ball_name << i; balls_handle[i] = simGetObjectHandle(ball_name.str().c_str()); //std::cout << "Ball " << i << " handle: " << balls_handle[i] << std::endl; } camera_handle = simGetObjectHandle("camera_sensor"); std::cout << "[V-REP POOL PLUGIN] Camera handle: " << camera_handle << std::endl; //Read all positions and make them as old positions get_ball_positions(); old_table_state = table_state; pre_hit_table_state = table_state; initial_table_state = table_state; } if (message==sim_message_eventcallback_simulationended) { // Simulation just ended // Here you could kill the ROS node(s) that are still active. There could also be a custom Lua function to kill a specific ROS node. } // Keep following unchanged: simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings simLockInterface(0); return (retVal); }