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