diff --git a/hubero_ros/launch/actor.launch b/hubero_ros/launch/actor.launch
index 35ca42df..31256f17 100644
--- a/hubero_ros/launch/actor.launch
+++ b/hubero_ros/launch/actor.launch
@@ -94,11 +94,11 @@
-
-
-
-
-
+
+
+
+
+
diff --git a/hubero_ros/src/navigation_ros.cpp b/hubero_ros/src/navigation_ros.cpp
index fec04c9a..37b36e1d 100644
--- a/hubero_ros/src/navigation_ros.cpp
+++ b/hubero_ros/src/navigation_ros.cpp
@@ -156,18 +156,12 @@ void NavigationRos::update(const Pose3& pose, const Vector3& vel_lin, const Vect
NavigationBase::update(pose);
- // prepare full frame names
- auto frame_global_ref_full = actor_name_ + "/" + frame_global_ref_;
- auto frame_local_ref_full = actor_name_ + "/" + frame_local_ref_;
- auto frame_base_full = actor_name_ + "/" + frame_base_;
- auto frame_laser_full = actor_name_ + "/" + frame_laser_;
-
// publish odom
nav_msgs::Odometry odometry {};
// header
odometry.header.stamp = ros::Time::now();
- odometry.header.frame_id = frame_local_ref_full;
- odometry.child_frame_id = frame_base_full;
+ odometry.header.frame_id = frame_local_ref_;
+ odometry.child_frame_id = frame_base_;
// pose
Pose3 odom_pose = pose - pose_initial_;
odometry.pose.pose = ignPoseToMsgPose(odom_pose);
@@ -186,14 +180,14 @@ void NavigationRos::update(const Pose3& pose, const Vector3& vel_lin, const Vect
*/
Pose3 global_ref_shift;
try {
- auto tf_world_global_ref = tf_buffer_.lookupTransform(frame_global_ref_full, world_frame_name_, ros::Time(0));
+ auto tf_world_global_ref = tf_buffer_.lookupTransform(frame_global_ref_, world_frame_name_, ros::Time(0));
global_ref_shift = msgTfToPose(tf_world_global_ref.transform);
} catch (tf2::TransformException& ex) {
HUBERO_LOG(
"[%s].[NavigationRos] Could not transform '%s' to '%s' - exception: '%s'",
actor_name_.c_str(),
world_frame_name_.c_str(),
- frame_global_ref_full.c_str(),
+ frame_global_ref_.c_str(),
ex.what()
);
}
@@ -201,16 +195,16 @@ void NavigationRos::update(const Pose3& pose, const Vector3& vel_lin, const Vect
// publish odom TF
geometry_msgs::TransformStamped transform_odom {};
transform_odom.header.stamp = ros::Time::now();
- transform_odom.header.frame_id = frame_global_ref_full;
- transform_odom.child_frame_id = frame_local_ref_full;
+ transform_odom.header.frame_id = frame_global_ref_;
+ transform_odom.child_frame_id = frame_local_ref_;
transform_odom.transform = ignPoseToMsgTf(pose_initial_ - global_ref_shift);
tf_broadcaster_.sendTransform(transform_odom);
// publish actor base TF
geometry_msgs::TransformStamped transform_actor {};
transform_actor.header.stamp = ros::Time::now();
- transform_actor.header.frame_id = frame_local_ref_full;
- transform_actor.child_frame_id = frame_base_full;
+ transform_actor.header.frame_id = frame_local_ref_;
+ transform_actor.child_frame_id = frame_base_;
transform_actor.transform = ignPoseToMsgTf(pose - pose_initial_);
tf_broadcaster_.sendTransform(transform_actor);
}