From 6d68d06787f7e70696f8244f0ed56d80168d7545 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sun, 30 Jan 2022 00:37:12 +0300
Subject: [PATCH] simple_offboard: default reference frames To simplify running
 with rosrun

---
 clover/launch/clover.launch    | 3 ---
 clover/src/simple_offboard.cpp | 7 +++++++
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch
index 26ec4454..bbf77e8b 100644
--- a/clover/launch/clover.launch
+++ b/clover/launch/clover.launch
@@ -47,9 +47,6 @@
 
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-        <param name="reference_frames/navigate_target" value="map"/>
         <param name="reference_frames/main_camera_optical" value="map"/>
     </node>
 
diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp
index db4d0dee..7e7aaaf9 100644
--- a/clover/src/simple_offboard.cpp
+++ b/clover/src/simple_offboard.cpp
@@ -873,6 +873,13 @@ int main(int argc, char **argv)
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);
 
+	// Default reference frames
+	std::map<string, string> default_reference_frames;
+	default_reference_frames[body.child_frame_id] = local_frame;
+	default_reference_frames[fcu_frame] = local_frame;
+	if (!target.child_frame_id.empty()) default_reference_frames[target.child_frame_id] = local_frame;
+	reference_frames.insert(default_reference_frames.begin(), default_reference_frames.end()); // merge defaults
+
 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
 	local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
 	velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));