From 3b17b7c0f9c6eb5ab6f8131a121103f9055ed2e6 Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 14 Jul 2021 18:18:45 +0200 Subject: [PATCH] turn to ros service --- GPU_version/vio/CMakeLists.txt | 17 +- .../vio/include/gpu_svo/frame_handler_base.h | 5 - GPU_version/vio/include/gpu_svo/visualizer.h | 3 - GPU_version/vio/launch/Px30.launch | 3 + GPU_version/vio/package.xml | 2 + GPU_version/vio/src/frame_handler_base.cpp | 10 - GPU_version/vio/src/frame_handler_mono.cpp | 5 +- GPU_version/vio/src/visualizer.cpp | 3 +- GPU_version/vio/src/vo_node.cpp | 185 +++++++++--------- GPU_version/vio/srv/getOdom.srv | 9 + GPU_version/vio/srv/start.srv | 5 + GPU_version/vio/srv/stop.srv | 5 + 12 files changed, 134 insertions(+), 118 deletions(-) create mode 100644 GPU_version/vio/srv/getOdom.srv create mode 100644 GPU_version/vio/srv/start.srv create mode 100644 GPU_version/vio/srv/stop.srv diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index 938c850..4644e2b 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/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 - INCLUDE_DIRS include + DEPENDS Eigen OpenCV Sophus Boost + CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime + INCLUDE_DIRS include ) - - # Include dirs INCLUDE_DIRECTORIES( include diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_base.h b/GPU_version/vio/include/gpu_svo/frame_handler_base.h index c8d84e5..99b4195 100644 --- a/GPU_version/vio/include/gpu_svo/frame_handler_base.h +++ b/GPU_version/vio/include/gpu_svo/frame_handler_base.h @@ -18,8 +18,6 @@ #define SVO_FRAME_HANDLER_BASE_H_ #include -//#include -//#include #include #include #include @@ -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 acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. - //vk::RingBuffer 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. diff --git a/GPU_version/vio/include/gpu_svo/visualizer.h b/GPU_version/vio/include/gpu_svo/visualizer.h index 819885d..307c27e 100644 --- a/GPU_version/vio/include/gpu_svo/visualizer.h +++ b/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(); diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index efaecc2..b5371c6 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -6,6 +6,9 @@ + + + diff --git a/GPU_version/vio/package.xml b/GPU_version/vio/package.xml index 1d8d5a4..262fff1 100755 --- a/GPU_version/vio/package.xml +++ b/GPU_version/vio/package.xml @@ -21,6 +21,7 @@ cv_bridge tf geometry_msgs + message_generation roscpp @@ -29,4 +30,5 @@ cv_bridge tf geometry_msgs + message_runtime \ No newline at end of file diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp index 3c77fca..a232ce3 100644 --- a/GPU_version/vio/src/frame_handler_base.cpp +++ b/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: "<("keyframes", 10); diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index dfae0ab..a7df993 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -18,10 +18,10 @@ #include #include #include -#include #include #include #include +#include #include #include #include @@ -29,96 +29,101 @@ #include #include #include +#include +#include +#include +#if VIO_DEBUG +#include +#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_; - boost::thread* imu_th; - VoNode(); - ~VoNode(); + VioNode(); + ~VioNode(); 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(); + bool getOdom(vio::getOdom::Request& req, vio::getOdom::Response& res); + uint trace_id_= 0; + bool start_=false; + bool start(vio::start::Request& req, vio::start::Response& res){ + if(req.on==1){ + start_=true; + if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME) + 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; + }; + + 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_; -#if VIO_DEBUG - FILE* log_= nullptr; - double imu_t_[6]={0.0,0.0,0.0,0.0,0.0,0.0}; -#endif double* cmd_; ros::Time imu_time_; + boost::thread* imu_the_= nullptr; +#if VIO_DEBUG + svo::Visualizer visualizer_; +#endif }; -VoNode::VoNode() : +VioNode::VioNode() : vo_(NULL), - publish_markers_(vk::getParam("vio/publish_markers", true)), - cam_(NULL), - quit_(false) + cam_(NULL) { - // Create Camera - imu_=(double *)calloc(static_cast(3), sizeof(double)); - cmd_=(double *)calloc(static_cast(3), sizeof(double)); - if(!vk::camera_loader::loadFromRosNs("vio", cam_)) + imu_=(double *)calloc(static_cast(3), sizeof(double)); + cmd_=(double *)calloc(static_cast(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("vio/init_rx", 0.0), - vk::getParam("vio/init_ry", 0.0), - vk::getParam("vio/init_rz", 0.0))), - Eigen::Vector3d(vk::getParam("vio/init_tx", 0.0), - vk::getParam("vio/init_ty", 0.0), - vk::getParam("vio/init_tz", 0.0))); Eigen::Matrix 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; - - -#if VIO_DEBUG - fclose(log_); - delete log_; -#endif - imu_th->join(); - delete imu_th; + start_=false; + usleep(5000); + imu_the_->join(); + delete imu_the_; 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(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(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; - visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec()); -} -void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) { - double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z}; - memcpy(cmd_, _cmd, static_cast(3*sizeof(double))); #if VIO_DEBUG - fprintf(log_, ",,,,%f,%f,%f\n",_cmd[0],_cmd[1],_cmd[2]); + visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec()); #endif +} +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(3*sizeof(double))); vo_->UpdateCmd(_cmd,imu_time_); } -void VoNode::imu(){ +void VioNode::imu_th(){ ros::NodeHandle nh; - // subscribe to cam msgs - image_transport::ImageTransport it(nh); - image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 1, &VoNode::imgCb, this); - // start processing callbacks - while(ros::ok() && !quit_) + ros::Subscriber imu_sub=nh.subscribe(vk::getParam("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this); + ros::Subscriber cmd_sub=nh.subscribe(vk::getParam("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this); + while(start_) { 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("vio/imu_topic", "imu/raw"),10,&VoNode::imuCb, &vo_node); - ros::Subscriber cmd_sub=nh.subscribe(vk::getParam("vio/cmd_topic", "cmd/raw"),10,&VoNode::cmdCb, &vo_node); + VioNode vio; + ros::ServiceServer start = nh.advertiseService(vk::getParam("vio/start", "start"), + &VioNode::start, &vio); + ros::ServiceServer stop = nh.advertiseService(vk::getParam("vio/stop", "stop"), + &VioNode::stop, &vio); + ros::ServiceServer getOdom = nh.advertiseService(vk::getParam("vio/odom", "get_odom"), + &VioNode::getOdom, &vio); + image_transport::ImageTransport it(nh); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("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; diff --git a/GPU_version/vio/srv/getOdom.srv b/GPU_version/vio/srv/getOdom.srv new file mode 100644 index 0000000..9f60ae7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/GPU_version/vio/srv/start.srv b/GPU_version/vio/srv/start.srv new file mode 100644 index 0000000..a1c27cc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/GPU_version/vio/srv/stop.srv b/GPU_version/vio/srv/stop.srv new file mode 100644 index 0000000..7fa22db --- /dev/null +++ b/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 \ No newline at end of file