simple_offboard: implement navigate velocity feedforward

clover/launch/main_camera.launch

@@ -4,6 +4,8 @@
     <arg name="direction_z" default="down"/> <!-- direction the camera points: down, up -->
     <arg name="direction_y" default="backward"/> <!-- direction the camera cable points: backward, forward -->
     <arg name="device" default="/dev/video0"/> <!-- v4l2 device -->
+    <arg name="throttled_topic" default="true"/> <!-- enable throttled image topic -->
+    <arg name="throttled_topic_rate" default="5.0"/> <!-- throttled image topic rate -->
     <arg name="simulator" default="false"/>
 
     <node if="$(eval direction_z == 'down' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
@@ -43,4 +45,8 @@
     <node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
         <param name="scale" value="3.0"/>
     </node>
+
+    <!-- image topic throttled -->
+    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
+        args="messages image_raw $(arg throttled_topic_rate) image_raw_throttled" if="$(arg throttled_topic)"/>
 </launch>

clover/src/simple_offboard.cpp

@@ -78,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
@@ -94,6 +95,7 @@ PositionTarget position_raw_msg;
 AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
+Vector3 velocity_feedforward;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
 
@@ -341,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
@@ -355,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)
@@ -412,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,
@@ -428,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 +
-			                             PositionTarget::IGNORE_VY +
-			                             PositionTarget::IGNORE_VZ +
-			                             PositionTarget::IGNORE_AFX +
+			position_raw_msg.type_mask = PositionTarget::IGNORE_AFX +
 			                             PositionTarget::IGNORE_AFY +
-			                             PositionTarget::IGNORE_AFZ +
-			                             PositionTarget::IGNORE_YAW;
+			                             PositionTarget::IGNORE_AFZ;
+
+			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);
 		}
 
@@ -860,6 +890,13 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 	return false;
 }
 
+bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+{
+	setpoint_timer.stop();
+	res.success = true;
+	return true;
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "simple_offboard");
@@ -879,6 +916,7 @@ 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.getParam("reference_frames", reference_frames);
@@ -933,6 +971,7 @@ int main(int argc, char **argv)
 	auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
 	auto sr_serv = nh.advertiseService("set_rates", &setRates);
 	auto ld_serv = nh.advertiseService("land", &land);
+	auto rl_serv = nh_priv.advertiseService("release", &release);
 
 	// Setpoint timer
 	setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);

clover/test/basic.py

@@ -24,6 +24,7 @@ def test_simple_offboard_services_available():
     rospy.wait_for_service('set_attitude', timeout=5)
     rospy.wait_for_service('set_rates', timeout=5)
     rospy.wait_for_service('land', timeout=5)
+    rospy.wait_for_service('simple_offboard/release', timeout=5)
 
 def test_web_video_server(node):
     try:

clover/test/basic.test

@@ -37,6 +37,9 @@
 
     <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
 
+    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
+        args="messages image_raw 5.0 image_raw_throttled" required="true"/>
+
     <param name="test_module" value="$(find clover)/test/basic.py"/>
     <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>

docs/en/camera.md

@@ -145,6 +145,8 @@ rospy.spin()
 
 The script will take up to 100% CPU capacity. To slow down the script artificially, you can use [throttling](http://wiki.ros.org/topic_tools/throttle) of frames from the camera, for example, at 5 Hz (`main_camera.launch`):
 
+> **Note** Starting from [image](image.md) version **0.24** `image_raw_throttled` topic is available without addition configuration.
+
 ```xml
 <node pkg="topic_tools" name="cam_throttle" type="throttle"
     args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>

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).
 
@@ -305,6 +305,16 @@ rosservice call /land "{}"
 
 > **Caution** In recent PX4 versions, the vehicle will be switched out of LAND mode to manual mode, if the remote control sticks are moved significantly.
 
+### release
+
+If it's necessary to pause sending setpoint messages, use the `simple_offboard/release` service:
+
+```python
+release = rospy.ServiceProxy('simple_offboard/release', Trigger)
+
+release()
+```
+
 ## Additional materials
 
 * [ArUco-based position estimation and navigation](aruco.md).

docs/ru/camera.md

@@ -147,6 +147,8 @@ rospy.spin()
 
 Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`):
 
+> **Note** Начиная с версии [образа](image.md) **0.24** топик `image_raw_throttled` доступен без дополнительной конфигурации.
+
 ```xml
 <node pkg="topic_tools" name="cam_throttle" type="throttle"
     args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>

docs/ru/simple_offboard.md

@@ -305,6 +305,16 @@ rosservice call /land "{}"
 
 > **Caution** В более новых версиях PX4 коптер выйдет из режима LAND в ручной режим, если сильно перемещать стики.
 
+### release
+
+В случае необходимости приостановки отправки setpoint-сообщений, используйте сервис `simple_offboard/release`:
+
+```python
+release = rospy.ServiceProxy('simple_offboard/release', Trigger)
+
+release()
+```
+
 ## Дополнительные материалы
 
 * [Полеты в поле ArUco-маркеров](aruco.md).