turn to ros service

GPU_version/vio/CMakeLists.txt

@@ -31,14 +31,19 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
 FIND_PACKAGE(OpenCL REQUIRED)
 
 set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}")
-FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs)
+FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation)
+
+add_service_files(FILES
+        getOdom.srv
+        stop.srv
+        start.srv)
+
+generate_messages(DEPENDENCIES std_msgs)
 catkin_package(
         DEPENDS Eigen OpenCV Sophus Boost
-      CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs
+        CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime
         INCLUDE_DIRS include
 )
-
-
 # Include dirs
 INCLUDE_DIRECTORIES(
         include

GPU_version/vio/include/gpu_svo/frame_handler_base.h

@@ -18,8 +18,6 @@
 #define SVO_FRAME_HANDLER_BASE_H_
 
 #include <queue>
-//#include <vikit/timer.h>
-//#include <vikit/ringbuffer.h>
 #include <boost/noncopyable.hpp>
 #include <boost/function.hpp>
 #include <boost/thread.hpp>
@@ -90,9 +88,6 @@ protected:
   bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
   bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
   Map map_;                     //!< Map of keyframes created by the slam system.
-  //vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
-  //vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
-  //vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
   size_t num_obs_last_;                         //!< Number of observations in the previous frame.
   TrackingQuality tracking_quality_;            //!< An estimate of the tracking quality based on the number of tracked features.
 

GPU_version/vio/include/gpu_svo/visualizer.h

@@ -46,9 +46,6 @@ public:
   ros::Publisher pub_frames_;
   ros::Publisher pub_points_;
   ros::Publisher pub_pose_with_cov_;
-  ros::Publisher pub_info_;
-  tf::TransformBroadcaster br_;
-  SE3 T_world_from_vision_;
 
   Visualizer();
 

GPU_version/vio/launch/Px30.launch

@@ -6,6 +6,9 @@
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
     <param name="vio/imu_topic" value="/SensorNode/imu" type="str" />
     <param name="vio/cmd_topic" value="/cmd_vel" type="str" />
+    <param name="vio/start" value="/vio/start" type="str" />
+    <param name="vio/stop" value="/vio/stop" type="str" />
+    <param name="vio/getOdom" value="/vio/set_odom" type="str" />
     
     <!-- Camera calibration file -->
     <rosparam file="$(find vio)/param/px30.yaml" />

GPU_version/vio/package.xml

@@ -21,6 +21,7 @@
   <build_depend>cv_bridge</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
 
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
@@ -29,4 +30,5 @@
   <run_depend>cv_bridge</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
 </package>

GPU_version/vio/src/frame_handler_base.cpp

@@ -70,15 +70,9 @@ int FrameHandlerBase::finishFrameProcessingCommon(
     const UpdateResult dropout,
     const size_t num_observations)
 {
-//  SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
- // SVO_LOG(dropout);
 
-  // save processing time to calculate fps
-//  acc_frame_timings_.push_back(timer_.stop());
   if(stage_ == STAGE_DEFAULT_FRAME)
- //   acc_num_obs_.push_back(num_observations);
   num_obs_last_ = num_observations;
- // SVO_STOP_TIMER("tot_time");
 
   if(dropout == RESULT_FAILURE &&
       (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
@@ -86,10 +80,6 @@ int FrameHandlerBase::finishFrameProcessingCommon(
     stage_ = STAGE_RELOCALIZING;
     tracking_quality_ = TRACKING_INSUFFICIENT;
   }
-  /*
-  else if (dropout == RESULT_FAILURE)
-    resetAll();
-    */
   if(set_reset_)
     resetAll();
 

GPU_version/vio/src/frame_handler_mono.cpp

@@ -66,8 +66,9 @@ FrameHandlerMono::~FrameHandlerMono()
 
 void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
 {
-  if(!startFrameProcessingCommon(timestamp))
+  if(!startFrameProcessingCommon(timestamp)){
       return;
+  }
 
   // some cleanup from last iteration, can't do before because of visualization
   core_kfs_.clear();

GPU_version/vio/src/visualizer.cpp

@@ -31,8 +31,7 @@ namespace svo {
 Visualizer::
 Visualizer() :
     pnh_("~"),
-    trace_id_(0),
+    trace_id_(0)
-    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
 {
   // Init ROS Marker Publishers
   //pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);

GPU_version/vio/src/vo_node.cpp

@@ -18,10 +18,10 @@
 #include <string>
 #include <chrono>
 #include <gpu_svo/frame_handler_mono.h>
-#include <gpu_svo/visualizer.h>
 #include <gpu_svo/params_helper.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
+#include <geometry_msgs/Twist.h>
 #include <image_transport/image_transport.h>
 #include <boost/thread.hpp>
 #include <cv_bridge/cv_bridge.h>
@@ -29,96 +29,101 @@
 #include <gpu_svo/abstract_camera.h>
 #include <gpu_svo/camera_loader.h>
 #include <gpu_svo/frame.h>
+#include <vio/getOdom.h>
+#include <vio/start.h>
+#include <vio/stop.h>
+#if VIO_DEBUG
+#include <gpu_svo/visualizer.h>
+#endif
 
-class VoNode
+class VioNode
 {
 public:
       svo::FrameHandlerMono* vo_;
-      svo::Visualizer visualizer_;
-      bool publish_markers_;                 //!< publish only the minimal amount of info (choice for embedded devices)
       vk::AbstractCamera* cam_;
-      bool quit_;
+      VioNode();
-      boost::thread* imu_th;
+      ~VioNode();
-      VoNode();
-      ~VoNode();
       void imgCb(const sensor_msgs::ImageConstPtr& msg);
       void imuCb(const sensor_msgs::ImuPtr& imu);
     void cmdCb(const geometry_msgs::TwistPtr &imu);
-    void imu();
+    void imu_th();
-private:
+    bool getOdom(vio::getOdom::Request& req, vio::getOdom::Response& res);
-    double* imu_;
+    uint trace_id_= 0;
-#if VIO_DEBUG
+    bool start_=false;
-    FILE* log_= nullptr;
+    bool start(vio::start::Request& req, vio::start::Response& res){
-    double imu_t_[6]={0.0,0.0,0.0,0.0,0.0,0.0};
+        if(req.on==1){
-#endif
+            start_=true;
-    double* cmd_;
+            if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME)
-    ros::Time imu_time_;
+                vo_->start();
+            else
+                vo_->reset();
+            imu_the_=new boost::thread(&VioNode::imu_th,this);
+            ++trace_id_;
+            res.ret=0;
+        }else{
+            res.ret=100;
+        }
+        return true;
     };
 
-VoNode::VoNode() :
+    bool stop(vio::stop::Request& req, vio::stop::Response& res){
+        if(req.off==1){
+            start_=false;
+            usleep(5000);
+            imu_the_->join();
+            res.ret=0;
+        }else{
+            res.ret=100;
+        }
+        return true;
+    };
+private:
+    double* imu_;
+    double* cmd_;
+    ros::Time imu_time_;
+    boost::thread* imu_the_= nullptr;
+#if VIO_DEBUG
+    svo::Visualizer visualizer_;
+#endif
+};
+
+VioNode::VioNode() :
       vo_(NULL),
-      publish_markers_(vk::getParam<bool>("vio/publish_markers", true)),
+      cam_(NULL)
-      cam_(NULL),
-      quit_(false)
 {
-      // Create Camera
     imu_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double));
     cmd_=(double *)calloc(static_cast<std::size_t>(3), sizeof(double));
     if(!vk::camera_loader::loadFromRosNs("vio", cam_))
         throw std::runtime_error("Camera model not correctly specified.");
-      // Get initial position and orientation
-      visualizer_.T_world_from_vision_ = Sophus::SE3(
-          vk::rpy2dcm(Eigen::Vector3d(vk::getParam<double>("vio/init_rx", 0.0),
-                               vk::getParam<double>("vio/init_ry", 0.0),
-                               vk::getParam<double>("vio/init_rz", 0.0))),
-          Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
-                          vk::getParam<double>("vio/init_ty", 0.0),
-                          vk::getParam<double>("vio/init_tz", 0.0)));
     Eigen::Matrix<double,3,1> init;
     init<<1e-19,1e-19,1e-19;
     vo_ = new svo::FrameHandlerMono(cam_,init);
     usleep(500);
-
-#if VIO_DEBUG
-log_ = fopen((std::string(getenv("HOME"))+"/imu_integration.txt").c_str(), "w+");
-#endif
-    vo_->start();
-    imu_th=new boost::thread(&VoNode::imu,this);
-
 }
 
-VoNode::~VoNode()
+VioNode::~VioNode()
 {
-    quit_=true;
+    start_=false;
-
+    usleep(5000);
-
+    imu_the_->join();
-#if VIO_DEBUG
+    delete imu_the_;
-    fclose(log_);
-    delete log_;
-#endif
-    imu_th->join();
-    delete imu_th;
     delete vo_;
     delete cam_;
 }
 
-void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
+void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 {
+    if(!start_)return;
       try {
           auto start=std::chrono::steady_clock::now();
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
           vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp);
-#if VIO_DEBUG
-    fprintf(log_, ",,,,,,,%s,%f,%f,%f\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str(),vo_->lastFrame()->T_f_w_.translation()(0),
-            vo_->lastFrame()->T_f_w_.translation()(1),
-            acos(vo_->lastFrame()->T_f_w_.getSE2().rotation_matrix()(0,0)));
-#endif
-
       } catch (cv_bridge::Exception& e) {
         ROS_ERROR("cv_bridge exception: %s", e.what());
       }
 }
-void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
+void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
+    if(!start_)return;
     double imu_in[3];
     if(abs(imu->linear_acceleration.x)>5.0){
         return;
@@ -134,62 +139,62 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y;
     imu_in[2] = 0.5*imu_[2]+0.5*imu->angular_velocity.z;
     memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
-#if VIO_DEBUG
-        if(imu_t_[5]!=0.0){
-            double dt=1e-9*(imu->header.stamp.toNSec()-imu_t_[5]);
-            imu_t_[0]+=imu_in[0]*dt;
-            imu_t_[1]+=imu_t_[0]*dt;
-            imu_t_[2]+=imu_in[1]*dt;
-            imu_t_[3]+=imu_t_[2]*dt;
-            imu_t_[4]+=imu_in[2]*dt;
-            imu_t_[5]=imu->header.stamp.toNSec();
-            fprintf(log_, "%f,%f,%f,%f\n",imu_t_[1],imu_t_[3],imu_t_[4],dt);
-        }else{
-            imu_t_[0]=imu_in[0];
-            imu_t_[1]=0.0;
-            imu_t_[2]=imu_in[1];
-            imu_t_[3]=0.0;
-            imu_t_[4]=imu_in[2];
-            imu_t_[5]=imu->header.stamp.toNSec();
-        }
-#endif
     vo_->UpdateIMU(imu_in,imu->header.stamp);
     imu_time_=imu->header.stamp;
+#if VIO_DEBUG
     visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
+#endif
 }
-void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
+void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
+    if(!start_)return;
     double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};
     memcpy(cmd_, _cmd, static_cast<std::size_t>(3*sizeof(double)));
-#if VIO_DEBUG
-        fprintf(log_, ",,,,%f,%f,%f\n",_cmd[0],_cmd[1],_cmd[2]);
-#endif
     vo_->UpdateCmd(_cmd,imu_time_);
 }
-void VoNode::imu(){
+void VioNode::imu_th(){
     ros::NodeHandle nh;
-    // subscribe to cam msgs
+    ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this);
-    image_transport::ImageTransport it(nh);
+    ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this);
-    image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("vio/cam_topic", "camera/image_raw"), 1, &VoNode::imgCb, this);
+    while(start_)
-    // start processing callbacks
-    while(ros::ok() && !quit_)
     {
         ros::spinOnce();
+        usleep(1000);
     }
 
 }
+bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) {
+    if(req.get==1){
+        res.header.stamp = ros::Time();
+        res.header.frame_id = "/world";
+        res.header.seq = trace_id_;
+        auto odom=vo_->ukfPtr_.get_location();
+        res.x=odom.second.translation()(0);
+        res.y=odom.second.translation()(1);
+        res.yaw=odom.second.pitch();
+        res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2),
+                 odom.first(1,0),odom.first(1,1),odom.first(1,2),
+                 odom.first(2,0),odom.first(2,1),odom.first(2,2)};
+    }
+    return true;
+}
 
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "vio");
-  VoNode vo_node;
   ros::NodeHandle nh;
-  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VoNode::imuCb, &vo_node);
+  VioNode vio;
-  ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VoNode::cmdCb, &vo_node);
+  ros::ServiceServer start = nh.advertiseService(vk::getParam<std::string>("vio/start", "start"),
+                                                 &VioNode::start, &vio);
+  ros::ServiceServer stop = nh.advertiseService(vk::getParam<std::string>("vio/stop", "stop"),
+                                                &VioNode::stop, &vio);
+  ros::ServiceServer getOdom = nh.advertiseService(vk::getParam<std::string>("vio/odom", "get_odom"),
+                                                   &VioNode::getOdom, &vio);
+  image_transport::ImageTransport it(nh);
+  image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("vio/cam_topic", "camera/image_raw"), 1, &VioNode::imgCb, &vio);
   // start processing callbacks
-  while(ros::ok() && !vo_node.quit_)
+  while(ros::ok())
   {
       ros::spinOnce();
-      usleep(1000);
   }
   printf("SVO terminated.\n");
   return 0;

GPU_version/vio/srv/getOdom.srv

@@ -0,0 +1,9 @@
+# Odometry information will be return if get is 1
+uint8 get
+---
+# Odometry information
+Header header
+float64 x
+float64 y
+float64 yaw
+float64[9] cov

GPU_version/vio/srv/start.srv

@@ -0,0 +1,5 @@
+# Turn ON VIO if on is 1
+uint8 on
+---
+# Return 0 if VIO is ON
+uint8 ret

GPU_version/vio/srv/stop.srv

@@ -0,0 +1,5 @@
+# Turn off VIO if off is 1
+uint8 off
+---
+# Return 0 if VIO is off
+uint8 ret