public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

turn to ros service

Browse Source
This commit is contained in:
gfuse
2021-07-14 18:18:45 +02:00
parent 4cb5cbff09
commit 3b17b7c0f9
12 changed files with 134 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+11 -6
View File
@@ -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
GPU_version/vio/include/gpu_svo/frame_handler_base.h
-5
View File
@@ -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
-3
View File
@@ -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
+3
View File
@@ -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
+2
View File
@@ -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
-10
View File
@@ -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
+3 -2
View File
@@ -66,8 +66,9 @@ FrameHandlerMono::~FrameHandlerMono()
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
{
if(!startFrameProcessingCommon(timestamp))
return;
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
+1 -2
View File
@@ -31,8 +31,7 @@ namespace svo {
Visualizer::
Visualizer() :
pnh_("~"),
trace_id_(0),
T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
trace_id_(0)
{
// Init ROS Marker Publishers
//pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
GPU_version/vio/src/vo_node.cpp
+95 -90
View File
@@ -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_;
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<bool>("vio/publish_markers", true)),
cam_(NULL),
quit_(false)
cam_(NULL)
{
// 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_))
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;
#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<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;
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<std::size_t>(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<std::size_t>(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<std::string>("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<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this);
ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("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<std::string>("vio/imu_topic", "imu/raw"),10,&VoNode::imuCb, &vo_node);
ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VoNode::cmdCb, &vo_node);
VioNode vio;
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
+9
View File
@@ -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
+5
View File
@@ -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
+5
View File
@@ -0,0 +1,5 @@
# Turn off VIO if off is 1
uint8 off
---
# Return 0 if VIO is off
uint8 ret