bebop_itl_test.cpp
Go to the documentation of this file.
00001 
00025 #include <string>
00026 #include <numeric>
00027 #include <boost/shared_ptr.hpp>
00028 #include <boost/bind.hpp>
00029 #include <boost/thread/mutex.hpp>
00030 #include <boost/thread/locks.hpp>
00031 #include <boost/function.hpp>
00032 
00033 #include <ros/ros.h>
00034 #include <std_msgs/Empty.h>
00035 #include <geometry_msgs/Twist.h>
00036 #include <sensor_msgs/Image.h>
00037 #include <angles/angles.h>
00038 #include <sensor_msgs/CameraInfo.h>
00039 #include <nav_msgs/Odometry.h>
00040 #include <sensor_msgs/NavSatFix.h>
00041 #include <tf2_ros/transform_listener.h>
00042 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00043 
00044 #include <gtest/gtest.h>
00045 
00046 #include <bebop_msgs/Ardrone3PilotingStateFlyingStateChanged.h>
00047 #include <bebop_msgs/Ardrone3PilotingStateAttitudeChanged.h>
00048 #include <bebop_msgs/Ardrone3CameraStateOrientation.h>
00049 #include <bebop_msgs/Ardrone3PilotingStateFlyingStateChanged.h>
00050 #include <bebop_msgs/Ardrone3PilotingStateSpeedChanged.h>
00051 #include <bebop_msgs/Ardrone3PilotingStateAltitudeChanged.h>
00052 #include <bebop_msgs/CommonCommonStateBatteryStateChanged.h>
00053 
00054 #define TIMED_ASSERT(TIMEOUT, WAIT_UNTIL_TRUE, WAITING_TEXT)                      \
00055 do                                                                                \
00056 {                                                                                 \
00057   const ros::Time start = ros::Time::now();                                       \
00058   while (((ros::Time::now() - start).toSec() < TIMEOUT) && (!(WAIT_UNTIL_TRUE)))  \
00059   {                                                                               \
00060     ROS_INFO_ONCE(WAITING_TEXT);                                                  \
00061     ros::Rate(5.0).sleep();                                                       \
00062   }                                                                               \
00063   ASSERT_TRUE(WAIT_UNTIL_TRUE);                                                   \
00064 }                                                                                 \
00065 while (0)                                                                         \
00066 
00067 namespace bebop_driver
00068 {
00069 namespace util
00070 {
00071 
00072 template<typename T>
00073 class ASyncSub
00074 {
00075 private:
00076   typedef boost::function<void (const boost::shared_ptr<T const>& data)> callback_t;
00077 
00078   ros::NodeHandle nh;
00079   bool active_;
00080   ros::Time last_updated_;
00081   std::string topic_;
00082   std::size_t queue_size_;
00083   callback_t user_callback_;
00084   ros::Subscriber sub_;
00085   boost::shared_ptr<T const> msg_cptr_;
00086   mutable boost::mutex mutex_;
00087 
00088   void cb(const boost::shared_ptr<T const> &msg_cptr)
00089   {
00090     boost::lock_guard<boost::mutex> lock(mutex_);
00091     active_ = true;
00092     last_updated_ = ros::Time::now();
00093     msg_cptr_ = msg_cptr;
00094     if (user_callback_) user_callback_(msg_cptr_);
00095   }
00096 
00097 public:
00098   ASyncSub(ros::NodeHandle& nh,
00099            const std::string& topic,
00100            const std::size_t queue_size,
00101            callback_t user_callback = 0)
00102     : nh(nh), active_(false), last_updated_(0), topic_(topic), user_callback_(user_callback)
00103   {
00104     sub_ = nh.subscribe<T>(topic_, queue_size, boost::bind(&ASyncSub<T>::cb, this, _1));
00105   }
00106 
00107   T GetMsgCopy() const
00108   {
00109     boost::lock_guard<boost::mutex> lock(mutex_);
00110     return *msg_cptr_;
00111   }
00112 
00113   const boost::shared_ptr<T const>& GetMsgConstPtr() const
00114   {
00115     boost::lock_guard<boost::mutex> lock(mutex_);
00116     return msg_cptr_;
00117   }
00118 
00119   // T operator ()() const {return GetMsgCopy();}
00120 
00121   const boost::shared_ptr<T const>& operator()() const
00122   {
00123     // no need to acquire the lock here, since the underlying function does that
00124     return GetMsgConstPtr();
00125   }
00126 
00127   void Deactivate()
00128   {
00129     boost::lock_guard<boost::mutex> lock(mutex_);
00130     active_ = false;
00131   }
00132 
00133   void DeactivateIfOlderThan(const double seconds)
00134   {
00135     if (!IsActive()) return;
00136     if (GetFreshness().toSec() > seconds)
00137     {
00138       ROS_WARN("Information on topic (%s) is older than (%4.2lf) seconds. Deactivating.", topic_.c_str(), seconds);
00139       Deactivate();
00140     }
00141   }
00142 
00143   bool IsActive() const
00144   {
00145     boost::lock_guard<boost::mutex> lock(mutex_);
00146     return active_;
00147   }
00148 
00149   const ros::Time GetLastUpdated() const
00150   {
00151     boost::lock_guard<boost::mutex> lock(mutex_);
00152     return last_updated_;
00153   }
00154 
00155   const ros::Duration GetFreshness() const
00156   {
00157     boost::lock_guard<boost::mutex> lock(mutex_);
00158     return ros::Time::now() - last_updated_;
00159   }
00160 };
00161 
00162 }  // namespace util
00163 
00164 namespace test
00165 {
00166 
00167 void dummy_cb(const bebop_msgs::Ardrone3PilotingStateAttitudeChanged::ConstPtr& msg_ptr)
00168 {
00169   ROS_INFO("Roll: %0.2lf, Pitch: %0.2lf, Yaw: %0.2lf",
00170            msg_ptr->roll, msg_ptr->pitch, msg_ptr->yaw);
00171 }
00172 
00173 class BebopInTheLoopTest: public ::testing::Test
00174 {
00175 protected:
00176   ros::NodeHandle nh_;
00177   boost::shared_ptr<ros::AsyncSpinner> spinner_ptr_;
00178 
00179   ros::Publisher land_pub_;
00180   ros::Publisher cmdvel_pub_;
00181 
00182   std_msgs::Empty em;
00183   geometry_msgs::Twist tw;
00184 
00185   boost::shared_ptr<util::ASyncSub<sensor_msgs::Image> > image_;
00186 
00187 //  boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3PilotingStateAttitudeChanged> > attitude_state_;
00188 
00189   virtual void SetUp()
00190   {
00191     ros::NodeHandle priv_nh("~");
00192 
00193     ROS_INFO("In SetUp()");
00194 
00195     // Common Publishers
00196     land_pub_= nh_.advertise<std_msgs::Empty>("land", 1);
00197     cmdvel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00198 
00199     // Common Subs
00200     image_.reset(new util::ASyncSub<sensor_msgs::Image>(
00201                    nh_, "image_raw", 10));
00202 
00203     spinner_ptr_.reset(new ros::AsyncSpinner(4));
00204     spinner_ptr_->start();
00205 
00206     // Wait 10s for Bebop to come up
00207     ros::Time start = ros::Time::now();
00208 
00209     // Check image_ (when image_ is being published, everything else is ready)
00210     TIMED_ASSERT(15.0, image_->IsActive(), "Waiting for Bebop ...");
00211     TIMED_ASSERT(5.0, land_pub_.getNumSubscribers() > 0, "Waiting for land subscription ...");
00212 
00213     ROS_INFO("End SetUp()");
00214   }
00215 
00216   void StopBebop()
00217   {
00218     tw.linear.x = 0.0;
00219     tw.linear.y = 0.0;
00220     tw.linear.z = 0.0;
00221     tw.angular.z = 0.0;
00222     ROS_WARN("Stopping ...");
00223     cmdvel_pub_.publish(tw);
00224     ros::Duration(1.0).sleep();
00225   }
00226 
00227   virtual void TearDown()
00228   {
00229     ROS_INFO("In teardown()");
00230     StopBebop();
00231     std_msgs::Empty em;
00232     land_pub_.publish(em);
00233     ros::Rate(ros::Duration(2.0)).sleep();
00234     spinner_ptr_->stop();
00235   }
00236 
00237 };
00238 
00239 /*
00240  * Parrot's coordinate system for velocities:
00241  * +x: forward
00242  * +y: right
00243  * +z: downward
00244  *
00245  * (not ROS compatible)
00246  *
00247  * odom on the other hand is REP-103 compatible
00248  */
00249 TEST_F(BebopInTheLoopTest, Piloting)
00250 {
00251   boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3PilotingStateFlyingStateChanged> >
00252       flying_state(new util::ASyncSub<bebop_msgs::Ardrone3PilotingStateFlyingStateChanged>(
00253                       nh_, "states/ardrone3/PilotingState/FlyingStateChanged", 10));
00254 
00255   boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3PilotingStateSpeedChanged> >
00256       speed_state(new util::ASyncSub<bebop_msgs::Ardrone3PilotingStateSpeedChanged>(
00257                       nh_, "states/ardrone3/PilotingState/SpeedChanged", 10));
00258 
00259   boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3PilotingStateAltitudeChanged> >
00260       alt_state(new util::ASyncSub<bebop_msgs::Ardrone3PilotingStateAltitudeChanged>(
00261                       nh_, "states/ardrone3/PilotingState/AltitudeChanged", 10));
00262 
00263   boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3PilotingStateAttitudeChanged> >
00264       att_state(new util::ASyncSub<bebop_msgs::Ardrone3PilotingStateAttitudeChanged>(
00265                       nh_, "states/ardrone3/PilotingState/AttitudeChanged", 10));
00266 
00267   // The problem with the battery state is that it is only updated when its values changes,
00268   // this is more likely to happen when flying than sitting on the ground
00269   boost::shared_ptr<util::ASyncSub<bebop_msgs::CommonCommonStateBatteryStateChanged> >
00270       bat_state(new util::ASyncSub<bebop_msgs::CommonCommonStateBatteryStateChanged>(
00271                       nh_, "states/common/CommonState/BatteryStateChanged", 10));
00272 
00273   boost::shared_ptr<util::ASyncSub<nav_msgs::Odometry> >
00274       odom(new util::ASyncSub<nav_msgs::Odometry>(
00275              nh_, "odom", 10));
00276 
00277   boost::shared_ptr<util::ASyncSub<sensor_msgs::NavSatFix> >
00278       gps(new util::ASyncSub<sensor_msgs::NavSatFix>(
00279             nh_, "fix", 10));
00280 
00281   ros::Publisher takeoff_pub =  nh_.advertise<std_msgs::Empty>("takeoff", 1);
00282   ros::Publisher reset_pub = nh_.advertise<std_msgs::Empty>("reset", 1);
00283 
00284   // Wait 5s time for connections to establish
00285   TIMED_ASSERT(5.0, takeoff_pub.getNumSubscribers() > 0, "Waiting for takeoff subscription ...");
00286   TIMED_ASSERT(5.0, cmdvel_pub_.getNumSubscribers() > 0, "Waiting for cmd_vel subscription ...");
00287   TIMED_ASSERT(5.0, reset_pub.getNumSubscribers() > 0, "Waiting for reset subscription ...");
00288 
00289   ROS_WARN("Taking off ...");
00290   takeoff_pub.publish(em);
00291   TIMED_ASSERT(10.0,
00292                flying_state->IsActive() && flying_state->GetMsgCopy().state ==
00293                  bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_hovering,
00294                "Waiting for takeoff to finish ..."
00295                );
00296 
00297   TIMED_ASSERT(5.0, speed_state->IsActive(), "Waiting for Speed measurements ...");
00298   TIMED_ASSERT(5.0, alt_state->IsActive(), "Waiting for alt measurements ...");
00299   TIMED_ASSERT(5.0, att_state->IsActive(), "Waiting for attitude measurements ...");
00300   TIMED_ASSERT(5.0, odom->IsActive(), "Waiting for odom ...");
00301   TIMED_ASSERT(5.0, gps->IsActive(), "Waiting for GPS ...");
00302 
00303   // TODO(mani-monaj): Use proper values for pitch/roll test thresholds based on cmd_vel
00304 
00305   tw.linear.x = 0.4;
00306   tw.linear.y = 0.0;
00307   tw.linear.z = 0.0;
00308   tw.angular.z = 0.0;
00309   ROS_WARN("Moving forwared ...");
00310   cmdvel_pub_.publish(tw);
00311 
00312   TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().pitch) < -2.5 , "Measuring pitch ...");
00313   TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.x > 0.2),
00314                "Measuring the forward and lateral velocities from odom ...");
00315 
00316   StopBebop();
00317   TIMED_ASSERT(5.0,
00318                (fabs(speed_state->GetMsgCopy().speedX) < 0.05) &&
00319                (fabs(speed_state->GetMsgCopy().speedY) < 0.05) &&
00320                (fabs(speed_state->GetMsgCopy().speedZ) < 0.05)
00321                , "Measuring the speed vector ...");
00322 
00323   TIMED_ASSERT(5.0,
00324                (fabs(odom->GetMsgCopy().twist.twist.linear.x) < 0.05) &&
00325                (fabs(odom->GetMsgCopy().twist.twist.linear.y) < 0.05) &&
00326                (fabs(odom->GetMsgCopy().twist.twist.linear.z) < 0.05)
00327                , "Measuring the speed vector from odom...");
00328 
00329 
00330   tw.linear.x = -0.4;
00331   tw.linear.y = 0.0;
00332   tw.linear.z = 0.0;
00333   tw.angular.z = 0.0;
00334   ROS_WARN("Moving Backward ...");
00335   cmdvel_pub_.publish(tw);
00336 
00337   TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().pitch) > 2.5, "Measuring pitch ...");
00338   TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.x < -0.2),
00339                "Measuring the forward and lateral velocities from odom ...");
00340   StopBebop();
00341 
00342   tw.linear.x = 0.0;
00343   tw.linear.y = 0.4;
00344   tw.linear.z = 0.0;
00345   tw.angular.z = 0.0;
00346   ROS_WARN("Moving left ...");
00347   cmdvel_pub_.publish(tw);
00348   TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().roll) < -2.5, "Measuring roll ...");
00349   TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.y > 0.1),
00350                "Measuring the forward and lateral velocities from odom ...");
00351 
00352   StopBebop();
00353 
00354   tw.linear.x = 0.0;
00355   tw.linear.y = -0.4;
00356   tw.linear.z = 0.0;
00357   tw.angular.z = 0.0;
00358   ROS_WARN("Moving right ...");
00359   cmdvel_pub_.publish(tw);
00360   TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().roll) > 2.5, "Measuring roll ...");
00361   TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.y < -0.1),
00362                "Measuring the forward and lateral velocities from odom ...");
00363   StopBebop();
00364 
00365   double alt_start, yaw_start, alt_start_odom, yaw_start_odom;
00366   // Make sure altitude is fresh
00367   ASSERT_LT(alt_state->GetFreshness().toSec(), 0.5);
00368   ASSERT_LT(odom->GetFreshness().toSec(), 0.5);
00369 
00370   alt_start = alt_state->GetMsgCopy().altitude;
00371   alt_start_odom = odom->GetMsgCopy().pose.pose.position.z;
00372   tw.linear.x = 0.0;
00373   tw.linear.y = 0.0;
00374   tw.linear.z = 0.2;
00375   tw.angular.z = 0.0;
00376   ROS_WARN("Ascending for 0.5m ...");
00377   cmdvel_pub_.publish(tw);
00378   TIMED_ASSERT(10.0,
00379                ((alt_state->GetMsgCopy().altitude - alt_start) >= 0.5) &&
00380                ((odom->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.5) &&
00381                (speed_state->GetMsgCopy().speedZ < -0.05) &&
00382                (odom->GetMsgCopy().twist.twist.linear.z > 0.05),
00383                "Measuring altitude, speed.z and vertical velocity from odom ...");
00384 
00385   StopBebop();
00386 
00387   // Make sure altitude is fresh
00388   ASSERT_LT(alt_state->GetFreshness().toSec(), 0.5);
00389   ASSERT_LT(odom->GetFreshness().toSec(), 0.5);
00390   alt_start = alt_state->GetMsgCopy().altitude;
00391   alt_start_odom = odom->GetMsgCopy().pose.pose.position.z;
00392   tw.linear.x = 0.0;
00393   tw.linear.y = 0.0;
00394   tw.linear.z = -0.2;
00395   tw.angular.z = 0.0;
00396   ROS_WARN("Descending for 0.5m ...");
00397   cmdvel_pub_.publish(tw);
00398   TIMED_ASSERT(10.0,
00399                ((alt_state->GetMsgCopy().altitude - alt_start) <= -0.5) &&
00400                ((odom->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.5) &&
00401                (speed_state->GetMsgCopy().speedZ > 0.05) &&
00402                (odom->GetMsgCopy().twist.twist.linear.z < -0.05),
00403                "Measuring altitude, speed.z and vertical velocity from odom ...");
00404 
00405   StopBebop();
00406 
00407   // Make sure the attitude is fresh
00408   ASSERT_LT(att_state->GetFreshness().toSec(), 0.5);
00409   ASSERT_LT(odom->GetFreshness().toSec(), 0.5);
00410   yaw_start = att_state->GetMsgCopy().yaw;
00411   tw.linear.x = 0.0;
00412   tw.linear.y = 0.0;
00413   tw.linear.z = 0.0;
00414   tw.angular.z = -0.5;
00415   ROS_WARN("Rotating CW for 90 degrees ...");
00416   cmdvel_pub_.publish(tw);
00417 
00418   // TODO(mani-monaj): Add yaw
00419   TIMED_ASSERT(10.0,
00420                angles::normalize_angle(att_state->GetMsgCopy().yaw - yaw_start) >= 0.5 * 3.141596,
00421                "Measuring Yaw");
00422 
00423   StopBebop();
00424 
00425   // Make sure the attitude is fresh
00426   ASSERT_LT(att_state->GetFreshness().toSec(), 0.5);
00427   yaw_start = att_state->GetMsgCopy().yaw;
00428   tw.linear.x = 0.0;
00429   tw.linear.y = 0.0;
00430   tw.linear.z = 0.0;
00431   tw.angular.z = 0.5;
00432   ROS_WARN("Rotating CCW for 90 degrees ...");
00433   cmdvel_pub_.publish(tw);
00434   TIMED_ASSERT(10.0,
00435                angles::normalize_angle(att_state->GetMsgCopy().yaw - yaw_start) <= -0.5 * 3.141596,
00436                "Measuring Yaw");
00437 
00438   StopBebop();
00439 
00440   /* By this time, battery state must have been changed (even on Bebop 2) */
00441   TIMED_ASSERT(20.0, bat_state->IsActive(), "Measuring battery ...");
00442   const uint8_t bat_percent = bat_state->GetMsgCopy().percent;
00443 
00444   tw.linear.x = 0.0;
00445   tw.linear.y = 0.0;
00446   tw.linear.z = 0.0;
00447   tw.angular.z = 0.0;
00448   ROS_WARN("Stop ...");
00449   cmdvel_pub_.publish(tw);
00450 
00451   ROS_WARN("Landing ...");
00452   land_pub_.publish(em);
00453 
00454   TIMED_ASSERT(10.0,
00455                flying_state->IsActive() && flying_state->GetMsgCopy().state ==
00456                  bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_landed,
00457                "Waiting for land to finish..."
00458                );
00459 
00460   // emergency state is transient (unlike ardrone), we may miss the signal
00461 //  ROS_WARN("Emergency ...");
00462 //  reset_pub.publish(em);
00463 
00464 //  TIMED_ASSERT(5.0,
00465 //               flying_state->IsActive() && flying_state->GetMsgCopy().state ==
00466 //                 bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_emergency,
00467 //               "Waiting for reset to happen..."
00468 //               );
00469 
00470   ASSERT_GE(bat_percent - bat_state->GetMsgCopy().percent, 0);
00471 }
00472 
00473 
00474 // TODO(mani-monaj): Add value tests
00475 TEST_F(BebopInTheLoopTest, StaticTFTest)
00476 {
00477   tf2_ros::Buffer tf_buffer;
00478   tf2_ros::TransformListener tf_listener(tf_buffer, nh_);
00479 
00480   // We manually set the odom_frame_id to odom_custom in the launch file, so the following should throw
00481   ASSERT_THROW(tf_buffer.lookupTransform("odom", "base_link", ros::Time(0), ros::Duration(2.0)), tf2::TransformException);
00482   try
00483   {
00484     geometry_msgs::TransformStamped odom_to_base = tf_buffer.lookupTransform("odom_custom", "base_link", ros::Time(0), ros::Duration(5.0));
00485 
00486 //    ASSERT_TRUE((fabs(odom_to_base.transform.translation.x) > 0.0) &&
00487 //                (fabs(odom_to_base.transform.translation.y) > 0.0) &&
00488 //                (fabs(odom_to_base.transform.translation.z) > 0.0));
00489 
00490     geometry_msgs::TransformStamped base_to_optical = tf_buffer.lookupTransform(
00491           "base_link", "camera_optical", ros::Time(0), ros::Duration(5.0));
00492 
00493 //    geometry_msgs::TransformStamped camera_to_optical = tf_buffer.lookupTransform(
00494 //          "camera_base_link", "camera_optical", ros::Time(0), ros::Duration(5.0));
00495 
00496 //    tf2::Transform camera_to_optical_tf;
00497 //    double r, p, y;
00498 //    tf2::convert(camera_to_optical.transform, camera_to_optical_tf);
00499 
00500 //    tf2::Matrix3x3(camera_to_optical_tf.getRotation()).getRPY(r, p, y);
00501 //    ASSERT_DOUBLE_EQ(r, 0.0);
00502 //    ASSERT_DOUBLE_EQ(p, 0.0);
00503 //    ASSERT_DOUBLE_EQ(y, 0.0);
00504 //    ASSERT_DOUBLE_EQ(camera_to_optical_tf.getOrigin().length(), 0.0);
00505   }
00506   catch (const tf2::TransformException& ex)
00507   {
00508     ASSERT_TRUE(false) << ex.what();
00509   }
00510 }
00511 
00512 TEST_F(BebopInTheLoopTest, GPSTest)
00513 {
00514   boost::shared_ptr<util::ASyncSub<sensor_msgs::NavSatFix> > fix_sub(
00515         new util::ASyncSub<sensor_msgs::NavSatFix>(nh_, "fix", 10));
00516 
00517   TIMED_ASSERT(2.0, fix_sub->IsActive() , "Waiting for the GPS topic ...");
00518 
00519   sensor_msgs::NavSatFix gps = fix_sub->GetMsgCopy();
00520 
00521   ASSERT_GT(gps.header.frame_id.length(), 0);
00522   ASSERT_NE(gps.status.service, 0);
00523 
00524   const bool is_valid = gps.status.status >= sensor_msgs::NavSatStatus::STATUS_FIX;
00525 
00526   if (!is_valid)
00527   {
00528     ASSERT_DOUBLE_EQ(gps.latitude, 500.0);
00529     ASSERT_DOUBLE_EQ(gps.longitude, 500.0);
00530     ASSERT_DOUBLE_EQ(gps.altitude, 500.0);
00531   }
00532   else
00533   {
00534     ASSERT_NE(gps.latitude, 500.0);
00535     ASSERT_NE(gps.longitude, 500.0);
00536     ASSERT_NE(gps.altitude, 500.0);
00537   }
00538 }
00539 
00540 TEST_F(BebopInTheLoopTest, VisionTest)
00541 {
00542   boost::shared_ptr<util::ASyncSub<sensor_msgs::CameraInfo> > camera_(
00543         new util::ASyncSub<sensor_msgs::CameraInfo>(nh_, "camera_info", 10));
00544 
00545   TIMED_ASSERT(2.0, image_->IsActive() && camera_->IsActive() , "Waiting for front video stream ...");
00546 
00547   sensor_msgs::Image img = image_->GetMsgCopy();
00548 
00549   const std::size_t sz = img.step * img.height;
00550 
00551   ASSERT_TRUE(img.header.frame_id.length());
00552   ASSERT_GT(img.width, 0);
00553   ASSERT_GT(img.height, 0);
00554   ASSERT_GT(img.header.stamp.toSec(), 0.0);
00555   ASSERT_GT(sz, 0);
00556 
00557   ASSERT_GE(std::accumulate(img.data.begin(), img.data.end(), 0.0) , 0.0);
00558 
00559   sensor_msgs::CameraInfo cam_msg = camera_->GetMsgCopy();
00560 
00561   ASSERT_EQ(cam_msg.width, img.width);
00562   ASSERT_EQ(cam_msg.height, img.height);
00563   ASSERT_EQ(cam_msg.header.frame_id, img.header.frame_id);
00564   ASSERT_GT(cam_msg.header.stamp.toSec(), 0.0);
00565   ASSERT_FALSE(cam_msg.distortion_model.empty());
00566 
00567   ASSERT_FALSE(cam_msg.D.empty());
00568   ASSERT_FALSE(cam_msg.K.empty());
00569   ASSERT_FALSE(cam_msg.R.empty());
00570   ASSERT_FALSE(cam_msg.P.empty());
00571 
00572   ASSERT_GT(fabs(cam_msg.D[0]), 0.0);
00573   ASSERT_GT(fabs(cam_msg.K[0]), 0.0);
00574   ASSERT_GT(fabs(cam_msg.R[0]), 0.0);
00575   ASSERT_GT(fabs(cam_msg.P[0]), 0.0);
00576 }
00577 
00578 TEST_F(BebopInTheLoopTest, CameraMoveTest)
00579 {
00580   boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3CameraStateOrientation> >
00581       camera_state(new util::ASyncSub<bebop_msgs::Ardrone3CameraStateOrientation>(
00582                       nh_, "states/ardrone3/CameraState/Orientation", 10));
00583 
00584   ros::Publisher twist_pub =  nh_.advertise<geometry_msgs::Twist>("camera_control", 1);
00585   TIMED_ASSERT(5.0, twist_pub.getNumSubscribers() > 0, "Waiting for camera control subscription ...");
00586 
00587   // tilt
00588   tw.angular.y = 13;
00589   // pan
00590   tw.angular.z = -16;
00591 
00592   ROS_WARN("Moving the virtual camera");
00593   twist_pub.publish(tw);
00594   ros::Rate(ros::Duration(2)).sleep();
00595 
00596   TIMED_ASSERT(5.0, camera_state->IsActive(), "Waiting for camera state ...");
00597 
00598   ASSERT_EQ(camera_state->GetMsgCopy().tilt, 13);
00599   ASSERT_EQ(camera_state->GetMsgCopy().pan, -16);
00600 
00601   // TODO(mani-monaj): Add TF tests
00602 }
00603 
00604 TEST_F(BebopInTheLoopTest, DISABLED_FlatTrimTest)
00605 {
00606   boost::shared_ptr<util::ASyncSub<bebop_msgs::Ardrone3PilotingStateFlyingStateChanged> >
00607       flattrim_state(new util::ASyncSub<bebop_msgs::Ardrone3PilotingStateFlyingStateChanged>(
00608                       nh_, "states/ardrone3/PilotingState/FlyingStateChanged", 10));
00609     ros::Publisher flattrim_pub =  nh_.advertise<std_msgs::Empty>("flattrim", 1);
00610 
00611     // Wait 5s time for connection to establish
00612     TIMED_ASSERT(5.0, flattrim_pub.getNumSubscribers() > 0, "Waiting for flattrim subscription ...");
00613 
00614     ROS_WARN("Flat trim ...");
00615     flattrim_pub.publish(em);
00616 
00617     TIMED_ASSERT(10.0,
00618                  flattrim_state->IsActive(),
00619                  "Waiting for flattrim ack ..."
00620                  );
00621 }
00622 }  // namespace test
00623 }  // namespace bebop_driver
00624 
00625 
00626 
00627 int main(int argc, char* argv[])
00628 {
00629   ros::init(argc, argv, "bebop_itl_test");
00630   ros::NodeHandle keep_me_awake;
00631   testing::InitGoogleTest(&argc, argv);
00632   const int res = RUN_ALL_TESTS();
00633   ros::shutdown();
00634   return res;
00635 }


bebop_driver
Author(s): Mani Monajjemi
autogenerated on Sat Jun 8 2019 20:37:45