Add Gazebo simulator to image

.github/workflows/build.yml

@@ -16,8 +16,24 @@ jobs:
   #         docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
   noetic:
     runs-on: ubuntu-latest
+    container: ros:noetic-ros-base
+    defaults:
+      run:
+        working-directory: catkin_ws
+        shell: bash
     steps:
-      - uses: actions/checkout@v2
+    - uses: actions/checkout@v2
-      - name: Native Noetic build
+      with:
-        run: |
+        path: catkin_ws/src/clover
-          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
+    - 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
+      run: wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
+    - name: catkin_make
+      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: Install
+      run: source devel/setup.bash && catkin_make install

aruco_pose/CMakeLists.txt

@@ -251,4 +251,5 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
   add_rostest(test/crash_opencv.test)
+  add_rostest(test/duplicate.test)
 endif()

aruco_pose/src/aruco_detect.cpp

@@ -187,6 +187,8 @@ private:
 
 			array_.markers.reserve(ids.size());
 			aruco_pose::Marker marker;
+			vector<geometry_msgs::TransformStamped> transforms;
+			transforms.reserve(ids.size());
 			geometry_msgs::TransformStamped transform;
 			transform.header.stamp = msg->header.stamp;
 			transform.header.frame_id = msg->header.frame_id;
@@ -204,20 +206,33 @@ private:
 						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
 					}
 
-					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
 
 						// check if such static transform is in the map
 						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
-							transform.transform.rotation = marker.pose.orientation;
+							// check if a markers with that id is already added
-							fillTranslation(transform.transform.translation, tvecs[i]);
+							bool send = true;
-							br_->sendTransform(transform);
+							for (auto &t : transforms) {
+								if (t.child_frame_id == transform.child_frame_id) {
+									send = false;
+									break;
+								}
+							}
+							if (send) {
+								transform.transform.rotation = marker.pose.orientation;
+								fillTranslation(transform.transform.translation, tvecs[i]);
+								transforms.push_back(transform);
+							}
 						}
 					}
 				}
 				array_.markers.push_back(marker);
 			}
+
+			if (send_tf_) {
+				br_->sendTransform(transforms);
+			}
 		}
 
 		markers_pub_.publish(array_);

aruco_pose/test/duplicate.py

@@ -0,0 +1,8 @@
+import pytest
+import subprocess
+
+def test_no_tf_repeated_data():
+    # `/rosout` acts weirdly inside rostest, so using a subprocess
+    cmd = """python -c 'import rospy, tf; rospy.init_node("foo"); listener = tf.TransformListener(); rospy.sleep(2)'"""
+    output = str(subprocess.check_output(cmd, shell=True, stderr=subprocess.STDOUT))
+    assert 'TF_REPEATED_DATA' not in output, 'TF_REPEATED_DATA was logged on duplicate markers'

aruco_pose/test/duplicate.test

@@ -0,0 +1,21 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/duplicate.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="cornerRefinementMethod" value="1"/>
+    </node>
+
+	<param name="test_module" value="$(find aruco_pose)/test/duplicate.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

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/clover.launch

@@ -12,7 +12,7 @@
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
     <arg name="rc" default="false"/>
-    <arg name="force_init" value="true"/> <!-- force estimator to init by publishing zero pose -->
+    <arg name="force_init" default="true"/> <!-- force estimator to init by publishing zero pose -->
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 

clover/www/js/topics.js

@@ -40,6 +40,7 @@ function viewTopicsList() {
 let rosdistro;
 
 function viewTopic(topic) {
+	let counter = 0;
 	let index = '<a href=topics.html>Topics</a>';
 	title.innerHTML = `${index}: ${topic}`;
 	topicMessage.style.display = 'block';
@@ -51,10 +52,11 @@ function viewTopic(topic) {
 	});
 
 	new ROSLIB.Topic({ ros: ros, name: topic }).subscribe(function(msg) {
+		counter++;
 		document.title = topic;
 		if (mouseDown) return;
 
-		if (msg.header.stamp) {
+		if (msg.header && msg.header.stamp) {
 			if (params.date || params.offset) {
 				let date = new Date(msg.header.stamp.secs * 1e3 + msg.header.stamp.nsecs * 1e-6);
 				if (params.date) msg.header.date = date.toISOString();
@@ -62,7 +64,8 @@ function viewTopic(topic) {
 			}
 		}
 
-		topicMessage.innerHTML = yamlStringify(msg); // JSON.stringify(msg, null, 4);
+		let txt = `<div class=counter>${counter} received</div>${yamlStringify(msg)}`; // JSON.stringify(msg, null, 4);
+		topicMessage.innerHTML = txt;
 	});
 }
 

clover/www/topics.html

@@ -15,6 +15,7 @@
 				white-space: pre;
 				font-family: monospace;
 			}
+			.counter { color: #b9b9b9; margin-bottom: 1em; }
 			#topic-type { font-family: monospace; font-size: 0.5em; vertical-align: super; font-weight: normal; }
 			.topic { font-family: monospace; }
 			body.closed { background-color: rgb(207, 207, 207); }

clover_description/launch/spawn_drone.launch

@@ -10,9 +10,8 @@
     <arg name="gps" default="true"/>
     <!-- Use physics parameters from CAD programs -->
     <arg name="use_clover_physics" default="false"/>
-    <arg name="hitl" default="false"/>
 
-    <arg name="cmd" default="$(find xacro)/xacro $(find clover_description)/urdf/clover/clover4.xacro main_camera:=$(arg main_camera) rangefinder:=$(arg rangefinder) led:=$(arg led) gps:=$(arg gps) maintain_camera_rate:=$(arg maintain_camera_rate) use_clover_physics:=$(arg use_clover_physics) hitl:=$(arg hitl)"/>
+    <arg name="cmd" default="$(find xacro)/xacro $(find clover_description)/urdf/clover/clover4.xacro main_camera:=$(arg main_camera) rangefinder:=$(arg rangefinder) led:=$(arg led) gps:=$(arg gps) maintain_camera_rate:=$(arg maintain_camera_rate) use_clover_physics:=$(arg use_clover_physics)"/>
     <param command="$(arg cmd)" name="drone_description"/>
     <!-- Note: -package_to_model replaces all mentions of "package://" with "model://" in urdf URIs -->
     <node name="$(anon spawn)" output="screen" pkg="gazebo_ros" type="spawn_model" args="-urdf -param drone_description -model clover -z 0.3"/> 

clover_description/urdf/clover/clover4.xacro

@@ -7,7 +7,6 @@
   <xacro:arg name="gps" default="true"/>
   <xacro:arg name="maintain_camera_rate" default="false"/>
   <xacro:arg name="use_clover_physics" default="false"/>
-  <xacro:arg name="hitl" default="false"/>
 
   <xacro:include filename="clover4_base.xacro" />
   <xacro:include filename="../sensors/rpi_cam.urdf.xacro"/>

clover_description/urdf/clover/clover4_gazebo.xacro

@@ -53,14 +53,14 @@
       <mavlink_addr>INADDR_ANY</mavlink_addr>
       <mavlink_tcp_port>4560</mavlink_tcp_port>
       <mavlink_udp_port>14560</mavlink_udp_port>
-      <serialEnabled>$(arg hitl)</serialEnabled>
+      <serialEnabled>false</serialEnabled>
       <serialDevice>/dev/ttyACM0</serialDevice>
       <baudRate>921600</baudRate>
       <qgc_addr>INADDR_ANY</qgc_addr>
       <qgc_udp_port>14550</qgc_udp_port>
       <sdk_addr>INADDR_ANY</sdk_addr>
       <sdk_udp_port>14540</sdk_udp_port>
-      <hil_mode>$(arg hitl)</hil_mode>
+      <hil_mode>false</hil_mode>
       <hil_state_level>0</hil_state_level>
       <send_vision_estimation>0</send_vision_estimation>
       <send_odometry>1</send_odometry>

clover_simulation/launch/simulator.launch

@@ -1,6 +1,5 @@
 <launch>
     <arg name="type" default="gazebo"/> <!-- gazebo, jmavsim, none (only clover packages) -->
-    <arg name="mode" default="sitl"/> <!-- sitl or hitl -->
     <arg name="mav_id" default="0"/>
     <arg name="est" default="ekf2"/> <!-- PX4 estimator: lpe, ekf2 -->
     <arg name="vehicle" default="clover"/> <!-- PX4 vehicle configuration: clover, clover_vpe -->
@@ -22,7 +21,7 @@
     </include>
 
     <!-- PX4 instance -->
-    <node name="sitl_$(arg mav_id)" pkg="px4" type="px4" output="screen" required="true" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg mav_id)" unless="$(eval type == 'none' or mode == 'hitl')">
+    <node name="sitl_$(arg mav_id)" pkg="px4" type="px4" output="screen" required="true" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg mav_id)" unless="$(eval type == 'none')">
         <env name="PX4_SIM_MODEL" value="$(arg vehicle)"/>
         <env name="PX4_ESTIMATOR" value="$(arg est)"/>
     </node>
@@ -35,7 +34,6 @@
         <arg name="led" value="$(arg led)"/>
         <arg name="gps" value="$(arg gps)"/>
         <arg name="use_clover_physics" value="$(arg use_clover_physics)"/>
-        <arg name="hitl" value="$(eval mode == 'hitl')"/>
     </include>
 
     <node name="jmavsim" pkg="px4" required="true" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
@@ -47,7 +45,7 @@
     <!-- Clover services -->
     <include file="$(find clover)/launch/clover.launch">
         <arg name="simulator" value="true"/>
-        <arg name="fcu_conn" value="$(arg mode)"/>
+        <arg name="fcu_conn" value="sitl"/>
         <arg name="fcu_ip" value="127.0.0.1"/>
         <arg name="gcs_bridge" value=""/>
         <arg name="web_video_server" default="false" if="$(eval type == 'jmavsim')"/>