int getHandleFromParam(const Param& params_){
int handle = simGetObjectHandle(std::get<I> (params_).c_str() );
if(handle == -1){
BOOST_THROW_EXCEPTION(std::runtime_error("getHandleFromParam: Handles are not valid. Make sure the object you are looking for actually exists."));
}
return handle;
};
void JointControlPluglet::positionCallback(
const brics_actuator::JointPositions msg) {
//if (ctl_state != Position) {
//ctl_state = Position;
//}
for (unsigned int i = 0; i < msg.positions.size(); i++) {
if (!controlledJoint(msg.positions[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.positions[i].joint_uri.c_str());
if (handle > 0) {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 1);
simSetJointTargetPosition(handle,
msg.positions[i].value);
}
}
}
bool JointControlPluglet::controlledJoint(std::string name) {
int handle = simGetObjectHandle(name.c_str());
for (size_t i=0; i<handles.size(); i++) {
if (handle == handles[i]) {
return true;
}
}
return false;
}
void JointControlPluglet::velocityCallback(
const brics_actuator::JointVelocities msg) {
//ctl_state = Velocity;
//}
for (unsigned int i = 0; i < msg.velocities.size(); i++) {
if (!controlledJoint(msg.velocities[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.velocities[i].joint_uri.c_str());
if (handle > 0) {
if (std::fabs(msg.velocities[i].value) < 0.0000001) {
brics_actuator::JointPositions msg_single;
msg_single.poisonStamp = msg.poisonStamp;
brics_actuator::JointValue value = msg.velocities[i];
value.unit = "rad";
float dval = 0.0;
simGetJointPosition(handles[i], &dval);
value.value = dval;
msg_single.positions.push_back(value);
//positionCallback(msg_single);
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 1);
simSetJointTargetPosition(handle, dval);
} else {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 0);
simSetObjectIntParameter(handle, 1000, 1);
simSetJointTargetVelocity(handle, static_cast<float>(msg.velocities[i].value));
}
}
}
}
void getJointHandles()
{
std::stringstream ss;
bool error = false;
for (int i = 1; i <= 6; ++i)
{
ss.str("");
ss << "Joint" << i;
joint_handles[i-1] = simGetObjectHandle(ss.str().c_str());
if (joint_handles[i-1] == -1)
{
error = true;
ROS_WARN("Unable to retrieve Joint%d handle.", i);
}
}
if (!error) ROS_INFO("Finished reading joint handles.");
}
void JointControlPluglet::torqueCallback(
const brics_actuator::JointTorques msg) {
for (unsigned int i = 0; i < msg.torques.size(); i++) {
if (!controlledJoint(msg.torques[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.torques[i].joint_uri.c_str());
if (handle > 0) {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 0);
simSetObjectIntParameter(handle, 1000, 2);
simSetJointForce(handle,
msg.torques[i].value);
}
}
}
// 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);
}
