configure mavros to just use system time to avoid a bug with its algo…

robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml

@@ -10,7 +10,7 @@
     <group>
         <push-ros-namespace namespace="interface" />
         <!-- <include file="$(find-pkg-share mavros_interface)/launch/mavros_connection_poll.launch.py"> -->
-        <include file="$(find-pkg-share mavros)/launch/px4.launch">
+        <include file="$(find-pkg-share interface_bringup)/launch/px4.launch.xml">
             <!-- See robot .bashrc for how this gets set -->
             <arg name="fcu_url" value="$(env FCU_URL)" />
             <arg name="tgt_system" value="$(env TGT_SYSTEM)" />

robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/px4.launch.xml

@@ -0,0 +1,23 @@
+<launch>
+	<arg name="fcu_url" default="/dev/ttyACM0:57600" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
+	<arg name="fcu_protocol" default="v2.0" />
+	<arg name="respawn_mavros" default="false" />
+	<arg name="namespace" default="mavros"/>
+
+	<include file="$(find-pkg-share mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find-pkg-share mavros)/launch/px4_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find-pkg-share interface_bringup)/launch/px4_config.yaml" />
+		<arg name="fcu_url" value="$(var fcu_url)" />
+		<arg name="gcs_url" value="$(var gcs_url)" />
+		<arg name="tgt_system" value="$(var tgt_system)" />
+		<arg name="tgt_component" value="$(var tgt_component)" />
+		<arg name="log_output" value="$(var log_output)" />
+		<arg name="fcu_protocol" value="$(var fcu_protocol)" />
+		<arg name="respawn_mavros" value="$(var respawn_mavros)" />
+		<arg name="namespace" value="$(var namespace)"/>
+	</include>
+</launch>

robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/px4_config.yaml

@@ -0,0 +1,297 @@
+# Common configuration for PX4 autopilot
+#
+/**:
+  ros__parameters:
+    startup_px4_usb_quirk: false
+
+# --- system plugins ---
+
+# sys_status
+/**/sys:
+  ros__parameters:
+    min_voltage: [10.0]   # diagnostics min voltage, use a vector i.e. [16.2, 16.0] for multiple batteries, up-to 10 are supported
+                          # to achieve the same on a ROS launch file do: <rosparam param="sys/min_voltage">[16.2, 16.0]</rosparam>
+    disable_diag: false   # disable all sys_status diagnostics, except heartbeat
+    heartbeat_rate: 1.0   # send heartbeat rate in Hertz
+    conn_timeout: 10.0    # heartbeat timeout in seconds
+
+# sys_time
+/**/time:
+  ros__parameters:
+    time_ref_source: "fcu"    # time_reference source
+    timesync_mode: SYSTEM_TIME
+    timesync_avg_alpha: 0.6   # timesync averaging factor
+    timesync_rate: 10.0       # TIMESYNC rate in Hertz (feature disabled if 0.0)
+    system_time_rate: 1.0     # send system time to FCU rate in Hertz (disabled if 0.0)
+
+# --- mavros plugins (alphabetical order) ---
+
+# 3dr_radio
+/**/tdr_radio:
+  ros__parameters:
+    low_rssi: 40  # raw rssi lower level for diagnostics
+
+# actuator_control
+# None
+
+# command
+/**/cmd:
+  ros__parameters:
+    use_comp_id_system_control: false   # quirk for some old FCUs
+
+# dummy
+# None
+
+# ftp
+# None
+
+# global_position
+/**/global_position:
+  ros__parameters:
+    frame_id: "map"               # origin frame
+    child_frame_id: "base_link"   # body-fixed frame
+    rot_covariance: 99999.0       # covariance for attitude?
+    gps_uere: 1.0                 # User Equivalent Range Error (UERE) of GPS sensor (m)
+    use_relative_alt: true        # use relative altitude for local coordinates
+    tf.send: false                # send TF?
+    tf.frame_id: "map"            # TF frame_id
+    tf.global_frame_id: "earth"   # TF earth frame_id
+    tf.child_frame_id: "base_link"  # TF child_frame_id
+
+# imu_pub
+/**/imu:
+  ros__parameters:
+    frame_id: "base_link"
+    # need find actual values
+    linear_acceleration_stdev: 0.0003
+    angular_velocity_stdev: 0.0003490659  # 0.02 degrees
+    orientation_stdev: 1.0
+    magnetic_stdev: 0.0
+
+# local_position
+/**/local_position:
+  ros__parameters:
+    frame_id: "map"
+    tf.send: false
+    tf.frame_id: "map"
+    tf.child_frame_id: "base_link"
+    tf.send_fcu: false
+
+# param
+# None, used for FCU params
+
+# rc_io
+# None
+
+# setpoint_accel
+/**/setpoint_accel:
+  ros__parameters:
+    send_force: false
+
+# setpoint_attitude
+/**/setpoint_attitude:
+  ros__parameters:
+    reverse_thrust: false       # allow reversed thrust
+    use_quaternion: false       # enable PoseStamped topic subscriber
+    tf.listen: false            # enable tf listener (disable topic subscribers)
+    tf.frame_id: "map"
+    tf.child_frame_id: "target_attitude"
+    tf.rate_limit: 50.0
+
+# setpoint_raw
+/**/setpoint_raw:
+  ros__parameters:
+    thrust_scaling: 1.0       # used in setpoint_raw attitude callback.
+    # Note: PX4 expects normalized thrust values between 0 and 1, which means that
+    # the scaling needs to be unitary and the inputs should be 0..1 as well.
+
+# setpoint_position
+/**/setpoint_position:
+  ros__parameters:
+    tf.listen: false            # enable tf listener (disable topic subscribers)
+    tf.frame_id: "map"
+    tf.child_frame_id: "target_position"
+    tf.rate_limit: 50.0
+    mav_frame: LOCAL_NED
+
+# guided_target
+/**/guided_target:
+  ros__parameters:
+    tf.listen: false           # enable tf listener (disable topic subscribers)
+    tf.frame_id: "map"
+    tf.child_frame_id: "target_position"
+    tf.rate_limit: 50.0
+
+# setpoint_velocity
+/**/setpoint_velocity:
+  ros__parameters:
+    mav_frame: LOCAL_NED
+
+# vfr_hud
+# None
+
+# waypoint
+/**/mission:
+  ros__parameters:
+    pull_after_gcs: true        # update mission if gcs updates
+    use_mission_item_int: true  # use the MISSION_ITEM_INT message instead of MISSION_ITEM
+                                # for uploading waypoints to FCU
+
+# --- mavros extras plugins (same order) ---
+
+# adsb
+# None
+
+# debug_value
+# None
+
+# distance_sensor
+## Currently available orientations:
+#    Check http://wiki.ros.org/mavros/Enumerations
+##
+/**/distance_sensor:
+  ros__parameters:
+    config: |
+      hrlv_ez4_pub:
+        id: 0
+        frame_id: "hrlv_ez4_sonar"
+        orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
+        field_of_view: 0.0  # XXX TODO
+        send_tf: true
+        sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
+      lidarlite_pub:
+        id: 1
+        frame_id: "lidarlite_laser"
+        orientation: PITCH_270
+        field_of_view: 0.0  # XXX TODO
+        send_tf: true
+        sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
+      sonar_1_sub:
+        subscriber: true
+        id: 2
+        orientation: PITCH_270
+        horizontal_fov_ratio: 1.0   # horizontal_fov = horizontal_fov_ratio * msg.field_of_view
+        vertical_fov_ratio: 1.0     # vertical_fov = vertical_fov_ratio * msg.field_of_view
+        # custom_orientation:       # Used for orientation == CUSTOM
+        #   roll: 0
+        #   pitch: 270
+        #   yaw: 0
+      laser_1_sub:
+        subscriber: true
+        id: 3
+        orientation: PITCH_270
+
+# image_pub
+/**/image:
+  ros__parameters:
+    frame_id: "px4flow"
+
+# fake_gps
+/**/fake_gps:
+  ros__parameters:
+    # select data source
+    use_mocap: true         # ~mocap/pose
+    mocap_transform: true   # ~mocap/tf instead of pose
+    use_vision: false       # ~vision (pose)
+    # origin (default: Zürich)
+    geo_origin.lat: 47.3667       # latitude [degrees]
+    geo_origin.lon: 8.5500        # longitude [degrees]
+    geo_origin.alt: 408.0         # altitude (height over the WGS-84 ellipsoid) [meters]
+    eph: 2.0
+    epv: 2.0
+    satellites_visible: 5   # virtual number of visible satellites
+    fix_type: 3             # type of GPS fix (default: 3D)
+    tf.listen: false
+    tf.send: false           # send TF?
+    tf.frame_id: "map"       # TF frame_id
+    tf.child_frame_id: "fix" # TF child_frame_id
+    tf.rate_limit: 10.0      # TF rate
+    gps_rate: 5.0           # GPS data publishing rate
+
+# landing_target
+/**/landing_target:
+  ros__parameters:
+    listen_lt: false
+    mav_frame: "LOCAL_NED"
+    land_target_type: "VISION_FIDUCIAL"
+    image.width: 640              # [pixels]
+    image.height: 480
+    camera.fov_x: 2.0071286398    # default: 115 [degrees]
+    camera.fov_y: 2.0071286398
+    tf.send: true
+    tf.listen: false
+    tf.frame_id: "landing_target"
+    tf.child_frame_id: "camera_center"
+    tf.rate_limit: 10.0
+    target_size: {x: 0.3, y: 0.3}
+
+# mocap_pose_estimate
+/**/mocap:
+  ros__parameters:
+    # select mocap source
+    use_tf: false   # ~mocap/tf
+    use_pose: true  # ~mocap/pose
+
+# mount_control
+/**/mount:
+  ros__parameters:
+    debounce_s: 4.0
+    err_threshold_deg: 10.0
+    negate_measured_roll: false
+    negate_measured_pitch: false
+    negate_measured_yaw: false
+
+# odom
+/**/odometry:
+  ros__parameters:
+    fcu.odom_parent_id_des: "odom"        # desired parent frame rotation of the FCU's odometry
+    fcu.odom_child_id_des: "base_link"    # desired child frame rotation of the FCU's odometry
+    fcu.map_id_des: "map"
+
+# px4flow
+/**/px4flow:
+  ros__parameters:
+    frame_id: "px4flow"
+    ranger_fov: 0.118682      # 6.8 degrees at 5 meters, 31 degrees at 1 meter
+    ranger_min_range: 0.3     # meters
+    ranger_max_range: 5.0     # meters
+
+# vision_pose_estimate
+/**/vision_pose:
+  ros__parameters:
+    tf.listen: false           # enable tf listener (disable topic subscribers)
+    tf.frame_id: "odom"
+    tf.child_frame_id: "vision_estimate"
+    tf.rate_limit: 10.0
+
+# vision_speed_estimate
+/**/vision_speed:
+  ros__parameters:
+    listen_twist: true    # enable listen to twist topic, else listen to vec3d topic
+    twist_cov: true       # enable listen to twist with covariance topic
+
+# vibration
+/**/vibration:
+  ros__parameters:
+    frame_id: "base_link"
+
+# wheel_odometry
+/**/wheel_odometry:
+  ros__parameters:
+    count: 2                      # number of wheels to compute odometry
+    use_rpm: false                # use wheel's RPM instead of cumulative distance to compute odometry
+    wheel0: {x: 0.0, y: -0.15, radius: 0.05}  # x-, y-offset (m,NED) and radius (m)
+    wheel1: {x: 0.0, y:  0.15, radius: 0.05}  # x-, y-offset (m,NED) and radius (m)
+    send_raw: true                # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance)
+    send_twist: false             # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
+    frame_id: "odom"              # origin frame
+    child_frame_id: "base_link"   # body-fixed frame
+    vel_error: 0.1                # wheel velocity measurement error 1-std (m/s)
+    tf.send: false
+    tf.frame_id: "odom"
+    tf.child_frame_id: "base_link"
+
+# camera
+/**/camera:
+  ros__parameters:
+    frame_id: "base_link"