p71924506
/
att_ekf
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix bug about the base_footprint link, now use world as the frame id

This commit is contained in:
libing64 2016-03-22 20:19:56 +08:00
parent 9578932d58
commit ec04217a15
3 changed files with 21 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
launch/att_ekf.launch
View File

@ -3,6 +3,7 @@
<remap from="/imu" to="/raw_imu"/>
<remap from="/magnetic_field" to="/magnetic" />
<remap from="/imu_bias" to="/raw_imu/bias" />
<remap from="/pose_gt" to="/ground_truth_to_tf/pose" />
</node>
<!-- Start rviz visualization with preset config -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_estimation.rviz"/>

31
rviz_cfg/att_estimation.rviz
View File

@ -59,29 +59,6 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_cam_link:
Alpha: 1
Show Axes: false
Show Trail: false
front_cam_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
laser0_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
sonar_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
@ -183,7 +160,7 @@ Visualization Manager:
Shaft Length: 1
Shaft Radius: 0.05
Shape: Axes
Topic: /pose_gt
Topic: /ground_truth_to_tf/pose
Value: true
Enabled: true
Global Options:
@ -209,7 +186,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 12.4519
Distance: 13.7495
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
@ -221,10 +198,10 @@ Visualization Manager:
Z: -0.589262
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.530399
Pitch: 0.310399
Target Frame: base_stabilized
Value: Orbit (rviz)
Yaw: 3.21541
Yaw: 3.29541
Saved: ~
Window Geometry:
Camera:

28
src/att_ekf_node.cpp
View File

@ -23,7 +23,9 @@ ros::Subscriber imu_bias_sub;
bool imu_bias_initialized = false;
Vector3d gyro_bias, acc_bias;
ros::Publisher pose_pub, pose_gt_pub;
Vector3d currentPosition;
ros::Publisher pose_pub;
void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg)
{
@ -38,16 +40,14 @@ void imuCallback(const sensor_msgs::ImuConstPtr & msg)
double t = msg->header.stamp.toSec();
sensor_msgs::Imu imu_msg = *msg;
imu_q.push_back(make_pair(t, imu_msg));
}
//publish the groundtruth of the pose
geometry_msgs::PoseStamped pose;
pose.header.stamp = msg->header.stamp;
pose.header.frame_id = "/base_footprint";
pose.pose.orientation.w = msg->orientation.w;
pose.pose.orientation.x = msg->orientation.x;
pose.pose.orientation.y = msg->orientation.y;
pose.pose.orientation.z = msg->orientation.z;
pose_gt_pub.publish(pose);
void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
{
currentPosition(0) = msg->pose.position.x;
currentPosition(1) = msg->pose.position.y;
currentPosition(2) = msg->pose.position.z;
}
void imuBiasCallback(const sensor_msgs::ImuConstPtr & msg)
@ -68,7 +68,10 @@ void publish_pose()
MatrixXd m = att_ekf.get_rotation_matrix();
Quaterniond q = mat2quaternion(m);
pose.header.stamp = ros::Time(att_ekf.get_time());
pose.header.frame_id = "/base_footprint";
pose.header.frame_id = "/world";
pose.pose.position.x = currentPosition(0);
pose.pose.position.y = currentPosition(1);
pose.pose.position.z = currentPosition(2);
pose.pose.orientation.w = q.w();
pose.pose.orientation.x = q.x();
pose.pose.orientation.y = q.y();
@ -84,7 +87,8 @@ int main(int argc, char **argv)
imu_bias_sub = n.subscribe("/imu_bias", 100, imuBiasCallback);
ros::Subscriber mag_sub = n.subscribe("/magnetic_field", 100, magCallback);
pose_pub = n.advertise<geometry_msgs::PoseStamped>("/pose", 10);
pose_gt_pub = n.advertise<geometry_msgs::PoseStamped>("/pose_gt", 10);
ros::Subscriber pose_gt_sub = n.subscribe("/pose_gt", 100, poseCallback);
ros::Rate loop_rate(100);
while(ros::ok())
{