From ec04217a15b4c2ae49b435c25c47682d16ab0848 Mon Sep 17 00:00:00 2001
From: libing64 <libing64@gmail.com>
Date: Tue, 22 Mar 2016 20:19:56 +0800
Subject: [PATCH] fix bug about the base_footprint link, now use world as the
 frame id

---
 launch/att_ekf.launch        |  1 +
 rviz_cfg/att_estimation.rviz | 31 ++++---------------------------
 src/att_ekf_node.cpp         | 28 ++++++++++++++++------------
 3 files changed, 21 insertions(+), 39 deletions(-)

diff --git a/launch/att_ekf.launch b/launch/att_ekf.launch
index 888b0aa..dbf8e44 100644
--- a/launch/att_ekf.launch
+++ b/launch/att_ekf.launch
@@ -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"/>
diff --git a/rviz_cfg/att_estimation.rviz b/rviz_cfg/att_estimation.rviz
index 5109014..0a3d41f 100644
--- a/rviz_cfg/att_estimation.rviz
+++ b/rviz_cfg/att_estimation.rviz
@@ -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:
diff --git a/src/att_ekf_node.cpp b/src/att_ekf_node.cpp
index 74cd4e4..fce2621 100644
--- a/src/att_ekf_node.cpp
+++ b/src/att_ekf_node.cpp
@@ -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())
 	{