pong.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
32 
33 #include <ros/ros.h>
34 #include <math.h>
35 #include <boost/thread/mutex.hpp>
36 
37 #include <tf/tf.h>
38 
39 using namespace visualization_msgs;
40 
41 static const float FIELD_WIDTH = 12.0;
42 static const float FIELD_HEIGHT = 8.0;
43 static const float BORDER_SIZE = 0.5;
44 static const float PADDLE_SIZE = 2.0;
45 static const float UPDATE_RATE = 1.0 / 30.0;
46 static const float PLAYER_X = FIELD_WIDTH * 0.5 + BORDER_SIZE;
47 static const float AI_SPEED_LIMIT = 0.25;
48 
49 
50 class PongGame
51 {
52 public:
53 
55  server_("pong", "", false),
56  last_ball_pos_x_(0),
57  last_ball_pos_y_(0)
58  {
59  player_contexts_.resize(2);
60 
61  makeFieldMarker();
62  makePaddleMarkers();
63  makeBallMarker();
64 
65  reset();
66  updateScore();
67 
68  ros::NodeHandle nh;
69  game_loop_timer_ = nh.createTimer(ros::Duration(UPDATE_RATE), boost::bind( &PongGame::spinOnce, this ) );
70  }
71 
72 private:
73 
74  // main control loop
75  void spinOnce()
76  {
77  if ( player_contexts_[0].active || player_contexts_[1].active )
78  {
79  float ball_dx = speed_ * ball_dir_x_;
80  float ball_dy = speed_ * ball_dir_y_;
81 
82  ball_pos_x_ += ball_dx;
83  ball_pos_y_ += ball_dy;
84 
85  // bounce off top / bottom
86  float t = 0;
87  if ( reflect ( ball_pos_y_, last_ball_pos_y_, FIELD_HEIGHT * 0.5, t ) )
88  {
89  ball_pos_x_ -= t * ball_dx;
90  ball_pos_y_ -= t * ball_dy;
91 
92  ball_dir_y_ *= -1.0;
93 
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;
98  }
99 
100  int player = ball_pos_x_ > 0 ? 1 : 0;
101 
102  // reflect on paddles
103  if ( fabs(last_ball_pos_x_) < FIELD_WIDTH * 0.5 &&
104  fabs(ball_pos_x_) >= FIELD_WIDTH * 0.5 )
105  {
106  // check if the paddle is roughly at the right position
107  if ( ball_pos_y_ > player_contexts_[player].pos - PADDLE_SIZE * 0.5 - 0.5*BORDER_SIZE &&
108  ball_pos_y_ < player_contexts_[player].pos + PADDLE_SIZE * 0.5 + 0.5*BORDER_SIZE )
109  {
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;
113 
114  // change direction based on distance to paddle center
115  float offset = (ball_pos_y_ - player_contexts_[player].pos) / PADDLE_SIZE;
116 
117  ball_dir_x_ *= -1.0;
118  ball_dir_y_ += offset*2.0;
119 
120  normalizeVel();
121 
122  // limit angle to 45 deg
123  if ( fabs(ball_dir_y_) > 0.707106781 )
124  {
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;
127  normalizeVel();
128  }
129 
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;
134  }
135  }
136 
137  // ball hits the left/right border of the playing field
138  if ( fabs(ball_pos_x_) >= FIELD_WIDTH * 0.5 + 1.5*BORDER_SIZE )
139  {
140  reflect ( ball_pos_x_, last_ball_pos_x_, FIELD_WIDTH * 0.5 + 1.5*BORDER_SIZE, t );
141  ball_pos_x_ -= t * ball_dx;
142  ball_pos_y_ -= t * ball_dy;
143  updateBall();
144 
145  player_contexts_[1-player].score++;
146  updateScore();
147 
148  server_.applyChanges();
149  reset();
150  ros::Duration(1.0).sleep();
151  }
152  else
153  {
154  updateBall();
155  }
156 
157  last_ball_pos_x_ = ball_pos_x_;
158  last_ball_pos_y_ = ball_pos_y_;
159 
160  // control computer player
161  if ( !player_contexts_[0].active || !player_contexts_[1].active )
162  {
163  int player = player_contexts_[0].active ? 1 : 0;
164  float delta = ball_pos_y_ - player_contexts_[player].pos;
165  // limit movement speed
166  if ( delta > AI_SPEED_LIMIT ) delta = AI_SPEED_LIMIT;
167  if ( delta < -AI_SPEED_LIMIT ) delta = -AI_SPEED_LIMIT;
168  setPaddlePos( player, player_contexts_[player].pos + delta );
169  }
170 
171  speed_ += 0.0003;
172  }
173 
174  server_.applyChanges();
175  }
176 
177  void setPaddlePos( unsigned player, float pos )
178  {
179  if ( player > 1 )
180  {
181  return;
182  }
183 
184  // clamp
185  if ( pos > (FIELD_HEIGHT - PADDLE_SIZE) * 0.5 )
186  {
187  pos = (FIELD_HEIGHT - PADDLE_SIZE) * 0.5;
188  }
189  if ( pos < (FIELD_HEIGHT - PADDLE_SIZE) * -0.5 )
190  {
191  pos = (FIELD_HEIGHT - PADDLE_SIZE) * -0.5;
192  }
193 
194  player_contexts_[player].pos = pos;
195 
196  geometry_msgs::Pose pose;
197  pose.position.x = (player == 0) ? -PLAYER_X : PLAYER_X;
198  pose.position.y = pos;
199 
200  std::string marker_name = (player == 0) ? "paddle0" : "paddle1";
201  server_.setPose( marker_name, pose );
202  server_.setPose( marker_name+"_display", pose );
203  }
204 
205  void processPaddleFeedback( unsigned player, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
206  {
207  if ( player > 1 )
208  {
209  return;
210  }
211 
212  std::string control_marker_name = feedback->marker_name;
213  geometry_msgs::Pose pose = feedback->pose;
214 
215  setPaddlePos( player, pose.position.y );
216 
217  if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN )
218  {
219  player_contexts_[player].active = true;
220  }
221  if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP )
222  {
223  player_contexts_[player].active = false;
224  }
225  }
226 
227  // restart round
228  void reset()
229  {
230  speed_ = 6.0 * UPDATE_RATE;
231  ball_pos_x_ = 0.0;
232  ball_pos_y_ = 0.0;
233  ball_dir_x_ = ball_dir_x_ > 0.0 ? 1.0 : -1.0;
234  ball_dir_y_ = rand() % 2 ? 1.0 : -1.0;
235  normalizeVel();
236  }
237 
238  // set length of velocity vector to 1
240  {
241  float l = sqrt( ball_dir_x_*ball_dir_x_ + ball_dir_y_*ball_dir_y_ );
242  ball_dir_x_ /= l;
243  ball_dir_y_ /= l;
244  }
245 
246  // compute reflection
247  // returns true if the given limit has been surpassed
248  // t [0...1] says how much the limit has been surpassed, relative to the distance
249  // between last_pos and pos
250  bool reflect( float &pos, float last_pos, float limit, float &t )
251  {
252  if ( pos > limit )
253  {
254  t = (pos - limit) / (pos - last_pos);
255  return true;
256  }
257  if ( -pos > limit )
258  {
259  t = (-pos - limit) / (last_pos - pos);
260  return true;
261  }
262  return false;
263  }
264 
265  // update ball marker
266  void updateBall()
267  {
268  geometry_msgs::Pose pose;
269  pose.position.x = ball_pos_x_;
270  pose.position.y = ball_pos_y_;
271  server_.setPose( "ball", pose );
272  }
273 
274  // update score marker
275  void updateScore()
276  {
277  InteractiveMarker int_marker;
278  int_marker.header.frame_id = "base_link";
279  int_marker.name = "score";
280 
281  InteractiveMarkerControl control;
282  control.always_visible = true;
283 
284  Marker marker;
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;
293 
294  std::ostringstream s;
295  s << player_contexts_[0].score;
296  marker.text = s.str();
297  marker.pose.position.y = FIELD_HEIGHT*0.5 + 4.0*BORDER_SIZE;
298  marker.pose.position.x = -1.0 * ( FIELD_WIDTH * 0.5 + BORDER_SIZE );
299  control.markers.push_back( marker );
300 
301  s.str("");
302  s << player_contexts_[1].score;
303  marker.text = s.str();
304  marker.pose.position.x *= -1;
305  control.markers.push_back( marker );
306 
307  int_marker.controls.push_back( control );
308 
309  server_.insert( int_marker );
310  }
311 
313  {
314  InteractiveMarker int_marker;
315  int_marker.header.frame_id = "base_link";
316  int_marker.name = "field";
317 
318  InteractiveMarkerControl control;
319  control.always_visible = true;
320 
321  Marker marker;
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;
327 
328  // Top Border
329  marker.scale.x = FIELD_WIDTH + 6.0 * BORDER_SIZE;
330  marker.scale.y = BORDER_SIZE;
331  marker.scale.z = BORDER_SIZE;
332  marker.pose.position.x = 0;
333  marker.pose.position.y = FIELD_HEIGHT*0.5 + BORDER_SIZE;
334  control.markers.push_back( marker );
335 
336  // Bottom Border
337  marker.pose.position.y *= -1;
338  control.markers.push_back( marker );
339 
340  // Left Border
341  marker.scale.x = BORDER_SIZE;
342  marker.scale.y = FIELD_HEIGHT + 3.0*BORDER_SIZE;
343  marker.scale.z = BORDER_SIZE;
344  marker.pose.position.x = FIELD_WIDTH*0.5 + 2.5*BORDER_SIZE;
345  marker.pose.position.y = 0;
346  control.markers.push_back( marker );
347 
348  // Right Border
349  marker.pose.position.x *= -1;
350  control.markers.push_back( marker );
351 
352  // store
353  int_marker.controls.push_back( control );
354  server_.insert( int_marker );
355  }
356 
358  {
359  InteractiveMarker int_marker;
360  int_marker.header.frame_id = "base_link";
361 
362  // Add a control for moving the paddle
363  InteractiveMarkerControl control;
364  control.always_visible = false;
365  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
366  tf::Quaternion orien(0.0, 0.0, 1.0, 1.0);
367  orien.normalize();
368  tf::quaternionTFToMsg(orien, control.orientation);
369 
370  // Add a visualization marker
371  Marker marker;
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;
377  marker.scale.x = BORDER_SIZE + 0.1;
378  marker.scale.y = PADDLE_SIZE + 0.1;
379  marker.scale.z = BORDER_SIZE + 0.1;
380  marker.pose.position.z = 0;
381  marker.pose.position.y = 0;
382 
383  control.markers.push_back( marker );
384 
385  int_marker.controls.push_back( control );
386 
387  // Control for player 1
388  int_marker.name = "paddle0";
389  int_marker.pose.position.x = -PLAYER_X;
390  server_.insert( int_marker );
391  server_.setCallback( int_marker.name, boost::bind( &PongGame::processPaddleFeedback, this, 0, _1 ) );
392 
393  // Control for player 2
394  int_marker.name = "paddle1";
395  int_marker.pose.position.x = PLAYER_X;
396  server_.insert( int_marker );
397  server_.setCallback( int_marker.name, boost::bind( &PongGame::processPaddleFeedback, this, 1, _1 ) );
398 
399  // Make display markers
400  marker.scale.x = BORDER_SIZE;
401  marker.scale.y = PADDLE_SIZE;
402  marker.scale.z = BORDER_SIZE;
403  marker.color.r = 0.5;
404  marker.color.a = 1.0;
405 
406  control.interaction_mode = InteractiveMarkerControl::NONE;
407  control.always_visible = true;
408 
409  // Display for player 1
410  int_marker.name = "paddle0_display";
411  int_marker.pose.position.x = -PLAYER_X;
412 
413  marker.color.g = 1.0;
414  marker.color.b = 0.5;
415 
416  int_marker.controls.clear();
417  control.markers.clear();
418  control.markers.push_back( marker );
419  int_marker.controls.push_back( control );
420  server_.insert( int_marker );
421 
422  // Display for player 2
423  int_marker.name = "paddle1_display";
424  int_marker.pose.position.x = PLAYER_X;
425 
426  marker.color.g = 0.5;
427  marker.color.b = 1.0;
428 
429  int_marker.controls.clear();
430  control.markers.clear();
431  control.markers.push_back( marker );
432  int_marker.controls.push_back( control );
433  server_.insert( int_marker );
434  }
435 
437  {
438  InteractiveMarker int_marker;
439  int_marker.header.frame_id = "base_link";
440 
441  InteractiveMarkerControl control;
442  control.always_visible = true;
443 
444  // Ball
445  int_marker.name = "ball";
446 
447  control.interaction_mode = InteractiveMarkerControl::NONE;
448  tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
449  orien.normalize();
450  tf::quaternionTFToMsg(orien, control.orientation);
451 
452  Marker marker;
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;
458  marker.scale.x = BORDER_SIZE;
459  marker.scale.y = BORDER_SIZE;
460  marker.scale.z = BORDER_SIZE;
461  control.markers.push_back( marker );
462 
463  int_marker.controls.push_back( control );
464 
465  server_.insert( int_marker );
466  }
467 
469 
471 
472  InteractiveMarker field_marker_;
473 
475  {
476  PlayerContext(): pos(0),active(false),score(0) {}
477  float pos;
478  bool active;
479  int score;
480  };
481 
482  std::vector<PlayerContext> player_contexts_;
483 
486 
487  float ball_pos_x_;
488  float ball_pos_y_;
489 
490  float ball_dir_x_;
491  float ball_dir_y_;
492  float speed_;
493 };
494 
495 
496 
497 int main(int argc, char** argv)
498 {
499  ros::init(argc, argv, "pong");
500 
501  PongGame pong_game;
502  ros::spin();
503  ROS_INFO("Exiting..");
504 }
static const float UPDATE_RATE
Definition: pong.cpp:45
float ball_dir_x_
Definition: pong.cpp:490
def rand(min_, max_)
static const float FIELD_WIDTH
Definition: pong.cpp:41
void processPaddleFeedback(unsigned player, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: pong.cpp:205
float ball_pos_y_
Definition: pong.cpp:488
void setPaddlePos(unsigned player, float pos)
Definition: pong.cpp:177
XmlRpcServer s
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
float speed_
Definition: pong.cpp:492
float ball_dir_y_
Definition: pong.cpp:491
int main(int argc, char **argv)
Definition: pong.cpp:497
Quaternion & normalize()
void reset()
Definition: pong.cpp:228
void makeFieldMarker()
Definition: pong.cpp:312
void makePaddleMarkers()
Definition: pong.cpp:357
ROSCPP_DECL void spin(Spinner &spinner)
float last_ball_pos_x_
Definition: pong.cpp:484
interactive_markers::InteractiveMarkerServer server_
Definition: pong.cpp:468
static const float PLAYER_X
Definition: pong.cpp:46
void makeBallMarker()
Definition: pong.cpp:436
bool reflect(float &pos, float last_pos, float limit, float &t)
Definition: pong.cpp:250
void normalizeVel()
Definition: pong.cpp:239
void updateBall()
Definition: pong.cpp:266
#define ROS_INFO(...)
float last_ball_pos_y_
Definition: pong.cpp:485
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
static const float AI_SPEED_LIMIT
Definition: pong.cpp:47
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
static const float FIELD_HEIGHT
Definition: pong.cpp:42
ros::Timer game_loop_timer_
Definition: pong.cpp:470
PongGame()
Definition: pong.cpp:54
std::vector< PlayerContext > player_contexts_
Definition: pong.cpp:482
static const float PADDLE_SIZE
Definition: pong.cpp:44
static const float BORDER_SIZE
Definition: pong.cpp:43
void updateScore()
Definition: pong.cpp:275
void spinOnce()
Definition: pong.cpp:75
InteractiveMarker field_marker_
Definition: pong.cpp:472
float ball_pos_x_
Definition: pong.cpp:487


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Sat May 16 2020 03:49:16