35 #include <boost/thread/mutex.hpp>
39 using namespace visualization_msgs;
55 server_(
"pong",
"", false),
59 player_contexts_.resize(2);
77 if ( player_contexts_[0].active || player_contexts_[1].active )
79 float ball_dx = speed_ * ball_dir_x_;
80 float ball_dy = speed_ * ball_dir_y_;
82 ball_pos_x_ += ball_dx;
83 ball_pos_y_ += ball_dy;
87 if ( reflect ( ball_pos_y_, last_ball_pos_y_,
FIELD_HEIGHT * 0.5,
t ) )
89 ball_pos_x_ -=
t * ball_dx;
90 ball_pos_y_ -=
t * ball_dy;
94 ball_dx = speed_ * ball_dir_x_;
95 ball_dy = speed_ * ball_dir_y_;
96 ball_pos_x_ +=
t * ball_dx;
97 ball_pos_y_ +=
t * ball_dy;
100 int player = ball_pos_x_ > 0 ? 1 : 0;
110 reflect ( ball_pos_x_, last_ball_pos_x_,
FIELD_WIDTH * 0.5,
t );
111 ball_pos_x_ -=
t * ball_dx;
112 ball_pos_y_ -=
t * ball_dy;
115 float offset = (ball_pos_y_ - player_contexts_[player].pos) /
PADDLE_SIZE;
118 ball_dir_y_ += offset*2.0;
123 if ( fabs(ball_dir_y_) > 0.707106781 )
125 ball_dir_x_ = ball_dir_x_ > 0.0 ? 1.0 : -1.0;
126 ball_dir_y_ = ball_dir_y_ > 0.0 ? 1.0 : -1.0;
130 ball_dx = speed_ * ball_dir_x_;
131 ball_dy = speed_ * ball_dir_y_;
132 ball_pos_x_ +=
t * ball_dx;
133 ball_pos_y_ +=
t * ball_dy;
141 ball_pos_x_ -=
t * ball_dx;
142 ball_pos_y_ -=
t * ball_dy;
145 player_contexts_[1-player].score++;
148 server_.applyChanges();
157 last_ball_pos_x_ = ball_pos_x_;
158 last_ball_pos_y_ = ball_pos_y_;
161 if ( !player_contexts_[0].active || !player_contexts_[1].active )
163 int player = player_contexts_[0].active ? 1 : 0;
164 float delta = ball_pos_y_ - player_contexts_[player].pos;
168 setPaddlePos( player, player_contexts_[player].pos + delta );
174 server_.applyChanges();
194 player_contexts_[player].pos = pos;
196 geometry_msgs::Pose pose;
198 pose.position.y = pos;
200 std::string marker_name = (player == 0) ?
"paddle0" :
"paddle1";
201 server_.setPose( marker_name, pose );
202 server_.setPose( marker_name+
"_display", pose );
205 void processPaddleFeedback(
unsigned player,
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
212 std::string control_marker_name = feedback->marker_name;
213 geometry_msgs::Pose pose = feedback->pose;
215 setPaddlePos( player, pose.position.y );
217 if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN )
219 player_contexts_[player].active =
true;
221 if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP )
223 player_contexts_[player].active =
false;
233 ball_dir_x_ = ball_dir_x_ > 0.0 ? 1.0 : -1.0;
234 ball_dir_y_ =
rand() % 2 ? 1.0 : -1.0;
241 float l = sqrt( ball_dir_x_*ball_dir_x_ + ball_dir_y_*ball_dir_y_ );
250 bool reflect(
float &pos,
float last_pos,
float limit,
float &t )
254 t = (pos - limit) / (pos - last_pos);
259 t = (-pos - limit) / (last_pos - pos);
268 geometry_msgs::Pose pose;
269 pose.position.x = ball_pos_x_;
270 pose.position.y = ball_pos_y_;
271 server_.setPose(
"ball", pose );
281 InteractiveMarkerControl control;
282 control.always_visible =
true;
285 marker.type = Marker::TEXT_VIEW_FACING;
286 marker.color.r = 1.0;
287 marker.color.g = 1.0;
288 marker.color.b = 1.0;
289 marker.color.a = 1.0;
290 marker.scale.x = 1.5;
291 marker.scale.y = 1.5;
292 marker.scale.z = 1.5;
294 std::ostringstream
s;
295 s << player_contexts_[0].score;
296 marker.text =
s.str();
299 control.markers.push_back( marker );
302 s << player_contexts_[1].score;
303 marker.text =
s.str();
304 marker.pose.position.x *= -1;
305 control.markers.push_back( marker );
318 InteractiveMarkerControl control;
319 control.always_visible =
true;
322 marker.type = Marker::CUBE;
323 marker.color.r = 1.0;
324 marker.color.g = 1.0;
325 marker.color.b = 1.0;
326 marker.color.a = 1.0;
332 marker.pose.position.x = 0;
334 control.markers.push_back( marker );
337 marker.pose.position.y *= -1;
338 control.markers.push_back( marker );
345 marker.pose.position.y = 0;
346 control.markers.push_back( marker );
349 marker.pose.position.x *= -1;
350 control.markers.push_back( marker );
363 InteractiveMarkerControl control;
364 control.always_visible =
false;
365 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
372 marker.type = Marker::CUBE;
373 marker.color.r = 1.0;
374 marker.color.g = 1.0;
375 marker.color.b = 1.0;
376 marker.color.a = 0.0;
380 marker.pose.position.z = 0;
381 marker.pose.position.y = 0;
383 control.markers.push_back( marker );
403 marker.color.r = 0.5;
404 marker.color.a = 1.0;
406 control.interaction_mode = InteractiveMarkerControl::NONE;
407 control.always_visible =
true;
413 marker.color.g = 1.0;
414 marker.color.b = 0.5;
417 control.markers.clear();
418 control.markers.push_back( marker );
426 marker.color.g = 0.5;
427 marker.color.b = 1.0;
430 control.markers.clear();
431 control.markers.push_back( marker );
441 InteractiveMarkerControl control;
442 control.always_visible =
true;
447 control.interaction_mode = InteractiveMarkerControl::NONE;
453 marker.color.r = 1.0;
454 marker.color.g = 1.0;
455 marker.color.b = 1.0;
456 marker.color.a = 1.0;
457 marker.type = Marker::CYLINDER;
461 control.markers.push_back( marker );
497 int main(
int argc,
char** argv)