Merge pull request #14 from trainman419/fix_frame_id

Use configured frame IDs
ros-perception · Jun 11, 2014 · 5bb48b2 · 5bb48b2
2 parents 82c8ea1 + 9308c2f
commit 5bb48b2
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 9 deletions.
diff --git a/src/graft_ukf_absolute.cpp b/src/graft_ukf_absolute.cpp
@@ -53,6 +53,9 @@ nav_msgs::Odometry odom_;
 bool publish_tf_;
 boost::shared_ptr<tf::TransformBroadcaster> broadcaster_;
 
+std::string parent_frame_id_;
+std::string child_frame_id_;
+
 void publishTF(const nav_msgs::Odometry& msg){
   geometry_msgs::TransformStamped tf;
   tf.header.stamp = msg.header.stamp;
@@ -76,8 +79,8 @@ void timer_callback(const ros::TimerEvent& event){
 
 	// Update Odometry
 	odom_.header.stamp = ros::Time::now();
-	odom_.header.frame_id = "map";
-	odom_.child_frame_id = "base_link";
+	odom_.header.frame_id = parent_frame_id_;
+	odom_.child_frame_id = child_frame_id_;
 	odom_.pose.pose = state.pose;
 	odom_.twist.twist = state.twist;
 	odom_pub.publish(odom_);
@@ -102,6 +105,9 @@ int main(int argc, char **argv)
 
 	publish_tf_ = manager.getPublishTF();
 
+	parent_frame_id_ = manager.getParentFrameID();
+	child_frame_id_ = manager.getChildFrameID();
+
 	// Set up the E
 	std::vector<double> initial_covariance = manager.getInitialCovariance();
 	std::vector<double> Q = manager.getProcessNoise();
@@ -136,4 +142,4 @@ int main(int argc, char **argv)
 
 	// Spin
 	ros::spin();
-}
+}
diff --git a/src/graft_ukf_attitude.cpp b/src/graft_ukf_attitude.cpp
@@ -53,6 +53,9 @@ nav_msgs::Odometry odom_;
 bool publish_tf_;
 boost::shared_ptr<tf::TransformBroadcaster> broadcaster_;
 
+std::string parent_frame_id_;
+std::string child_frame_id_;
+
 void publishTF(const nav_msgs::Odometry& msg){
   geometry_msgs::TransformStamped tf;
   tf.header.stamp = msg.header.stamp;
@@ -75,8 +78,8 @@ void timer_callback(const ros::TimerEvent& event){
 	state_pub.publish(state);
 
 	odom_.header.stamp = ros::Time::now();
-	odom_.header.frame_id = "odom_fused_ukfv";
-	odom_.child_frame_id = "base_link";
+	odom_.header.frame_id = parent_frame_id_;
+	odom_.child_frame_id = child_frame_id_;
 	odom_.pose.pose.orientation = state.pose.orientation;
 	odom_.twist.twist.angular = state.twist.angular;
 
@@ -102,6 +105,9 @@ int main(int argc, char **argv)
 
 	publish_tf_ = manager.getPublishTF();
 
+	parent_frame_id_ = manager.getParentFrameID();
+	child_frame_id_ = manager.getChildFrameID();
+
 	// Set up the E
 	std::vector<double> initial_covariance = manager.getInitialCovariance();
 	std::vector<double> Q = manager.getProcessNoise();
@@ -136,4 +142,4 @@ int main(int argc, char **argv)
 
 	// Spin
 	ros::spin();
-}
+}
diff --git a/src/graft_ukf_velocity.cpp b/src/graft_ukf_velocity.cpp
@@ -53,6 +53,9 @@ nav_msgs::Odometry odom_;
 bool publish_tf_;
 boost::shared_ptr<tf::TransformBroadcaster> broadcaster_;
 
+std::string parent_frame_id_;
+std::string child_frame_id_;
+
 void publishTF(const nav_msgs::Odometry& msg){
   geometry_msgs::TransformStamped tf;
   tf.header.stamp = msg.header.stamp;
@@ -75,8 +78,8 @@ void timer_callback(const ros::TimerEvent& event){
 	state_pub.publish(state);
 
 	odom_.header.stamp = ros::Time::now();
-	odom_.header.frame_id = "odom_fused_ukfv";
-	odom_.child_frame_id = "base_link";
+	odom_.header.frame_id = parent_frame_id_;
+	odom_.child_frame_id = child_frame_id_;
 	odom_.twist.twist.linear.x = state.twist.linear.x;
 	odom_.twist.twist.linear.y = state.twist.linear.y;
 	odom_.twist.twist.angular.z = state.twist.angular.z;
@@ -123,6 +126,9 @@ int main(int argc, char **argv)
 
 	publish_tf_ = manager.getPublishTF();
 
+	parent_frame_id_ = manager.getParentFrameID();
+	child_frame_id_ = manager.getChildFrameID();
+
 	// Set up the E
 	std::vector<double> initial_covariance = manager.getInitialCovariance();
 	std::vector<double> Q = manager.getProcessNoise();
@@ -157,4 +163,4 @@ int main(int argc, char **argv)
 
 	// Spin
 	ros::spin();
-}
+}