void SetTwistHandler::handleSimulation(){ // called when the main script calls: simHandleModule if(!_initialized){ _initialize(); } Eigen::Quaternion < simFloat > orientation; //(x,y,z,w) Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x, (simFloat)_twistCommands.twist.linear.y, (simFloat)_twistCommands.twist.linear.z); Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x, (simFloat)_twistCommands.twist.angular.y, (simFloat)_twistCommands.twist.angular.z); // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame. if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){ linVelocity = orientation*linVelocity; angVelocity = orientation*angVelocity; } else { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl; ConsoleHandler::printInConsole(ss); } simResetDynamicObject(_associatedObjectID); //Set object velocity if (_isStatic){ Eigen::Matrix<simFloat, 3, 1> position; simGetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat timeStep = simGetSimulationTimeStep(); position = position + timeStep*linVelocity; simSetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat angle = timeStep*angVelocity.norm(); if(angle > 1e-6){ orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation; } simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()); } else { // Apply the linear velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } // Apply the angular velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } } }
// 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); }