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

edit svo

Browse Source
This commit is contained in:
gfuse
2021-04-12 12:43:04 +02:00
parent 651dcdfb2f
commit 56c7f23088
8 changed files with 139 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vio_svo/launch/test.launch
+2 -1
View File
@@ -4,6 +4,7 @@
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/camera/image_raw" type="str" />
<param name="imu_topic" value="/SensorNode/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
@@ -18,4 +19,4 @@
</node>
</launch>
</launch>
vio_svo/src/vo_node.cpp
+18 -11
View File
@@ -30,6 +30,7 @@
#include <vikit/abstract_camera.h>
#include <vikit/camera_loader.h>
#include <vikit/user_input_thread.h>
#include <svo/frame.h>
namespace svo {
@@ -44,12 +45,12 @@ public:
vk::AbstractCamera* cam_;
FILE* logtime;
bool quit_;
boost::thread* vio_th;
boost::thread* imu_th;
VoNode();
~VoNode();
void imgCb(const sensor_msgs::ImageConstPtr& msg);
void imuCb(const sensor_msgs::ImuPtr& imu);
void vio();
void imu();
};
VoNode::VoNode() :
@@ -70,10 +71,10 @@ VoNode::VoNode() :
vk::getParam<double>("vio/init_ty", 0.0),
vk::getParam<double>("vio/init_tz", 0.0)));
// Init VO and start
vo_ = new svo::FrameHandlerMono(cam_);
vo_ = new svo::FrameHandlerMono(cam_,visualizer_.T_world_from_vision_);
vo_->start();
logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+");
vio_th=new boost::thread(&VoNode::vio,this);
imu_th=new boost::thread(&VoNode::imu,this);
logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+");
}
@@ -81,8 +82,8 @@ VoNode::~VoNode()
{
quit_=true;
fclose(logtime);
vio_th->join();
delete vio_th;
imu_th->join();
delete imu_th;
delete logtime;
delete vo_;
delete cam_;
@@ -91,7 +92,6 @@ VoNode::~VoNode()
void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{
try {
ROS_INFO("image back");
auto start=std::chrono::steady_clock::now();
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
vo_->addImage(img, msg->header.stamp.toSec());
@@ -107,9 +107,17 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
}
}
void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
ROS_INFO("imu back");
float imu_in[7];
imu_in[0] = imu->angular_velocity.x;
imu_in[1] = imu->angular_velocity.y;
imu_in[2] = imu->angular_velocity.z;
imu_in[3] = imu->linear_acceleration.x;
imu_in[4] = imu->linear_acceleration.y;
imu_in[5] = imu->linear_acceleration.z;
imu_in[6] = imu->header.stamp.sec;
vo_->imu_integPtr_->update(imu_in);
}
void VoNode::vio(){
void VoNode::imu(){
ros::NodeHandle nh;
std::cout << "create vo_node" << std::endl;
// subscribe to cam msgs
@@ -119,7 +127,6 @@ void VoNode::vio(){
while(ros::ok() && !quit_)
{
ros::spinOnce();
// TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
}
}