Add Gazebo simulator to image

.github/workflows/build.yml

@@ -25,8 +25,8 @@ jobs:
     - uses: actions/checkout@v2
       with:
         path: catkin_ws/src/clover
-    - name: Install requirements
-      run: apt-get update && apt-get -y install python3-pip fakeroot python3-bloom debhelper dpkg-dev
+    - name: Install pip
+      run: apt-get update && apt-get -y install python3-pip
     - name: Install dependencies
       run: rosdep update && rosdep install --from-paths src --ignore-src -y
     - name: Install GeographicLib datasets
@@ -35,17 +35,5 @@ jobs:
       run: source /opt/ros/$ROS_DISTRO/setup.bash && catkin_make
     - name: Run tests
       run: source devel/setup.bash && catkin_make run_tests && catkin_test_results
-    - name: Build Debian packages
-      run: |
-        source devel/setup.bash
-        for file in `find . -name "package.xml"`; do
-          cd $(dirname ${file})
-          bloom-generate rosdebian --os-name ubuntu --os-version $(lsb_release -cs) --ros-distro $ROS_DISTRO  
-          fakeroot debian/rules binary
-          cd -
-        done
-    - uses: actions/upload-artifact@v3
-      with:
-        name: debian-packages
-        path: catkin_ws/src/clover/*.deb
-        retention-days: 1
+    - name: Install
+      run: source devel/setup.bash && catkin_make install

builder/image-build.sh

@@ -120,6 +120,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clover.rosinstall' '/home/pi/ros_catkin_ws/'
 # Add rename script
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-simulator.sh'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
 
 ${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH}

builder/image-simulator.sh

@@ -0,0 +1,42 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+
+set -ex # exit on error, echo commands
+
+echo "--- Downloading PX4"
+cd /home/pi/catkin_ws/
+git clone --recursive --depth 1 --branch v1.13.0 https://github.com/PX4/PX4-Autopilot.git /home/pi/PX4-Autopilot
+ln -s /home/pi/PX4-Autopilot /home/pi/catkin_ws/src/
+ln -s /home/pi/PX4-Autopilot/Tools/sitl_gazebo /home/pi/catkin_ws/src/
+
+echo "--- Installing PX4 dependencies"
+echo "progress=dot:giga" > /home/pi/.wgetrc # make wget don't spam to log
+apt-get install --no-install-recommends -y gazebo11
+/home/pi/PX4-Autopilot/Tools/setup/ubuntu.sh --no-nuttx
+rm /home/pi/.wgetrc
+pip3 install --user toml
+
+echo "--- Patching mavlink_sitl_gazebo"
+# See https://github.com/PX4/PX4-SITL_gazebo/pull/872
+cd /home/pi/PX4-Autopilot/Tools/sitl_gazebo
+patch -p1 < /tmp/patches/sitl_gazebo.patch
+
+echo "--- Build mavlink"
+cd /home/pi/catkin_ws
+catkin_make mavlink_c_generate -DCATKIN_WHITELIST_PACKAGES="px4"  # at first build PX4's mavlink to enforce mavlink_sitl_gazebo using it
+ln -s "." build/mavlink/mavlink  # fix https://github.com/PX4/PX4-Autopilot/pull/19964
+
+echo "--- Building the workspace"
+catkin_make -DCATKIN_WHITELIST_PACKAGES=""

clover/launch/main_camera.launch

@@ -4,8 +4,6 @@
     <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"/>
@@ -45,8 +43,4 @@
     <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,7 +78,6 @@ 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
@@ -95,7 +94,6 @@ PositionTarget position_raw_msg;
 AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
-Vector3 velocity_feedforward;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
 
@@ -343,15 +341,7 @@ inline float getDistance(const Point& from, const Point& to)
 	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
 }
 
-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)
+void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
 {
 	if (wait_armed) {
 		// don't start navigating if we're waiting arming
@@ -365,14 +355,6 @@ 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)
@@ -430,7 +412,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, velocity_feedforward);
+		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
 
 		if (setpoint_yaw_type == TOWARDS) {
 			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
@@ -446,31 +428,19 @@ void publish(const ros::Time stamp)
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
 		position_msg.header.stamp = stamp;
 
-		if (!nav_feedforward && setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
-			// simple pose setpoint without velocity feedforward and yaw rate
+		if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
 			position_pub.publish(position_msg);
 
 		} else {
-			position_raw_msg.type_mask = PositionTarget::IGNORE_AFX +
+			position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
+			                             PositionTarget::IGNORE_VY +
+			                             PositionTarget::IGNORE_VZ +
+			                             PositionTarget::IGNORE_AFX +
 			                             PositionTarget::IGNORE_AFY +
-			                             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;
-
+			                             PositionTarget::IGNORE_AFZ +
+			                             PositionTarget::IGNORE_YAW;
 			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);
 		}
 
@@ -890,13 +860,6 @@ 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");
@@ -916,7 +879,6 @@ 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);
@@ -971,7 +933,6 @@ 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,7 +24,6 @@ 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,9 +37,6 @@
 
     <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,8 +145,6 @@ 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` and `yaw`. Example: `map`, `body`, `aruco_map`. Default value: `map`.
+* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z`, `vx`, `vy`, `vz`. 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,16 +305,6 @@ 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/en/simulation_usage.md

@@ -97,13 +97,3 @@ PX4_SIM_SPEED_FACTOR=0.42 roslaunch clover_simulation simulator.launch
 The virtual machine may benefit from several CPU cores, especially if the cores are not very performant. In our tests, a four-core machine with only a single core allocated to the VM was unable to run the simulation, with constant interface freezes and dropped ROS messages. The same machine with all four cores available to the VM was able to run the simulation at 0.25 real-time speed.
 
 Do note that you should not allocate more resources than you have on your host hardware.
-
-### Changing the map of ArUco-markers in the simulator
-
-In order to change the map of ArUco-markers in the simulator, you can use the following command:
-
-```bash
-rosrun clover_simulation aruco_gen --single-model --source-world=$(catkin_find clover_simulation resources/worlds/clover.world) $(catkin_find aruco_pose map/map.txt) > $(catkin_find clover_simulation resources/worlds/clover_aruco.world)
-```
-
-In this example, `map.txt` is the name of markers name.

docs/en/snippets.md

@@ -240,30 +240,6 @@ ros_msg = mavlink.convert_to_rosmsg(msg)
 mavlink_pub.publish(ros_msg)
 ```
 
-<!-- markdownlint-disable MD044 -->
-
-### # {#mavlink-receive}
-
-<!-- markdownlint-enable MD044 -->
-
-Subscribe to all MAVLink messages from the flight controller and decode them:
-
-```python
-from mavros_msgs.msg import Mavlink
-from mavros import mavlink
-from pymavlink import mavutil
-
-link = mavutil.mavlink.MAVLink('', 255, 1)
-
-def mavlink_cb(msg):
-    mav_msg = link.decode(mavlink.convert_to_bytes(msg))
-    print('msgid =', msg.msgid, mav_msg) # print message id and parsed message
-
-mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_cb)
-
-rospy.spin()
-```
-
 ### # {#rc-sub}
 
 React to the drone's mode switching (may be used for starting an autonomous flight, see [example](https://gist.github.com/okalachev/b709f04522d2f9af97e835baedeb806b)):

docs/ru/camera.md

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

docs/ru/simulation_usage.md

@@ -99,13 +99,3 @@ PX4_SIM_SPEED_FACTOR=0.42 roslaunch clover_simulation simulator.launch
 Выделение нескольких процессорных ядер для виртуальной машины может значительно повысить производительность симуляции. В наших испытаниях виртуальная машина, для которой было выделено одно ядро, не позволяла работать в симуляторе: окно Gazebo не реагировало на пользовательский ввод, сообщения ROS терялись. После выделения четырёх ядер для этой же виртуальной машины симуляция стала работать со скоростью 0.25 от реального времени.
 
 При этом не следует пытаться выделить для виртуальной машины больше ресурсов, чем доступно на основной системе.
-
-### Изменение карты ArUco-меток в симуляторе
-
-Для того, чтобы изменить карту ArUco-меток в симуляторе, можно использовать следующую команду:
-
-```bash
-rosrun clover_simulation aruco_gen --single-model --source-world=$(catkin_find clover_simulation resources/worlds/clover.world) $(catkin_find aruco_pose map/map.txt) > $(catkin_find clover_simulation resources/worlds/clover_aruco.world)
-```
-
-В данном примере `map.txt` – имя карты меток.

docs/ru/snippets.md

@@ -251,30 +251,6 @@ ros_msg = mavlink.convert_to_rosmsg(msg)
 mavlink_pub.publish(ros_msg)
 ```
 
-<!-- markdownlint-disable MD044 -->
-
-### # {#mavlink-receive}
-
-<!-- markdownlint-enable MD044 -->
-
-Подписка на все MAVLink-сообщения от полетного контроллера и их декодирование:
-
-```python
-from mavros_msgs.msg import Mavlink
-from mavros import mavlink
-from pymavlink import mavutil
-
-link = mavutil.mavlink.MAVLink('', 255, 1)
-
-def mavlink_cb(msg):
-    mav_msg = link.decode(mavlink.convert_to_bytes(msg))
-    print('msgid =', msg.msgid, mav_msg) # print message id and parsed message
-
-mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_cb)
-
-rospy.spin()
-```
-
 ### # {#rc-sub}
 
 Реакция на переключение режима на пульте радиоуправления (может быть использовано для запуска автономного полета, см. [пример](https://gist.github.com/okalachev/b709f04522d2f9af97e835baedeb806b)):