Add simple_offboard/state topic publishing and some tests

clover/CMakeLists.txt

@@ -80,11 +80,10 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
+add_message_files(
-#   FILES
+  FILES
-#   Message1.msg
+  State.msg
-#   Message2.msg
+)
-# )
 
 ## Generate services in the 'srv' folder
 add_service_files(

clover/msg/State.msg

@@ -0,0 +1,38 @@
+uint8 MODE_NONE = 0
+uint8 MODE_NAVIGATE = 1
+uint8 MODE_NAVIGATE_GLOBAL = 2
+uint8 MODE_POSITION = 3
+uint8 MODE_VELOCITY = 4
+uint8 MODE_ATTITUDE = 5
+uint8 MODE_RATES = 6
+
+uint8 YAW_MODE_YAW = 0
+uint8 YAW_MODE_YAW_RATE = 1
+uint8 YAW_MODE_YAW_TOWARDS = 2
+
+# type of offboard control
+uint8 mode
+uint8 yaw_mode
+
+# targets
+float32 x
+float32 y
+float32 z
+float32 speed
+float32 lat
+float32 lon
+float32 vx
+float32 vy
+float32 vz
+float32 roll
+float32 pitch
+float32 yaw
+float32 roll_rate
+float32 pitch_rate
+float32 yaw_rate
+float32 thrust
+
+# frames of reference
+string xy_frame_id
+string z_frame_id
+string yaw_frame_id

clover/src/simple_offboard.cpp

@@ -46,6 +46,7 @@
 #include <clover/SetVelocity.h>
 #include <clover/SetAttitude.h>
 #include <clover/SetRates.h>
+#include <clover/State.h>
 
 using std::string;
 using std::isnan;
@@ -82,7 +83,7 @@ bool land_only_in_offboard, nav_from_sp, check_kill_switch;
 std::map<string, string> reference_frames;
 
 // Publishers
-ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
+ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
 
 // Service clients
 ros::ServiceClient arming, set_mode;
@@ -106,6 +107,7 @@ Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
 QuaternionStamped setpoint_attitude, setpoint_attitude_transformed; // attitude or only the yaw
 Vector3 setpoint_rates;
 float nav_speed;
+float setpoint_lat, setpoint_lon;
 bool busy = false;
 bool wait_armed = false;
 bool nav_from_sp_flag = false;
@@ -553,6 +555,44 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
 }
 
+void publishState()
+{
+	clover::State msg;
+	msg.mode = setpoint_type;
+	msg.yaw_mode = setpoint_yaw_type;
+	msg.x = setpoint_position.pose.position.x;
+	msg.y = setpoint_position.pose.position.y;
+	msg.z = setpoint_position.pose.position.z;
+	msg.speed = nav_speed;
+	msg.lat = setpoint_lat;
+	msg.lon = setpoint_lon;
+	msg.vx = setpoint_velocity.vector.x;
+	msg.vy = setpoint_velocity.vector.y;
+	msg.vz = setpoint_velocity.vector.z;
+
+	double roll, pitch, yaw;
+	tf2::getEulerYPR(setpoint_attitude.quaternion, yaw, pitch, roll);
+	msg.roll = roll;
+	msg.pitch = pitch;
+	msg.yaw = yaw;
+
+	msg.roll_rate = setpoint_rates.x;
+	msg.pitch_rate = setpoint_rates.y;
+	msg.yaw_rate = setpoint_rates.z;
+	msg.thrust = thrust_msg.thrust;
+
+	if (setpoint_type == VELOCITY) {
+		msg.xy_frame_id = setpoint_velocity.header.frame_id;
+		msg.z_frame_id = setpoint_velocity.header.frame_id;
+	} else {
+		msg.xy_frame_id = setpoint_position.header.frame_id;
+		msg.z_frame_id = setpoint_altitude.header.frame_id;
+	}
+	msg.yaw_frame_id = setpoint_attitude.header.frame_id;
+
+	state_pub.publish(msg);
+}
+
 #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
 
 bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
@@ -691,6 +731,8 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
 			x = xy_in_req_frame.pose.position.x;
 			y = xy_in_req_frame.pose.position.y;
+			setpoint_lat = lat;
+			setpoint_lon = lon;
 		}
 
 		// Everything fine - switch setpoint type
@@ -798,6 +840,8 @@ publish_setpoint:
 			}
 		}
 
+		publishState();
+
 		if (auto_arm) {
 			offboardAndArm();
 			wait_armed = false;
@@ -897,6 +941,8 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	setpoint_timer.stop();
+	setpoint_type = NONE;
+	publishState();
 	res.success = true;
 	return true;
 }
@@ -965,6 +1011,9 @@ int main(int argc, char **argv)
 	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
 	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
 
+	// State publisher
+	state_pub = nh_priv.advertise<clover::State>("state", 1, true);
+
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
 	auto na_serv = nh.advertiseService("navigate", &navigate);

clover/test/offboard.py

@@ -5,12 +5,16 @@ import threading
 import mavros_msgs.msg
 from geometry_msgs.msg import PoseStamped
 from clover import srv
+from clover.msg import State
 from math import nan
 
 @pytest.fixture()
 def node():
     return rospy.init_node('offboard_test', anonymous=True)
 
+def get_state():
+    return rospy.wait_for_message('/simple_offboard/state', State, timeout=1)
+
 def test_offboard(node):
     navigate = rospy.ServiceProxy('navigate', srv.Navigate)
     get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
@@ -68,5 +72,13 @@ def test_offboard(node):
     threading.Thread(target=publish_local_position, daemon=True).start()
     rospy.sleep(0.5)
 
-    res = navigate()
+    res = navigate(z=1, frame_id='map')
     assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'