hubero_ros - full frame names published as params, loaded into Naviga…

…tionRos (fixed use_shared_map param issue)
rayvburn · Feb 20, 2022 · 5d7d431 · 5d7d431
1 parent 1aa92b4
commit 5d7d431
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 19 deletions.
diff --git a/hubero_ros/launch/actor.launch b/hubero_ros/launch/actor.launch
@@ -94,11 +94,11 @@
     <param name="hubero_ros/navigation/odometry_topic" value="$(arg nav_odometry_topic)"/>
     <param name="hubero_ros/navigation/nav_get_plan_tolerance" value="$(arg nav_get_plan_tolerance)"/>
 
-    <param name="hubero_ros/actor_frames/base" value="$(arg base_frame)"/>
-    <param name="hubero_ros/actor_frames/global_ref" value="$(arg map_frame)"/>
-    <param name="hubero_ros/actor_frames/local_ref" value="$(arg odom_frame)"/>
-    <param name="hubero_ros/actor_frames/lidar" value="$(arg lidar_frame)"/>
-    <param name="hubero_ros/actor_frames/camera" value="$(arg camera_frame)"/>
+    <param name="hubero_ros/actor_frames/base" value="$(arg actor_base_frame)"/>
+    <param name="hubero_ros/actor_frames/global_ref" value="$(arg actor_map_frame)"/>
+    <param name="hubero_ros/actor_frames/local_ref" value="$(arg actor_odom_frame)"/>
+    <param name="hubero_ros/actor_frames/lidar" value="$(arg actor_lidar_frame)"/>
+    <param name="hubero_ros/actor_frames/camera" value="$(arg actor_camera_frame)"/>
 
     <!-- If shared map approach is not used, map server will be started and static tf publisher will start too -->
     <!-- NOTE: in a shared_map approach, user MUST externally run 'shared' map server and publish 'shared' world-map static TF -->

diff --git 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,31 +180,31 @@ 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()
 		);
 	}
 
 	// 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);
 }