rviz: rviz: interactive_marker_client

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 #include <gtest/gtest.h>
00031 #include <ros/ros.h>
00032 #include <rviz/default_plugin/interactive_markers/interactive_marker_client.h>
00033 
00034 class MockReceiver: public rviz::InteractiveMarkerReceiver
00035 {
00036 public:
00037   std::map<std::string, double> marks_;
00038 //  std::map<std::string, std::string> status_;
00039   std::string calls_;
00040 
00041   virtual void processMarkerChanges( const std::vector<visualization_msgs::InteractiveMarker>* markers = NULL,
00042                                      const std::vector<visualization_msgs::InteractiveMarkerPose>* poses = NULL,
00043                                      const std::vector<std::string>* erases = NULL )
00044     {
00045       if( markers != NULL )
00046       {
00047         std::vector<visualization_msgs::InteractiveMarker>::const_iterator marker_it;
00048         for( marker_it = markers->begin(); marker_it != markers->end(); marker_it++ )
00049         {
00050           marks_[ marker_it->name ] = marker_it->pose.position.x;
00051         }
00052       }
00053 
00054       if( poses != NULL )
00055       {
00056         std::vector<visualization_msgs::InteractiveMarkerPose>::const_iterator pose_it;
00057         for( pose_it = poses->begin(); pose_it != poses->end(); pose_it++ )
00058         {
00059           marks_[ pose_it->name ] = pose_it->pose.position.x;
00060         }
00061       }
00062 
00063       if( erases != NULL )
00064       {
00065         std::vector<std::string>::const_iterator erase_it;
00066         for( erase_it = erases->begin(); erase_it != erases->end(); erase_it++ )
00067         {
00068           std::map<std::string, double>::iterator find_it = marks_.find( *erase_it );
00069           if( find_it != marks_.end() )
00070           {
00071             marks_.erase( find_it );
00072           }
00073         }
00074       }
00075 
00076       calls_ += "processMarkerChanges ";
00077     }
00078 
00079   virtual void clearMarkers()
00080     {
00081       marks_.clear();
00082       calls_ += "clearMarkers ";
00083     }
00084 
00085   virtual bool subscribeToInit()
00086     {
00087       calls_ += "subscribeToInit ";
00088       return true;
00089     }
00090 
00091   virtual void unsubscribeFromInit()
00092     {
00093       calls_ += "unsubscribeFromInit ";
00094     }
00095 
00096   virtual void setStatusOk(const std::string& name, const std::string& text)
00097     {
00098     }
00099 
00100   virtual void setStatusWarn(const std::string& name, const std::string& text)
00101     {
00102     }
00103 
00104   virtual void setStatusError(const std::string& name, const std::string& text)
00105     {
00106     }
00107 
00108   void clearCalls()
00109     {
00110       calls_ = "";
00111     }
00112 };
00113 
00114 visualization_msgs::InteractiveMarkerInit::Ptr makeInit( const std::string& server_name,
00115                                                          uint64_t seq_num,
00116                                                          const std::string& marker_name,
00117                                                          double x_value )
00118 {
00119   visualization_msgs::InteractiveMarkerInit::Ptr msg( new visualization_msgs::InteractiveMarkerInit() );
00120   msg->server_id = server_name;
00121   msg->seq_num = seq_num;
00122   msg->markers.resize( 1 );
00123   msg->markers[0].name = marker_name;
00124   msg->markers[0].pose.position.x = x_value;
00125   return msg;
00126 }
00127 
00128 visualization_msgs::InteractiveMarkerUpdate::Ptr makeUpdateMarker( const std::string& server_name,
00129                                                                    uint64_t seq_num,
00130                                                                    const std::string& marker_name,
00131                                                                    double x_value )
00132 {
00133   visualization_msgs::InteractiveMarkerUpdate::Ptr msg( new visualization_msgs::InteractiveMarkerUpdate() );
00134   msg->server_id = server_name;
00135   msg->seq_num = seq_num;
00136   msg->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00137   msg->markers.resize( 1 );
00138   msg->markers[0].name = marker_name;
00139   msg->markers[0].pose.position.x = x_value;
00140   return msg;
00141 }
00142 
00143 visualization_msgs::InteractiveMarkerUpdate::Ptr makeUpdatePose( const std::string& server_name,
00144                                                                  uint64_t seq_num,
00145                                                                  const std::string& marker_name,
00146                                                                  double x_value )
00147 {
00148   visualization_msgs::InteractiveMarkerUpdate::Ptr msg( new visualization_msgs::InteractiveMarkerUpdate() );
00149   msg->server_id = server_name;
00150   msg->seq_num = seq_num;
00151   msg->type = visualization_msgs::InteractiveMarkerUpdate::UPDATE;
00152   msg->poses.resize( 1 );
00153   msg->poses[0].name = marker_name;
00154   msg->poses[0].pose.position.x = x_value;
00155   return msg;
00156 }
00157 
00158 visualization_msgs::InteractiveMarkerUpdate::Ptr makeKeepAlive( const std::string& server_name,
00159                                                                 uint64_t seq_num )
00160 {
00161   visualization_msgs::InteractiveMarkerUpdate::Ptr msg( new visualization_msgs::InteractiveMarkerUpdate() );
00162   msg->server_id = server_name;
00163   msg->seq_num = seq_num;
00164   msg->type = visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE;
00165   return msg;
00166 }
00167 
00168 TEST(InteractiveMarkerClient, normal_startup)
00169 {
00170   MockReceiver rcvr;
00171   rviz::InteractiveMarkerClient client( &rcvr );
00172 
00173   client.clear();
00174   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00175   rcvr.clearCalls();
00176 
00177   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00178 
00179   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00180   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00181   rcvr.clearCalls();
00182 
00183   client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00184 
00185   EXPECT_EQ( "unsubscribeFromInit processMarkerChanges ", rcvr.calls_ );
00186   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00187   rcvr.clearCalls();
00188 
00189   client.processMarkerUpdate( makeUpdatePose( "s1", 3, "m1", 1.3 ));
00190 
00191   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00192   EXPECT_EQ( 1.3, rcvr.marks_["m1"] );
00193   rcvr.clearCalls();
00194 }
00195 
00196 TEST(InteractiveMarkerClient, normal_keepalive)
00197 {
00198   MockReceiver rcvr;
00199   rviz::InteractiveMarkerClient client( &rcvr );
00200 
00201   client.clear();
00202   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00203   rcvr.clearCalls();
00204 
00205   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00206 
00207   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00208   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00209   rcvr.clearCalls();
00210 
00211   client.processMarkerUpdate( makeKeepAlive( "s1", 1 ));
00212 
00213   EXPECT_EQ( "unsubscribeFromInit ", rcvr.calls_ );
00214   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00215   rcvr.clearCalls();
00216 
00217   client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00218 
00219   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00220   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00221   rcvr.clearCalls();
00222 }
00223 
00224 TEST(InteractiveMarkerClient, update_first)
00225 {
00226   MockReceiver rcvr;
00227   rviz::InteractiveMarkerClient client( &rcvr );
00228 
00229   client.clear();
00230   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00231   rcvr.clearCalls();
00232 
00233   client.processMarkerUpdate( makeUpdatePose( "s1", 1, "m1", 1.1 ));
00234 
00235   EXPECT_EQ( "", rcvr.calls_ );
00236   EXPECT_EQ( 0, rcvr.marks_["m1"] );
00237   rcvr.clearCalls();
00238 
00239   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.2 ));
00240 
00241   EXPECT_EQ( "processMarkerChanges unsubscribeFromInit ", rcvr.calls_ );
00242   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00243   rcvr.clearCalls();
00244 }
00245 
00246 TEST(InteractiveMarkerClient, good_keepalive_first)
00247 {
00248   MockReceiver rcvr;
00249   rviz::InteractiveMarkerClient client( &rcvr );
00250 
00251   client.clear();
00252   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00253   rcvr.clearCalls();
00254 
00255   client.processMarkerUpdate( makeKeepAlive( "s1", 1 ));
00256 
00257   EXPECT_EQ( "", rcvr.calls_ );
00258   rcvr.clearCalls();
00259 
00260   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.2 ));
00261 
00262   EXPECT_EQ( "processMarkerChanges unsubscribeFromInit ", rcvr.calls_ );
00263   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00264   rcvr.clearCalls();
00265 }
00266 
00267 TEST(InteractiveMarkerClient, later_keepalive_first)
00268 {
00269   MockReceiver rcvr;
00270   rviz::InteractiveMarkerClient client( &rcvr );
00271 
00272   client.clear();
00273   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00274   rcvr.clearCalls();
00275 
00276   client.processMarkerUpdate( makeKeepAlive( "s1", 2 ));
00277 
00278   EXPECT_EQ( "", rcvr.calls_ );
00279   rcvr.clearCalls();
00280 
00281   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00282 
00283   EXPECT_EQ( "", rcvr.calls_ );
00284   EXPECT_EQ( 0, rcvr.marks_["m1"] );
00285   rcvr.clearCalls();
00286 
00287   client.processMarkerInit( makeInit( "s1", 2, "m1", 1.2 ));
00288 
00289   EXPECT_EQ( "processMarkerChanges unsubscribeFromInit ", rcvr.calls_ );
00290   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00291   rcvr.clearCalls();
00292 }
00293 
00294 // This tests that updates which arrive before an appropriate init
00295 // message get queued up and replayed, and that init messages which
00296 // are too early to initialize the queued updates are dropped.
00297 TEST(InteractiveMarkerClient, queued_updates_second_init)
00298 {
00299   MockReceiver rcvr;
00300   rviz::InteractiveMarkerClient client( &rcvr );
00301 
00302   client.clear();
00303   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00304   rcvr.clearCalls();
00305 
00306   client.processMarkerUpdate( makeUpdatePose( "s1", 3, "m3", 3.1 ));
00307 
00308   EXPECT_EQ( "", rcvr.calls_ );
00309   EXPECT_EQ( 0, rcvr.marks_["m3"] );
00310   rcvr.clearCalls();
00311 
00312   client.processMarkerUpdate( makeUpdatePose( "s1", 4, "m4", 4.1 ));
00313 
00314   EXPECT_EQ( "", rcvr.calls_ );
00315   EXPECT_EQ( 0, rcvr.marks_["m3"] );
00316   EXPECT_EQ( 0, rcvr.marks_["m4"] );
00317   rcvr.clearCalls();
00318 
00319   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00320 
00321   EXPECT_EQ( "", rcvr.calls_ );
00322   EXPECT_EQ( 0, rcvr.marks_["m1"] );
00323   EXPECT_EQ( 0, rcvr.marks_["m3"] );
00324   EXPECT_EQ( 0, rcvr.marks_["m4"] );
00325   rcvr.clearCalls();
00326 
00327   client.processMarkerInit( makeInit( "s1", 2, "m2", 2.1 ));
00328 
00329   EXPECT_EQ( "processMarkerChanges unsubscribeFromInit processMarkerChanges processMarkerChanges ", rcvr.calls_ );
00330   EXPECT_EQ( 0, rcvr.marks_["m1"] );
00331   EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00332   EXPECT_EQ( 3.1, rcvr.marks_["m3"] );
00333   EXPECT_EQ( 4.1, rcvr.marks_["m4"] );
00334   rcvr.clearCalls();
00335 }
00336 
00337 TEST(InteractiveMarkerClient, normal_two_server)
00338 {
00339   MockReceiver rcvr;
00340   rviz::InteractiveMarkerClient client( &rcvr );
00341 
00342   client.clear();
00343   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00344   rcvr.clearCalls();
00345 
00346   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00347 
00348   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00349   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00350   rcvr.clearCalls();
00351 
00352   client.processMarkerInit( makeInit( "s2", 1, "m2", 2.1 ));
00353 
00354   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00355   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00356   EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00357   rcvr.clearCalls();
00358 
00359   client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00360 
00361   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00362   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00363   EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00364   rcvr.clearCalls();
00365 
00366   client.processMarkerUpdate( makeUpdatePose( "s2", 2, "m2", 2.2 ));
00367 
00368   EXPECT_EQ( "unsubscribeFromInit processMarkerChanges ", rcvr.calls_ );
00369   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00370   EXPECT_EQ( 2.2, rcvr.marks_["m2"] );
00371   rcvr.clearCalls();
00372 }
00373 
00374 TEST(InteractiveMarkerClient, two_server_one_slow)
00375 {
00376   MockReceiver rcvr;
00377   rviz::InteractiveMarkerClient client( &rcvr );
00378 
00379   client.clear();
00380   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00381   rcvr.clearCalls();
00382 
00383   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00384 
00385   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00386   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00387   rcvr.clearCalls();
00388 
00389   client.processMarkerInit( makeInit( "s2", 1, "m2", 2.1 ));
00390 
00391   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00392   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00393   EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00394   rcvr.clearCalls();
00395 
00396   client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00397 
00398   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00399   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00400   EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00401   rcvr.clearCalls();
00402 
00403   // Init corresponding to update just received, because we can't
00404   // unsubscribe from init yet.  Should be ignored.
00405   client.processMarkerInit( makeInit( "s1", 2, "m1", 1.2 ));
00406 
00407   EXPECT_EQ( "", rcvr.calls_ );
00408   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00409   EXPECT_EQ( 2.1, rcvr.marks_["m2"] );
00410   rcvr.clearCalls();
00411 
00412   client.processMarkerUpdate( makeUpdatePose( "s2", 2, "m2", 2.2 ));
00413 
00414   EXPECT_EQ( "unsubscribeFromInit processMarkerChanges ", rcvr.calls_ );
00415   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00416   EXPECT_EQ( 2.2, rcvr.marks_["m2"] );
00417   rcvr.clearCalls();
00418 }
00419 
00420 TEST(InteractiveMarkerClient, init_init_update)
00421 {
00422   MockReceiver rcvr;
00423   rviz::InteractiveMarkerClient client( &rcvr );
00424 
00425   client.clear();
00426   EXPECT_EQ( "subscribeToInit ", rcvr.calls_ );
00427   rcvr.clearCalls();
00428 
00429   client.processMarkerInit( makeInit( "s1", 1, "m1", 1.1 ));
00430 
00431   EXPECT_EQ( "processMarkerChanges ", rcvr.calls_ );
00432   EXPECT_EQ( 1.1, rcvr.marks_["m1"] );
00433   rcvr.clearCalls();
00434 
00435   client.processMarkerInit( makeInit( "s1", 2, "m1", 1.2 ));
00436 
00437   EXPECT_EQ( "clearMarkers processMarkerChanges ", rcvr.calls_ );
00438   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00439   rcvr.clearCalls();
00440 
00441   client.processMarkerUpdate( makeUpdatePose( "s1", 2, "m1", 1.2 ));
00442 
00443   EXPECT_EQ( "unsubscribeFromInit ", rcvr.calls_ );
00444   EXPECT_EQ( 1.2, rcvr.marks_["m1"] );
00445   rcvr.clearCalls();
00446 }
00447 
00448 
00449 int main(int argc, char **argv){
00450   testing::InitGoogleTest(&argc, argv);
00451   ros::Time::init(); //needed for ros::TIme::now()
00452   return RUN_ALL_TESTS();
00453 }
interactive_marker_client_test.cpp
