simple_offboard: implement navigate velocity feedforward

clover/src/simple_offboard.cpp

@@ -37,7 +37,6 @@
 #include <mavros_msgs/State.h>
 #include <mavros_msgs/StatusText.h>
 #include <mavros_msgs/ManualControl.h>
-#include <mavros_msgs/Altitude.h>
 
 #include <clover/GetTelemetry.h>
 #include <clover/Navigate.h>
@@ -55,7 +54,6 @@ using namespace clover;
 using mavros_msgs::PositionTarget;
 using mavros_msgs::AttitudeTarget;
 using mavros_msgs::Thrust;
-using mavros_msgs::Altitude;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
@@ -80,6 +78,7 @@ ros::Duration manual_control_timeout;
 float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
+bool nav_feedforward;
 std::map<string, string> reference_frames;
 
 // Publishers
@@ -96,14 +95,13 @@ PositionTarget position_raw_msg;
 AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
+Vector3 velocity_feedforward;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
-geometry_msgs::TransformStamped terrain;
 
 // State
 PoseStamped nav_start;
 PoseStamped setpoint_position, setpoint_position_transformed;
-Vector3Stamped setpoint_z, setpoint_z_transformed; // for z-only commands
 Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
 QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
 float setpoint_yaw_rate;
@@ -126,8 +124,6 @@ enum setpoint_type_t setpoint_type = NONE;
 
 enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type;
 
-bool setpoint_z_valid = false;
-
 // Last received telemetry messages
 mavros_msgs::State state;
 mavros_msgs::StatusText statustext;
@@ -179,15 +175,6 @@ void handleLocalPosition(const PoseStamped& pose)
 	// TODO: terrain?, home?
 }
 
-void handleAltitude(const Altitude& alt)
-{
-	// publish terrain frame
-	if (!std::isfinite(alt.bottom_clearance)) return;
-	terrain.header.stamp = alt.header.stamp;
-	terrain.transform.translation.z = -alt.bottom_clearance;
-	transform_broadcaster->sendTransform(terrain);
-}
-
 // wait for transform without interrupting publishing setpoints
 inline bool waitTransform(const string& target, const string& source,
                           const ros::Time& stamp, const ros::Duration& timeout) // editorconfig-checker-disable-line
@@ -356,7 +343,15 @@ inline float getDistance(const Point& from, const Point& to)
 	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
 }
 
-void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
+inline void normalize(Vector3& vector, double length = 1.0)
+{
+	double len = hypot(vector.x, vector.y, vector.z);
+	vector.x *= length / len;
+	vector.y *= length / len;
+	vector.z *= length / len;
+}
+
+void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint, Vector3& velocity_feedforward)
 {
 	if (wait_armed) {
 		// don't start navigating if we're waiting arming
@@ -370,6 +365,14 @@ void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoin
 	nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
 	nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
 	nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
+
+	if (nav_feedforward) {
+		// calculate velocity feedforward
+		velocity_feedforward.x = setpoint_position_transformed.pose.position.x - nav_start.pose.position.x;
+		velocity_feedforward.y = setpoint_position_transformed.pose.position.y - nav_start.pose.position.y;
+		velocity_feedforward.z = setpoint_position_transformed.pose.position.z - nav_start.pose.position.z;
+		normalize(velocity_feedforward, speed);
+	}
 }
 
 PoseStamped globalToLocal(double lat, double lon)
@@ -427,7 +430,7 @@ void publish(const ros::Time stamp)
 
 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
 		position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
-		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
+		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position, velocity_feedforward);
 
 		if (setpoint_yaw_type == TOWARDS) {
 			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
@@ -436,18 +439,6 @@ void publish(const ros::Time stamp)
 		}
 	}
 
-	if (setpoint_z_valid && setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		setpoint_z.header.stamp = stamp;
-		try {
-			tf_buffer.transform(setpoint_z, setpoint_z_transformed, local_frame, ros::Duration(0.05));
-			setpoint_position_transformed.pose.position.z = setpoint_z_transformed.vector.z;
-
-		} catch (const tf2::TransformException& e) {
-			ROS_WARN_THROTTLE(10, "can't transform z coordinate from %s to %s, ignoring z coordinate", 
-			                  setpoint_z.header.frame_id.c_str(), local_frame.c_str());
-		}
-	}
-
 	if (setpoint_type == POSITION) {
 		position_msg = setpoint_position_transformed;
 	}
@@ -455,19 +446,31 @@ void publish(const ros::Time stamp)
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
 		position_msg.header.stamp = stamp;
 
-		if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
+		if (!nav_feedforward && setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
+			// simple pose setpoint without velocity feedforward and yaw rate
 			position_pub.publish(position_msg);
 
 		} else {
-			position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
+			position_raw_msg.type_mask = PositionTarget::IGNORE_AFX +
-			                             PositionTarget::IGNORE_VY +
-			                             PositionTarget::IGNORE_VZ +
-			                             PositionTarget::IGNORE_AFX +
 			                             PositionTarget::IGNORE_AFY +
-			                             PositionTarget::IGNORE_AFZ +
+			                             PositionTarget::IGNORE_AFZ;
-			                             PositionTarget::IGNORE_YAW;
+
+			if (!nav_feedforward) {
+				position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
+				                             PositionTarget::IGNORE_VY +
+				                             PositionTarget::IGNORE_VZ;
+
+			}
+
+			position_raw_msg.type_mask += setpoint_yaw_type == YAW_RATE ?
+				PositionTarget::IGNORE_YAW : PositionTarget::IGNORE_YAW_RATE;
+
 			position_raw_msg.yaw_rate = setpoint_yaw_rate;
+			position_raw_msg.yaw = setpoint_yaw;
 			position_raw_msg.position = position_msg.pose.position;
+			position_raw_msg.velocity.x = velocity_feedforward.x;
+			position_raw_msg.velocity.y = velocity_feedforward.y;
+			position_raw_msg.velocity.z = velocity_feedforward.z;
 			position_raw_pub.publish(position_raw_msg);
 		}
 
@@ -583,7 +586,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 
 		// look up for reference frame
 		auto search = reference_frames.find(frame_id);
-		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second; // when not found it's the same frame
+		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
 
 		// Serve "partial" commands
 
@@ -610,33 +613,22 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			}
 		}
 
-		if (!auto_arm && std::isfinite(z) &&
+		if (!auto_arm && std::isfinite(yaw_rate) &&
-		    isnan(x) && isnan(y) && isnan(vx) && isnan(vy) && isnan(vz) &&
+		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
-		    isnan(pitch) && isnan(roll) && isnan(thrust) &&
+		    isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
 		    isnan(lat) && isnan(lon)) {
-			// set only the z
+			// change only the yaw rate
-			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
+			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
-				if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
+				message = "Changing yaw rate only";
-					throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);
 
-				message = "Changing z only";
+				setpoint_yaw_type = YAW_RATE;
-
+				setpoint_yaw_rate = yaw_rate;
-				setpoint_z.header.frame_id = frame_id;
-				setpoint_z.header.stamp = stamp;
-				setpoint_z.vector.z = z;
-				setpoint_z_valid = true;
 				goto publish_setpoint;
-
 			} else {
-				throw std::runtime_error("Setting z is possible only when position setpoint active");
+				throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
 			}
 		}
 
-		// commands without z
-		if (isnan(z) && setpoint_z_valid && (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION)) {
-			z = 0;
-		}
-
 		// Serve normal commands
 
 		if (sp_type == NAVIGATE || sp_type == POSITION) {
@@ -924,9 +916,9 @@ int main(int argc, char **argv)
 	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
 	nh_priv.param("nav_from_sp", nav_from_sp, true);
 	nh_priv.param("check_kill_switch", check_kill_switch, true);
+	nh_priv.param("nav_feedforward", nav_feedforward, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
-	nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	// Default reference frames
@@ -962,13 +954,6 @@ int main(int argc, char **argv)
 	auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
 	auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
 
-	ros::Subscriber altitude_sub;
-	if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
-		terrain.header.frame_id = body.child_frame_id;
-		terrain.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
-		altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
-	}
-
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
 	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);

docs/en/simple_offboard.md

@@ -103,7 +103,7 @@ Parameters:
 * `yaw_rate` – angular yaw velocity (will be used if yaw is set to `NaN`) *(rad/s)*;
 * `speed` – flight speed (setpoint speed) *(m/s)*;
 * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
-* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z`, `vx`, `vy`, `vz`. Example: `map`, `body`, `aruco_map`. Default value: `map`.
+* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z` and `yaw`. Example: `map`, `body`, `aruco_map`. Default value: `map`.
 
 > **Note** If you don't want to change your current yaw set the `yaw` parameter to `NaN` (angular velocity by default is 0).