pong.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include <interactive_markers/interactive_marker_server.h>
00032 
00033 #include <ros/ros.h>
00034 #include <math.h>
00035 #include <boost/thread/mutex.hpp>
00036 
00037 using namespace visualization_msgs;
00038 
00039 static const float FIELD_WIDTH = 12.0;
00040 static const float FIELD_HEIGHT = 8.0;
00041 static const float BORDER_SIZE = 0.5;
00042 static const float PADDLE_SIZE = 2.0;
00043 static const float UPDATE_RATE = 1.0 / 30.0;
00044 static const float PLAYER_X = FIELD_WIDTH * 0.5 + BORDER_SIZE;
00045 static const float AI_SPEED_LIMIT = 0.25;
00046 
00047 
00048 class PongGame
00049 {
00050 public:
00051 
00052   PongGame() :
00053   server_("pong", "", false),
00054   last_ball_pos_x_(0),
00055   last_ball_pos_y_(0)
00056   {
00057     player_contexts_.resize(2);
00058 
00059     makeFieldMarker();
00060     makePaddleMarkers();
00061     makeBallMarker();
00062 
00063     reset();
00064     updateScore();
00065 
00066     ros::NodeHandle nh;
00067     game_loop_timer_ =  nh.createTimer(ros::Duration(UPDATE_RATE), boost::bind( &PongGame::spinOnce, this ) );
00068   }
00069 
00070 private:
00071 
00072   // main control loop
00073   void spinOnce()
00074   {
00075     if ( player_contexts_[0].active || player_contexts_[1].active )
00076     {
00077       float ball_dx = speed_ * ball_dir_x_;
00078       float ball_dy = speed_ * ball_dir_y_;
00079 
00080       ball_pos_x_ += ball_dx;
00081       ball_pos_y_ += ball_dy;
00082 
00083       // bounce off top / bottom
00084       float t = 0;
00085       if ( reflect ( ball_pos_y_, last_ball_pos_y_, FIELD_HEIGHT * 0.5, t ) )
00086       {
00087         ball_pos_x_ -= t * ball_dx;
00088         ball_pos_y_ -= t * ball_dy;
00089 
00090         ball_dir_y_ *= -1.0;
00091 
00092         ball_dx = speed_ * ball_dir_x_;
00093         ball_dy = speed_ * ball_dir_y_;
00094         ball_pos_x_ += t * ball_dx;
00095         ball_pos_y_ += t * ball_dy;
00096       }
00097 
00098       int player = ball_pos_x_ > 0 ? 1 : 0;
00099 
00100       // reflect on paddles
00101       if ( fabs(last_ball_pos_x_) < FIELD_WIDTH * 0.5 &&
00102            fabs(ball_pos_x_) >= FIELD_WIDTH * 0.5 )
00103       {
00104         // check if the paddle is roughly at the right position
00105         if ( ball_pos_y_ > player_contexts_[player].pos - PADDLE_SIZE * 0.5 - 0.5*BORDER_SIZE &&
00106              ball_pos_y_ < player_contexts_[player].pos + PADDLE_SIZE * 0.5 + 0.5*BORDER_SIZE )
00107         {
00108           reflect ( ball_pos_x_, last_ball_pos_x_, FIELD_WIDTH * 0.5, t );
00109           ball_pos_x_ -= t * ball_dx;
00110           ball_pos_y_ -= t * ball_dy;
00111 
00112           // change direction based on distance to paddle center
00113           float offset = (ball_pos_y_ - player_contexts_[player].pos) / PADDLE_SIZE;
00114 
00115           ball_dir_x_ *= -1.0;
00116           ball_dir_y_ += offset*2.0;
00117 
00118           normalizeVel();
00119 
00120           // limit angle to 45 deg
00121           if ( fabs(ball_dir_y_) > 0.707106781 )
00122           {
00123             ball_dir_x_ = ball_dir_x_ > 0.0 ? 1.0 : -1.0;
00124             ball_dir_y_ = ball_dir_y_ > 0.0 ? 1.0 : -1.0;
00125             normalizeVel();
00126           }
00127 
00128           ball_dx = speed_ * ball_dir_x_;
00129           ball_dy = speed_ * ball_dir_y_;
00130           ball_pos_x_ += t * ball_dx;
00131           ball_pos_y_ += t * ball_dy;
00132         }
00133       }
00134 
00135       // ball hits the left/right border of the playing field
00136       if ( fabs(ball_pos_x_) >= FIELD_WIDTH * 0.5 + 1.5*BORDER_SIZE )
00137       {
00138         reflect ( ball_pos_x_, last_ball_pos_x_, FIELD_WIDTH * 0.5 + 1.5*BORDER_SIZE, t );
00139         ball_pos_x_ -= t * ball_dx;
00140         ball_pos_y_ -= t * ball_dy;
00141         updateBall();
00142 
00143         player_contexts_[1-player].score++;
00144         updateScore();
00145 
00146         server_.applyChanges();
00147         reset();
00148         ros::Duration(1.0).sleep();
00149       }
00150       else
00151       {
00152         updateBall();
00153       }
00154 
00155       last_ball_pos_x_ = ball_pos_x_;
00156       last_ball_pos_y_ = ball_pos_y_;
00157 
00158       // control computer player
00159       if ( !player_contexts_[0].active || !player_contexts_[1].active )
00160       {
00161         int player = player_contexts_[0].active ? 1 : 0;
00162         float delta = ball_pos_y_ - player_contexts_[player].pos;
00163         // limit movement speed
00164         if ( delta > AI_SPEED_LIMIT ) delta = AI_SPEED_LIMIT;
00165         if ( delta < -AI_SPEED_LIMIT ) delta = -AI_SPEED_LIMIT;
00166         setPaddlePos( player, player_contexts_[player].pos + delta );
00167       }
00168 
00169       speed_ += 0.0003;
00170     }
00171 
00172     server_.applyChanges();
00173   }
00174 
00175   void setPaddlePos( unsigned player, float pos )
00176   {
00177     if ( player > 1 )
00178     {
00179       return;
00180     }
00181 
00182     // clamp
00183     if ( pos > (FIELD_HEIGHT - PADDLE_SIZE) * 0.5 )
00184     {
00185       pos = (FIELD_HEIGHT - PADDLE_SIZE) * 0.5;
00186     }
00187     if ( pos < (FIELD_HEIGHT - PADDLE_SIZE) * -0.5 )
00188     {
00189       pos = (FIELD_HEIGHT - PADDLE_SIZE) * -0.5;
00190     }
00191 
00192     player_contexts_[player].pos = pos;
00193 
00194     geometry_msgs::Pose pose;
00195     pose.position.x = (player == 0) ? -PLAYER_X : PLAYER_X;
00196     pose.position.y = pos;
00197 
00198     std::string marker_name = (player == 0) ? "paddle0" : "paddle1";
00199     server_.setPose( marker_name, pose );
00200     server_.setPose( marker_name+"_display", pose );
00201   }
00202 
00203   void processPaddleFeedback( unsigned player, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00204   {
00205     if ( player > 1 )
00206     {
00207       return;
00208     }
00209 
00210     std::string control_marker_name = feedback->marker_name;
00211     geometry_msgs::Pose pose = feedback->pose;
00212 
00213     setPaddlePos( player, pose.position.y );
00214 
00215     if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN )
00216     {
00217       player_contexts_[player].active = true;
00218     }
00219     if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP )
00220     {
00221       player_contexts_[player].active = false;
00222     }
00223   }
00224 
00225   // restart round
00226   void reset()
00227   {
00228     speed_ = 6.0 * UPDATE_RATE;
00229     ball_pos_x_ = 0.0;
00230     ball_pos_y_ = 0.0;
00231     ball_dir_x_ = ball_dir_x_ > 0.0 ? 1.0 : -1.0;
00232     ball_dir_y_ = rand() % 2 ? 1.0 : -1.0;
00233     normalizeVel();
00234   }
00235 
00236   // set length of velocity vector to 1
00237   void normalizeVel()
00238   {
00239     float l = sqrt( ball_dir_x_*ball_dir_x_ + ball_dir_y_*ball_dir_y_ );
00240     ball_dir_x_ /= l;
00241     ball_dir_y_ /= l;
00242   }
00243 
00244   // compute reflection
00245   // returns true if the given limit has been surpassed
00246   // t [0...1] says how much the limit has been surpassed, relative to the distance
00247   // between last_pos and pos
00248   bool reflect( float &pos, float last_pos, float limit, float &t )
00249   {
00250     if ( pos > limit )
00251     {
00252       t = (pos - limit) / (pos - last_pos);
00253       return true;
00254     }
00255     if ( -pos > limit )
00256     {
00257       t = (-pos - limit) / (last_pos - pos);
00258       return true;
00259     }
00260     return false;
00261   }
00262 
00263   // update ball marker
00264   void updateBall()
00265   {
00266     geometry_msgs::Pose pose;
00267     pose.position.x = ball_pos_x_;
00268     pose.position.y = ball_pos_y_;
00269     server_.setPose( "ball", pose );
00270   }
00271 
00272   // update score marker
00273   void updateScore()
00274   {
00275     InteractiveMarker int_marker;
00276     int_marker.header.frame_id = "/base_link";
00277     int_marker.name = "score";
00278 
00279     InteractiveMarkerControl control;
00280     control.always_visible = true;
00281 
00282     Marker marker;
00283     marker.type = Marker::TEXT_VIEW_FACING;
00284     marker.color.r = 1.0;
00285     marker.color.g = 1.0;
00286     marker.color.b = 1.0;
00287     marker.color.a = 1.0;
00288     marker.scale.x = 1.5;
00289     marker.scale.y = 1.5;
00290     marker.scale.z = 1.5;
00291 
00292     std::ostringstream s;
00293     s << player_contexts_[0].score;
00294     marker.text = s.str();
00295     marker.pose.position.y = FIELD_HEIGHT*0.5 + 4.0*BORDER_SIZE;
00296     marker.pose.position.x = -1.0 * ( FIELD_WIDTH * 0.5 + BORDER_SIZE );
00297     control.markers.push_back( marker );
00298 
00299     s.str("");
00300     s << player_contexts_[1].score;
00301     marker.text = s.str();
00302     marker.pose.position.x *= -1;
00303     control.markers.push_back( marker );
00304 
00305     int_marker.controls.push_back( control );
00306 
00307     server_.insert( int_marker );
00308   }
00309 
00310   void makeFieldMarker()
00311   {
00312     InteractiveMarker int_marker;
00313     int_marker.header.frame_id = "/base_link";
00314     int_marker.name = "field";
00315 
00316     InteractiveMarkerControl control;
00317     control.always_visible = true;
00318 
00319     Marker marker;
00320     marker.type = Marker::CUBE;
00321     marker.color.r = 1.0;
00322     marker.color.g = 1.0;
00323     marker.color.b = 1.0;
00324     marker.color.a = 1.0;
00325 
00326     // Top Border
00327     marker.scale.x = FIELD_WIDTH + 6.0 * BORDER_SIZE;
00328     marker.scale.y = BORDER_SIZE;
00329     marker.scale.z = BORDER_SIZE;
00330     marker.pose.position.x = 0;
00331     marker.pose.position.y = FIELD_HEIGHT*0.5 + BORDER_SIZE;
00332     control.markers.push_back( marker );
00333 
00334     // Bottom Border
00335     marker.pose.position.y *= -1;
00336     control.markers.push_back( marker );
00337 
00338     // Left Border
00339     marker.scale.x = BORDER_SIZE;
00340     marker.scale.y = FIELD_HEIGHT + 3.0*BORDER_SIZE;
00341     marker.scale.z = BORDER_SIZE;
00342     marker.pose.position.x = FIELD_WIDTH*0.5 + 2.5*BORDER_SIZE;
00343     marker.pose.position.y = 0;
00344     control.markers.push_back( marker );
00345 
00346     // Right Border
00347     marker.pose.position.x *= -1;
00348     control.markers.push_back( marker );
00349 
00350     // store
00351     int_marker.controls.push_back( control );
00352     server_.insert( int_marker );
00353   }
00354 
00355   void makePaddleMarkers()
00356   {
00357     InteractiveMarker int_marker;
00358     int_marker.header.frame_id = "/base_link";
00359 
00360     // Add a control for moving the paddle
00361     InteractiveMarkerControl control;
00362     control.always_visible = false;
00363     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00364     control.orientation.w = 1;
00365     control.orientation.z = 1;
00366 
00367     // Add a visualization marker
00368     Marker marker;
00369     marker.type = Marker::CUBE;
00370     marker.color.r = 1.0;
00371     marker.color.g = 1.0;
00372     marker.color.b = 1.0;
00373     marker.color.a = 0.0;
00374     marker.scale.x = BORDER_SIZE + 0.1;
00375     marker.scale.y = PADDLE_SIZE + 0.1;
00376     marker.scale.z = BORDER_SIZE + 0.1;
00377     marker.pose.position.z = 0;
00378     marker.pose.position.y = 0;
00379 
00380     control.markers.push_back( marker );
00381 
00382     int_marker.controls.push_back( control );
00383 
00384     // Control for player 1
00385     int_marker.name = "paddle0";
00386     int_marker.pose.position.x = -PLAYER_X;
00387     server_.insert( int_marker );
00388     server_.setCallback( int_marker.name, boost::bind( &PongGame::processPaddleFeedback, this, 0, _1 ) );
00389 
00390     // Control for player 2
00391     int_marker.name = "paddle1";
00392     int_marker.pose.position.x = PLAYER_X;
00393     server_.insert( int_marker );
00394     server_.setCallback( int_marker.name, boost::bind( &PongGame::processPaddleFeedback, this, 1, _1 ) );
00395 
00396     // Make display markers
00397     marker.scale.x = BORDER_SIZE;
00398     marker.scale.y = PADDLE_SIZE;
00399     marker.scale.z = BORDER_SIZE;
00400     marker.color.r = 0.5;
00401     marker.color.a = 1.0;
00402 
00403     control.interaction_mode = InteractiveMarkerControl::NONE;
00404     control.always_visible = true;
00405 
00406     // Display for player 1
00407     int_marker.name = "paddle0_display";
00408     int_marker.pose.position.x = -PLAYER_X;
00409 
00410     marker.color.g = 1.0;
00411     marker.color.b = 0.5;
00412 
00413     int_marker.controls.clear();
00414     control.markers.clear();
00415     control.markers.push_back( marker );
00416     int_marker.controls.push_back( control );
00417     server_.insert( int_marker );
00418 
00419     // Display for player 2
00420     int_marker.name = "paddle1_display";
00421     int_marker.pose.position.x = PLAYER_X;
00422 
00423     marker.color.g = 0.5;
00424     marker.color.b = 1.0;
00425 
00426     int_marker.controls.clear();
00427     control.markers.clear();
00428     control.markers.push_back( marker );
00429     int_marker.controls.push_back( control );
00430     server_.insert( int_marker );
00431   }
00432 
00433   void makeBallMarker()
00434   {
00435     InteractiveMarker int_marker;
00436     int_marker.header.frame_id = "/base_link";
00437 
00438     InteractiveMarkerControl control;
00439     control.always_visible = true;
00440 
00441     // Ball
00442     int_marker.name = "ball";
00443 
00444     control.interaction_mode = InteractiveMarkerControl::NONE;
00445     control.orientation.w = 1;
00446     control.orientation.y = 1;
00447 
00448     Marker marker;
00449     marker.color.r = 1.0;
00450     marker.color.g = 1.0;
00451     marker.color.b = 1.0;
00452     marker.color.a = 1.0;
00453     marker.type = Marker::CYLINDER;
00454     marker.scale.x = BORDER_SIZE;
00455     marker.scale.y = BORDER_SIZE;
00456     marker.scale.z = BORDER_SIZE;
00457     control.markers.push_back( marker );
00458 
00459     int_marker.controls.push_back( control );
00460 
00461     server_.insert( int_marker );
00462   }
00463 
00464   interactive_markers::InteractiveMarkerServer server_;
00465 
00466   ros::Timer game_loop_timer_;
00467 
00468   InteractiveMarker field_marker_;
00469 
00470   struct PlayerContext
00471   {
00472     PlayerContext(): pos(0),active(false),score(0) {}
00473     float pos;
00474     bool active;
00475     int score;
00476   };
00477 
00478   std::vector<PlayerContext> player_contexts_;
00479 
00480   float last_ball_pos_x_;
00481   float last_ball_pos_y_;
00482 
00483   float ball_pos_x_;
00484   float ball_pos_y_;
00485 
00486   float ball_dir_x_;
00487   float ball_dir_y_;
00488   float speed_;
00489 };
00490 
00491 
00492 
00493 int main(int argc, char** argv)
00494 {
00495   ros::init(argc, argv, "pong");
00496 
00497   PongGame pong_game;
00498   ros::spin();
00499   ROS_INFO("Exiting..");
00500 }


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Thu Aug 27 2015 15:40:39