void chatterCallback(const ws_referee::custom::ConstPtr& msg_in)
{
//ROS_INFO("%s: I heard: [%s]", _name.c_str(),msg_in->data.c_str());
//std_msgs::String msg_out;
//msg_out.data = "hello world";
//chatter_pub.publish(msg_out);
ROS_INFO("%s: Received msg wit dist= %f", _name.c_str(), msg_in->dist);
// check fo the palayer in the field
tf::StampedTransform tf_2;
bool be_a_police= true;
try
{
listener->lookupTransform("world", "tf_"+_police_player, ros::Time(0), tf_2);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
be_a_police = false;
}
if (be_a_police)
{
if (!is_in_field (tf_2.getOrigin().x(),tf_2.getOrigin().y() ))
{
ROS_INFO("%s: I found that %s is out of the area will send him to the start point", _name.c_str(), _police_player.c_str());
ws_referee::MovePlayerTo srv;
srv.request.new_pos_x = -5;
srv.request.new_pos_y = 0;
srv.request.player_that_requested=_name;
if (client->call(srv))
{}
else
{
ROS_ERROR ("failed to call service ... ");
}
}
}
tf::Transform tf_tmp;
tf_tmp.setOrigin( tf::Vector3(msg_in->dist, 0.0, 0.0) );
tf_tmp.setRotation( tf::Quaternion(0,0,get_random_deg()*M_PI/180,1));
br->sendTransform(tf::StampedTransform(tf_tmp, ros::Time::now(), "tf_"+_name, "tf_tmp"+_name));
// query tranform world to tf_tmp_vahid
tf::StampedTransform tf_1;
try{
listener->lookupTransform("world", "tf_tmp" + _name, ros::Time(0), tf_1);
}
catch(tf::TransformException ex){
ROS_ERROR("%s", ex.what());
}
//send transform
transform= tf_1;
br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_"+_name));
//_pos_x+=msg_in->dist;
bool should_quit=false;
//Publish new msg
ws_referee::custom msg_out;
msg_out.sender = _name;
msg_out.dist=get_random_num();
if (msg_out.winner!="")
{
ROS_INFO("\n\n ------------ \n\n");
msg_out.winner = msg_in->winner;
msg_out.dist = 0;
should_quit=true;
}
// else if (_pos_x>5)
// {
// ROS_INFO("\n\n I WON - HAMID \n\n");
// msg_out.winner="Hamid";
//msg_out.dist = 0;
//should_quit=true;
// }
else
{
msg_out.winner = "";
msg_out.dist = get_random_num();
}
chatter_pub.publish(msg_out);
ROS_INFO("%s: I'm at %f. will publish a message", _name.c_str(), _pos_x);
//marker value
visualization_msgs::Marker marker;
marker.header.frame_id = "tf_"+_name;
marker.header.stamp = ros::Time();
marker.ns = "";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.3;
marker.scale.y = 0.3;
marker.scale.z = 0.3;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker_pub.publish( marker );
// a new text marker
marker.id = 1;
marker.color.r = 0.5;
marker.pose.position.x = - 0.3;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = "hamid";
marker_pub.publish( marker );
// transformation
//transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) );
//transform.setRotation( tf::Quaternion(0,0,0,1));
br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_"+_name));
ros::Duration(0.5).sleep();
if (should_quit==true)
{
ROS_INFO("%s: I will shutdown",_name.c_str());
ros::shutdown();
}
}
void chatterCallback(const ws_referee::custom::ConstPtr& msg_in)
{
ROS_INFO("%s: Received msg with dist=%f",_name.c_str(), msg_in->dist);
//Check for the policed player
bool be_a_police = true;
//query transform world to the policed player
tf::StampedTransform tf_2;
try{
listener->lookupTransform("world", "tf_" + _policed_player, ros::Time(0), tf_2);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
be_a_police = false;
}
if (be_a_police)
{
if (!is_in_field(tf_2.getOrigin().x(), tf_2.getOrigin().y()))
{
//send player to new_pos_x = -5 and new_pos_y=0
ROS_INFO("%s: Policing ... I found that %s if out of the arena. Will send him to -5,0", _name.c_str(), _policed_player.c_str());
}
}
//Position update
//Send transform from tf_mike to tf_tmp_mike
tf::Transform tf_tmp;
tf_tmp.setOrigin( tf::Vector3(msg_in->dist, 0.0, 0.0) );
tf_tmp.setRotation( tf::Quaternion(0, 0, get_random_deg()*M_PI/180, 1) );
br->sendTransform(tf::StampedTransform(tf_tmp, ros::Time::now(), "tf_" + _name, "tf_tmp" + _name));
ros::Duration(0.3).sleep(); //wait a bit
//query transform world to tf_tmp_mike
tf::StampedTransform tf_1;
try{
listener->lookupTransform("world", "tf_tmp" + _name, ros::Time(0), tf_1);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
//send new transform
transform = tf_1;
br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name));
bool should_quit=false;
//Publish new msg
ws_referee::custom msg_out;
msg_out.sender = _name;
//Check race status
if (msg_in->winner!="")//if someone else won (crap)
{
ROS_INFO("%s: Damn, %s has won the race. It was just luck",_name.c_str(), msg_in->winner.c_str());
msg_out.winner = msg_in->winner;
msg_out.dist = 0;
should_quit=true;
}
else if (_pos_x > 5) //if I won
{
ROS_WARN("\n\n%s: I WON IUPIIII\n\n ",_name.c_str());
msg_out.winner = "mike";
msg_out.dist = 0;
should_quit=true;
}
else //if nobody won
{
msg_out.winner = "";
msg_out.dist = get_random_num();
}
chatter_pub.publish(msg_out);
ROS_INFO("%s: I am at %f. I will win for sure. will publish a message",_name.c_str(), _pos_x);
//The visualization markers
visualization_msgs::Marker marker;
marker.header.frame_id = "tf_" + _name;
marker.header.stamp = ros::Time();
marker.ns = ""; marker.id = 0;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = 0; marker.pose.position.y = 0;
marker.pose.position.z = 0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0;
marker.scale.x = 0.3; marker.scale.y = 0.3; marker.scale.z = 0.3;
marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 0.3;
marker.color.b = 0.0;
marker_pub.publish( marker );
//a new text marker
marker.id = 1;
marker.color.r = 0.4;
marker.pose.position.x = - 0.3;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.text = "mike";
marker_pub.publish( marker );
//Send the transformation
br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name));
if (should_quit==true)
{
ROS_INFO("%s: I will shutdown",_name.c_str());
ros::shutdown();
}
}
