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); }