panorama.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Yujin Robot.
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 Yujin Robot 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 
00040 #include <cmath>
00041 #include <iostream>
00042 #include <eigen3/Eigen/Core>
00043 #include <eigen3/Eigen/Geometry>
00044 #include <turtlebot_panorama/panorama.h>
00045 
00046 namespace turtlebot_panorama
00047 {
00048 
00049 PanoApp::PanoApp() : nh(), priv_nh("~")
00050 {
00051   std::string name = ros::this_node::getName();
00052 
00053   ros::param::param<int>("~default_mode", default_mode, 1);
00054   ros::param::param<double>("~default_pano_angle", default_pano_angle, (2 * M_PI));
00055   ros::param::param<double>("~default_snap_interval", default_snap_interval, 2.0);
00056   ros::param::param<double>("~default_rotation_velocity", default_rotation_velocity, 0.3);
00057 
00058   ros::param::param<std::string>("~camera_name", params["camera_name"], "/camera/rgb");
00059   ros::param::param<std::string>("~bag_location", params["bag_location"], "/home/turtlebot/pano.bag");
00060 
00061   pub_log = priv_nh.advertise<std_msgs::String>("log", 100);
00062 }
00063 
00064 PanoApp::~PanoApp()
00065 {
00066   delete pano_ros_client;
00067 }
00068 
00069 void PanoApp::init()
00070 {
00071   //***************************
00072   // public API for the app
00073   //***************************
00074   srv_start_pano = priv_nh.advertiseService("take_pano", &PanoApp::takePanoServiceCb, this);
00075   sub_start_pano = priv_nh.subscribe("take_pano", 1, &PanoApp::takePanoCb, this);
00076   sub_stop_pano = priv_nh.subscribe("stop_pano", 1, &PanoApp::stopPanoCb, this);
00077   image_transport::ImageTransport it_priv(priv_nh);
00078   pub_stitched = it_priv.advertise("panorama", 1, true);
00079 
00080   //***************************
00081   // Robot control
00082   //***************************
00083   pub_cmd_vel = nh.advertise<geometry_msgs::Twist>("cmd_vel", 100);
00084   sub_odom = nh.subscribe("odom", 100, &PanoApp::odomCb, this);
00085 
00086   //***************************
00087   // pano_ros API
00088   //***************************
00089   pano_ros_client = new actionlib::SimpleActionClient<pano_ros::PanoCaptureAction>("pano_server", true);
00090   log("Waiting for Pano ROS server ...");
00091   pano_ros_client->waitForServer(); // will wait for infinite time
00092   log("Connected to Pano ROS server.");
00093   pub_action_snap = nh.advertise<std_msgs::Empty>("pano_server/snap", 100);
00094   pub_action_stop = nh.advertise<std_msgs::Empty>("pano_server/stop", 100);
00095   image_transport::ImageTransport it_pano(nh);
00096   sub_stitched = it_pano.subscribe("pano_server/stitch", 1, &PanoApp::stitchedImageCb, this);
00097 
00098   cmd_vel.linear.x = 0.0f;
00099   cmd_vel.linear.y = 0.0f;
00100   cmd_vel.linear.z = 0.0f;
00101   cmd_vel.angular.x = 0.0f;
00102   cmd_vel.angular.y = 0.0f;
00103   cmd_vel.angular.z = 0.0f;
00104   zero_cmd_vel = cmd_vel;
00105   is_active = false;
00106   continuous = false;
00107   ang_vel_cur = 0.0;
00108   given_angle = 0.0;
00109   angle = 0.0;
00110   last_angle = 0.0;
00111 }
00112 
00113 void PanoApp::spin()
00114 {
00115   ros::Rate loop_rate(10);
00116   double start_time;
00117   start_time = 0.0;
00118   bool take_snapshot = false;
00119 
00120   while (ros::ok())
00121   {
00122     if (is_active)
00123     {
00124       ROS_INFO_STREAM_THROTTLE(1.0, "Degrees to go: " << radians_to_degrees(std::abs(given_angle - angle)));
00125       if ((given_angle - angle) <= 0.0174) // check, if target angle is reached (< 1 degree)
00126       {
00127         snap();
00128         pub_cmd_vel.publish(zero_cmd_vel);
00129         ros::Duration(1.0).sleep(); // give the pano server some time to retrieve the last pciture
00130         stopPanoAction();
00131       }
00132       else
00133       {
00134         if (continuous) // then snap_interval is a duration
00135         {
00136           double now = ros::Time::now().toSec();
00137           if ((now - start_time) > snap_interval)
00138           {
00139             snap();
00140             start_time = now;
00141           }
00142           rotate();
00143         }
00144         else
00145         {
00146           if (hasReachedAngle())
00147           {
00148             pub_cmd_vel.publish(zero_cmd_vel); // stop before taking a snapshot
00149             take_snapshot = true;
00150           }
00151           if (take_snapshot)
00152           {
00153             if (std::abs(ang_vel_cur) <= 0.000001) // wait until robot has stopped
00154             {
00155               snap();
00156               take_snapshot = false;
00157             }
00158             else
00159             {
00160               std::stringstream ss;
00161               std::string str;
00162               ss << "Waiting for robot to stop ... (speed = " << ang_vel_cur << ")";
00163               str = ss.str();
00164               log(str);
00165             }
00166           }
00167           else
00168           {
00169             rotate();
00170           }
00171         }
00172       }
00173     }
00174     ros::spinOnce();
00175     loop_rate.sleep();
00176   }
00177 }
00178 
00179 
00180 void PanoApp::snap()
00181 {
00182   log("snap");
00183   pub_action_snap.publish(empty_msg);
00184 }
00185 
00186 void PanoApp::rotate()
00187 {
00188   log("rotate");
00189   pub_cmd_vel.publish(cmd_vel); // rotate a bit
00190 }
00191 
00192 bool PanoApp::hasReachedAngle()
00193 {
00194   if (angle > last_angle + degrees_to_radians(snap_interval))
00195   {
00196     last_angle = angle;
00197     return true;
00198   }
00199   else
00200   {
00201     return false;
00202   }
00203 }
00204 
00205 void PanoApp::odomCb(const nav_msgs::OdometryConstPtr& msg)
00206 {
00207   static double heading_last = 0.0f;
00208   double heading = 0.0f;
00209 
00210   Eigen::AngleAxisf angle_axis(Eigen::Quaternionf(msg->pose.pose.orientation.w,
00211                                                   msg->pose.pose.orientation.x,
00212                                                   msg->pose.pose.orientation.y,
00213                                                   msg->pose.pose.orientation.z));
00214   Eigen::Vector3f axis = angle_axis.axis();
00215 
00216   if (axis(2) > 0.0)
00217   {
00218     heading = angle_axis.angle();
00219   }
00220   else if (axis(2) < 0.0)
00221   {
00222     heading = -1.0 * angle_axis.angle();
00223   }
00224 
00225   angle += std::abs(wrap_angle(heading - heading_last));
00226   heading_last = heading;
00227   ang_vel_cur = msg->twist.twist.angular.z;
00228 }
00229 
00230 //*************************
00231 // Public interface
00232 //*************************
00233 bool PanoApp::takePanoServiceCb(turtlebot_msgs::TakePanorama::Request& request,
00234                                 turtlebot_msgs::TakePanorama::Response& response)
00235 {
00236   if (is_active && (request.mode == request.CONTINUOUS || request.mode == request.SNAPANDROTATE))
00237   {
00238     log("Panorama creation already in progress.");
00239     response.status = response.IN_PROGRESS;
00240   }
00241   else if (is_active && (request.mode == request.STOP))
00242   {
00243     stopPanoAction();
00244     is_active = false;
00245     log("Panorama creation stopped.");
00246     response.status = response.STOPPED;
00247     return true;
00248   }
00249   else if (!is_active && (request.mode == request.STOP))
00250   {
00251     log("No panorama creation in progress.");
00252     response.status = response.STOPPED;
00253     return true;
00254   }
00255   else
00256   {
00257     if (request.pano_angle <= 0.0)
00258     {
00259       log("Specified panorama angle is zero or negative! Panorama creation aborted.");
00260       return true;
00261     }
00262     else if (request.snap_interval <= 0.0)
00263     {
00264       log("Specified snapshot interval is zero or negative! Panorama creation aborted.");
00265       return true;
00266     }
00267     else if (request.rot_vel == 0.0)
00268     {
00269       log("Specified rotating speed is zero! Panorama creation aborted.");
00270       return true;
00271     }
00272     else
00273     {
00274       given_angle = degrees_to_radians(request.pano_angle);
00275       snap_interval = request.snap_interval;
00276       cmd_vel.angular.z = request.rot_vel;
00277     }
00278     if (request.mode == turtlebot_msgs::TakePanoramaRequest::CONTINUOUS)
00279     {
00280       continuous = true;
00281     }
00282     else
00283     {
00284       continuous = false;
00285     }
00286     log("Starting panorama creation.");
00287     startPanoAction();
00288     response.status = response.STARTED;
00289   }
00290   return true;
00291 }
00292 
00293 void PanoApp::takePanoCb(const std_msgs::EmptyConstPtr& msg)
00294 {
00295   if (is_active)
00296   {
00297     log("Panorama creation already in progress.");
00298     return;
00299   }
00300   if (default_mode == 0)
00301   {
00302     continuous = false;
00303   }
00304   else if (default_mode == 1)
00305   {
00306     continuous = true;
00307   }
00308   else
00309   {
00310     log("No default panorama mode set. Will use continuous mode.");
00311     continuous = true;
00312   }
00313   given_angle = default_pano_angle;
00314   snap_interval = default_snap_interval;
00315   cmd_vel.angular.z = default_rotation_velocity;
00316   log("Starting panorama creation.");
00317   startPanoAction();
00318 }
00319 
00320 void PanoApp::stopPanoCb(const std_msgs::EmptyConstPtr& msg)
00321 {
00322   if (is_active)
00323   {
00324     stopPanoAction();
00325     is_active = false;
00326     log("Panorama creation stopped.");
00327   }
00328   else
00329   {
00330     log("No panorama is currently being created.");
00331   }
00332 }
00333 
00334 //***************
00335 // Pano ROS API
00336 //***************
00337 void PanoApp::startPanoAction()
00338 {
00339   pano_ros::PanoCaptureGoal goal;
00340   goal.bag_filename = params["bag_location"];
00341   goal.camera_topic = params["camera_name"];
00342   pano_ros_client->sendGoal(goal,
00343                             boost::bind(&PanoApp::doneCb, this, _1, _2),
00344                             boost::bind(&PanoApp::activeCb, this),
00345                             boost::bind(&PanoApp::feedbackCb, this, _1));
00346   log("Pano ROS action goal sent.");
00347   angle = 0.0;
00348   last_angle = 0.0;
00349 }
00350 
00351 void PanoApp::stopPanoAction()
00352 {
00353   pub_action_stop.publish(empty_msg);
00354   is_active = false;
00355   log("Start of stitching triggered.");
00356 }
00357 
00358 void PanoApp::doneCb(const actionlib::SimpleClientGoalState& state, const pano_ros::PanoCaptureResultConstPtr& result)
00359 {
00360   std::string str = "Pano action finished in state : " + state.toString();
00361   log(str);
00362   is_active = false;
00363 }
00364 
00365 void PanoApp::activeCb()
00366 {
00367   log("Pano action goal just went active.");
00368   ros::Duration(1.0).sleep(); // Wait a bit for the server to activate the capture job
00369   snap(); // take first picture before start turning
00370   go_active = true;
00371 }
00372 
00373 void PanoApp::feedbackCb(const pano_ros::PanoCaptureFeedbackConstPtr& feedback)
00374 {
00375   if (go_active) // first pictures has been taken, so start the control loop
00376   {
00377     is_active = true;
00378     go_active = false;
00379   }
00380   std::stringstream ss;
00381   std::string str;
00382   if (continuous) // then snap_interval is a duration
00383   {
00384     ss << "Got pano action feedback: " << (int)feedback->n_captures << " pictures captured.";
00385   }
00386   else // then snap_interval is an angle
00387   {
00388     ss << "Got pano action feedback: " << (int)feedback->n_captures << " of "
00389        << int(given_angle/degrees_to_radians(snap_interval))
00390        << " pictures captured.";
00391   }
00392   log(ss.str());
00393 }
00394 
00395 void PanoApp::stitchedImageCb(const sensor_msgs::ImageConstPtr& msg)
00396 {
00397   pub_stitched.publish(msg);
00398   std::cout << "encoding: " << msg->encoding << std::endl;
00399   std::cout << "is_bigendian: " << msg->is_bigendian << std::endl;
00400 
00401   log("Published new panorama picture.");
00402 }
00403 
00404 //*************
00405 // Logging
00406 //*************
00407 void PanoApp::log(std::string log)
00408 {
00409   std_msgs::String msg;
00410   msg.data = log;
00411   pub_log.publish(msg);
00412   ROS_INFO_STREAM(log);
00413 }
00414 
00415 } //namespace turtlebot_panorama
00416 
00417 int main(int argc, char **argv)
00418 {
00419   ros::init(argc, argv, "turtlebot_panorama");
00420 
00421   turtlebot_panorama::PanoApp pano;
00422   pano.log("Panorama app starting...");
00423   pano.init();
00424   pano.log("Panorama application initialised.");
00425   pano.spin();
00426   pano.log("Bye, bye!");
00427 
00428   return 0;
00429 }


turtlebot_panorama
Author(s): Younghun Ju, Jihoon Lee, Marcus Liebhardt
autogenerated on Mon Mar 14 2016 10:57:25