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

debuging based on px30 -> still in progress

Browse Source
This commit is contained in:
gfuse
2021-04-16 17:55:40 +02:00
parent 382c41725b
commit 121cfe1095
4 changed files with 47 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vio_svo/launch/Px30.launch
+3 -3
View File
@@ -7,13 +7,13 @@
<param name="imu_topic" value="/SensorNode/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/camera_pinhole.yaml" />
<rosparam file="$(find vio_svo)/param/px30.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/px30.yaml" />
<rosparam file="$(find vio_svo)/param/vo_accurate.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="0.00" />
<param name="init_rx" value="3.14" />
<param name="init_ry" value="0.00" />
<param name="init_rz" value="0.00" />
vio_svo/src/vo_node.cpp
+10 -18
View File
@@ -50,8 +50,7 @@ public:
VoNode();
~VoNode();
void imgCb(const sensor_msgs::ImageConstPtr& msg);
//void imuCb(const sensor_msgs::ImuPtr& imu);
void imuCb(const geometry_msgs::PoseStamped& imu);
void imuCb(const sensor_msgs::ImuPtr& imu);
void imu();
};
@@ -108,22 +107,15 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
ROS_ERROR("cv_bridge exception: %s", e.what());
}
}
void VoNode::imuCb(const geometry_msgs::PoseStamped &imu) {
float imu_in[6];
/*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;
*/
imu_in[0] = imu.pose.position.z;
imu_in[1] = imu.pose.position.x;
imu_in[2] = imu.pose.position.y+9.8;
imu_in[3] = imu.pose.orientation.z;
imu_in[4] = imu.pose.orientation.x;
imu_in[5] = imu.pose.orientation.y;
void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
double imu_in[7];
imu_in[0] = imu->linear_acceleration.x;
imu_in[1] = imu->linear_acceleration.y;
imu_in[2] = imu->linear_acceleration.z;
imu_in[3] = imu->angular_velocity.x;
imu_in[4] = imu->angular_velocity.y;
imu_in[5] = imu->angular_velocity.z;
imu_in[6] = imu->header.stamp.toNSec();
vo_->imu_integPtr_->update(imu_in);
}
void VoNode::imu(){