visual_servo_pioneer.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ros/console.h>
00003 #include <std_msgs/Int8.h>
00004 #include <geometry_msgs/Twist.h>
00005 #include <geometry_msgs/PoseStamped.h>
00006 #include <visp_bridge/3dpose.h>
00007 #include <visp_bridge/camera.h>
00008 #include <sensor_msgs/CameraInfo.h>
00009 
00010 #include <visp/vpAdaptiveGain.h>
00011 #include <visp/vpCameraParameters.h>
00012 #include <visp/vpDot.h>
00013 #include <visp/vpDot2.h>
00014 #include <visp/vpFeatureBuilder.h>
00015 #include <visp/vpFeatureDepth.h>
00016 #include <visp/vpFeaturePoint.h>
00017 #include <visp/vpHomogeneousMatrix.h>
00018 #include <visp/vpPioneer.h>
00019 #include <visp/vpServo.h>
00020 #include <visp/vpVelocityTwistMatrix.h>
00021 
00022 class VS
00023 {
00024 private:
00025   ros::NodeHandle nh_;
00026   ros::Publisher  pubTwist_; // cmd_vel
00027   ros::Subscriber subPose_;  // pose_stamped
00028   ros::Subscriber subStatus_;  // pose_stamped
00029   ros::Subscriber sub_cam_info; // Camera parameters
00030 
00031   vpServo task;
00032   // Current and desired visual feature associated to the x coordinate of the point
00033   vpFeaturePoint s_x, s_xd;
00034   vpFeatureDepth s_Z, s_Zd;
00035 
00036 
00037 
00038   vpCameraParameters cam;
00039   bool Stream_info_camera; //Is equal to one if we received the information about the camera
00040   double depth;
00041   double Z, Zd;
00042   double lambda;
00043 
00044   bool valid_pose;
00045   bool valid_pose_prev;
00046 
00047   double t_start_loop;
00048   double tinit;
00049 
00050   vpColVector v;
00051   vpColVector vi;
00052   double mu;
00053   vpAdaptiveGain lambda_adapt;
00054 
00055 public:
00056   void init_vs();
00057   void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
00058   void statusCallback(const std_msgs::Int8ConstPtr& msg);
00059   void CameraInfoCb(const sensor_msgs::CameraInfo& msg);
00060   VS(int argc, char**argv);
00061   virtual ~VS() {
00062     task.kill();
00063   };
00064 };
00065 
00066 VS::VS(int argc, char**argv)
00067 {
00068   //init_vs();
00069 
00070   subPose_   = nh_.subscribe("/visp_auto_tracker/object_position", 1000, &VS::poseCallback, this);
00071   subStatus_ = nh_.subscribe("/visp_auto_tracker/status", 1000, &VS::statusCallback, this);
00072   pubTwist_  = nh_.advertise<geometry_msgs::Twist>("vs/pioneer/cmd_vel", 1000);
00073   // Subscribe to the topic Camera info in order to receive the camera paramenter. The callback function will be called only one time.
00074   sub_cam_info = nh_.subscribe("/camera_info", 1000,&VS::CameraInfoCb,this);
00075 
00076   depth = 0.4;
00077   lambda = 1.;
00078   valid_pose = false;
00079   valid_pose_prev = false;
00080 
00081   Stream_info_camera = 0;
00082 
00083   Z = Zd = depth;
00084 
00085   v.resize(2);
00086   vi.resize(2);
00087   v = 0; vi = 0;
00088   mu = 4;
00089 
00090   t_start_loop = 0.0;
00091   tinit = 0.0;
00092 }
00093 
00094 void VS::init_vs()
00095 {
00096 
00097 
00098   //cam.initPersProjWithoutDistortion(800, 795, 320, 216);
00099 
00100 
00101   lambda_adapt.initStandard(3, 0.2, 40);
00102 
00103 
00104   task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
00105   task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
00106   task.setLambda(lambda_adapt) ;
00107 
00108   vpPioneer robot; // Pas top ! devrait etre vpRobotPioneer
00109   vpVelocityTwistMatrix cVe = robot.get_cVe();
00110   vpMatrix eJe = robot.get_eJe();
00111   task.set_cVe( cVe );
00112   task.set_eJe( eJe );
00113 
00114   vpImagePoint ip(0,0);
00115 
00116   // Create the current x visual feature
00117   vpFeatureBuilder::create(s_x, cam, ip);
00118 
00119   // Create the desired x* visual feature
00120   s_xd.buildFrom(0, 0, Zd);
00121 
00122   // Add the feature
00123   task.addFeature(s_x, s_xd, vpFeaturePoint::selectX()) ;
00124 
00125   s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
00126   s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
00127 
00128   // Add the feature
00129   task.addFeature(s_Z, s_Zd) ;
00130 
00131 }
00132 
00133 void VS::statusCallback(const std_msgs::Int8ConstPtr& msg)
00134 {
00135   if (msg->data == 3)
00136     valid_pose = true;
00137   else
00138     valid_pose = false;
00139 }
00140 
00141 void VS::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
00142 {
00143 
00144         if (!Stream_info_camera ) // We check if the streaming of images is started or not
00145           {
00146                 std::cout << "Waiting for the camera parameters."<<std::endl;
00147              return;
00148           }
00149 
00150 
00151   geometry_msgs::Twist out_cmd_vel;
00152   try {
00153     t_start_loop = vpTime::measureTimeMs();
00154 
00155     std::ostringstream strs;
00156     strs << "Receive a new pose" << std::endl;
00157     std::string str;
00158     str = strs.str();
00159     ROS_DEBUG("%s", str.c_str());
00160 
00161     vpHomogeneousMatrix cMo = visp_bridge::toVispHomogeneousMatrix(msg->pose);
00162 
00163     vpPoint origin;
00164     origin.setWorldCoordinates(0,0,0);
00165     origin.project(cMo);
00166     Z = origin.get_Z();
00167 
00168     if (Z <= 0)
00169       ROS_DEBUG("Z <= 0");
00170 
00171     if (! valid_pose || Z <= 0) {
00172       ROS_DEBUG("not valid pose");
00173 
00174       out_cmd_vel.linear.x = 0;
00175       out_cmd_vel.linear.y = 0;
00176       out_cmd_vel.linear.z = 0;
00177       out_cmd_vel.angular.x = 0;
00178       out_cmd_vel.angular.y = 0;
00179       out_cmd_vel.angular.z = 0;
00180       pubTwist_.publish(out_cmd_vel);
00181 
00182       valid_pose = false;
00183       valid_pose_prev = valid_pose;
00184 
00185       return;
00186     }
00187 
00188     // Update the current x feature
00189     s_x.set_xyZ(origin.p[0], origin.p[1], Z);
00190 
00191     // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
00192     s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
00193 
00194     vpPioneer robot; // Pas top ! devrait etre vpRobotPioneer
00195     vpVelocityTwistMatrix cVe = robot.get_cVe();
00196     vpMatrix eJe = robot.get_eJe();
00197     task.set_cVe( cVe );
00198     task.set_eJe( eJe );
00199 
00200     // Compute the control law. Velocities are computed in the mobile robot reference frame
00201     v = task.computeControlLaw() ;
00202 
00203     if (0) { //valid_pose_prev == false) {
00204       // Start a new visual servo
00205       ROS_INFO("Reinit visual servo");
00206 
00207       tinit = t_start_loop;
00208       vi = v;
00209     }
00210 
00211     v = v - vi*exp(-mu*(t_start_loop - tinit)/1000.);
00212     double max_linear_vel = 0.5;
00213     double max_angular_vel = vpMath::rad(50);
00214 
00215     if (std::abs(v[0]) > max_linear_vel || std::abs(v[1]) > max_angular_vel) {
00216       ROS_INFO("Vel exceed max allowed");
00217       for (unsigned int i=0; i< v.size(); i++)
00218         ROS_INFO("v[%d]=%f", i, v[i]);
00219       v = 0;
00220     }
00221 
00222     out_cmd_vel.linear.x = v[0];
00223     out_cmd_vel.linear.y = 0;
00224     out_cmd_vel.linear.z = 0;
00225     out_cmd_vel.angular.x = 0;
00226     out_cmd_vel.angular.y = 0;
00227     out_cmd_vel.angular.z = v[1];
00228 
00229     pubTwist_.publish(out_cmd_vel);
00230     valid_pose_prev = valid_pose;
00231 
00232     valid_pose = false;
00233   }
00234   catch(...) {
00235     ROS_INFO("Catch an exception: set vel to 0");
00236     out_cmd_vel.linear.x = 0;
00237     out_cmd_vel.linear.y = 0;
00238     out_cmd_vel.linear.z = 0;
00239     out_cmd_vel.angular.x = 0;
00240     out_cmd_vel.angular.y = 0;
00241     out_cmd_vel.angular.z = 0;
00242     pubTwist_.publish(out_cmd_vel);
00243   }
00244 }
00245 
00246 
00247 void VS::CameraInfoCb(const sensor_msgs::CameraInfo& msg)
00248  {
00249           std::cout << "Received Camera INFO"<<std::endl;
00250      // Convert the paramenter in the visp format
00251      cam = visp_bridge::toVispCameraParameters(msg);
00252      cam.printParameters();
00253 
00254      // Stop the subscriber (we don't need it anymore)
00255      this->sub_cam_info.shutdown();
00256 
00257      Stream_info_camera = 1;
00258      init_vs();
00259 
00260 
00261  }
00262 
00263 
00264 
00265 int main(int argc, char **argv)
00266 {
00267   ros::init(argc, argv, "pioneer");
00268 
00269   VS vs(argc, argv);
00270 
00271   ros::spin();
00272 }
00273 
00274 


demo_pioneer
Author(s): Fabien Spindler
autogenerated on Fri Aug 19 2016 04:09:46