Rename package to clover (#179)

This commit is contained in:
Oleg Kalachev
2020-01-31 03:24:21 +03:00
committed by GitHub
parent 32ff2d4e15
commit 89ccf9c2b9
65 changed files with 869 additions and 114 deletions

View File

@@ -0,0 +1,41 @@
<launch>
<arg name="aruco_detect" default="true"/>
<arg name="aruco_map" default="false"/>
<arg name="aruco_vpe" default="false"/>
<!-- For additional help go to https://clever.coex.tech/aruco -->
<!-- aruco_detect: detect aruco markers, estimate poses -->
<node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
<remap from="image_raw" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
<param name="estimate_poses" value="true"/>
<param name="send_tf" value="true"/>
<param name="known_tilt" value="map"/>
<param name="length" value="0.33"/>
</node>
<!-- aruco_map: estimate aruco map pose -->
<node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
<remap from="image_raw" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<remap from="markers" to="aruco_detect/markers"/>
<param name="map" value="$(find aruco_pose)/map/map.txt"/>
<param name="known_tilt" value="map"/>
<param name="image_axis" value="true"/>
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
<param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
<param name="markers/frame_id" value="aruco_map"/>
<param name="markers/child_frame_id_prefix" value="aruco_"/>
</node>
<!-- vpe publisher from aruco markers -->
<node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
<remap from="~pose_cov" to="aruco_map/pose"/>
<remap from="~vpe" to="mavros/vision_pose/pose"/>
<param name="frame_id" value="aruco_map_detected"/>
<param name="publish_zero" value="true"/>
<param name="offset_frame_id" value="aruco_map"/>
</node>
</launch>

View File

@@ -0,0 +1,78 @@
<launch>
<arg name="fcu_conn" default="usb"/>
<arg name="fcu_ip" default="127.0.0.1"/>
<arg name="gcs_bridge" default="tcp"/>
<arg name="web_video_server" default="true"/>
<arg name="rosbridge" default="true"/>
<arg name="main_camera" default="true"/>
<arg name="optical_flow" default="false"/>
<arg name="aruco" default="false"/>
<arg name="rangefinder_vl53l1x" default="true"/>
<arg name="led" default="true"/>
<arg name="rc" default="true"/>
<!-- log formatting -->
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
<!-- mavros -->
<include file="$(find clover)/launch/mavros.launch">
<arg name="fcu_conn" value="$(arg fcu_conn)"/>
<arg name="fcu_ip" value="$(arg fcu_ip)"/>
<arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
</include>
<!-- web video server -->
<node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5">
<param name="default_stream_type" value="ros_compressed"/>
<param name="publish_rate" value="1.0"/>
</node>
<!-- aruco markers -->
<include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
<!-- optical flow -->
<node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
<remap from="image_raw" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<param name="calc_flow_gyro" value="true"/>
</node>
<!-- main nodelet manager -->
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
<param name="num_worker_threads" value="2"/>
</node>
<node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
<!-- simplified offboard control -->
<node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
<param name="reference_frames/body" value="map"/>
<param name="reference_frames/base_link" value="map"/>
<param name="reference_frames/navigate_target" value="map"/>
</node>
<!-- main camera -->
<include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)"/>
<!-- rosbridge -->
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
<!-- tf2 republisher for web visualization -->
<node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
<!-- vl53l1x ToF rangefinder -->
<node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
<param name="frame_id" value="rangefinder"/>
<param name="min_signal" value="0.4"/>
<param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
</node>
<!-- led strip -->
<include file="$(find clover)/launch/led.launch" if="$(arg led)"/>
<!-- rc backend -->
<node name="rc" pkg="clover" type="rc" output="screen" if="$(arg rc)" clear_params="true">
<!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
<param name="use_fake_gcs" value="false"/>
</node>
</launch>

View File

@@ -0,0 +1,6 @@
<launch>
<arg name="device" default="/dev/video1"/>
<arg name="port" default="9999"/>
<node name="fpv_camera" pkg="clover" type="fpv_camera" args="$(arg device) $(arg port)" output="screen"/>
</launch>

38
clover/launch/led.launch Normal file
View File

@@ -0,0 +1,38 @@
<launch>
<arg name="ws281x" default="true"/>
<arg name="led_effect" default="true"/>
<arg name="led_notify" default="true"/>
<!-- For additional help go to https://clover.coex.tech/led -->
<!-- ws281x led strip driver -->
<node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(arg ws281x)">
<param name="led_count" value="58"/>
<param name="gpio_pin" value="21"/>
<param name="brightness" value="64"/>
<param name="strip_type" value="WS2811_STRIP_GRB"/>
<param name="target_frequency" value="800000"/>
<param name="dma" value="10"/>
<param name="invert" value="false"/>
</node>
<!-- high level led effects control, events notification with leds -->
<node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
<param name="blink_rate" value="2"/>
<param name="fade_period" value="0.5"/>
<param name="rainbow_period" value="5"/>
<!-- events effects table -->
<rosparam param="notify" if="$(arg led_notify)">
startup: { r: 255, g: 255, b: 255 }
connected: { effect: rainbow }
disconnected: { effect: blink, r: 255, g: 50, b: 50 }
acro: { r: 245, g: 155, b: 0 }
stabilized: { r: 30, g: 180, b: 50 }
altctl: { r: 255, g: 255, b: 40 }
posctl: { r: 50, g: 100, b: 220 }
offboard: { r: 220, g: 20, b: 250 }
low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
error: { effect: flash, r: 255, g: 0, b: 0 }
</rosparam>
</node>
</launch>

View File

@@ -0,0 +1,39 @@
<launch>
<!-- Camera position and orientation are represented by base_link -> main_camera_optical transform -->
<!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
<!-- article about camera setup: https://clover.coex.tech/camera_frame -->
<!-- camera is oriented downward, camera cable goes backward [option 1] -->
<node 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"/>
<!-- camera is oriented downward, camera cable goes forward [option 2] -->
<!--<node 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"/>-->
<!-- camera is oriented upward, camera cable goes backward [option 3] -->
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>-->
<!-- camera is oriented upward, camera cable goes forward [option 4] -->
<!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 base_link main_camera_optical"/>-->
<!-- camera node -->
<node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">
<param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
<param name="frame_id" value="main_camera_optical"/>
<param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam_320.yaml"/>
<param name="rate" value="100"/> <!-- poll rate -->
<param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
<param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
<param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
<!-- camera resolution, NOTE: camera_info file should match it -->
<param name="image_width" value="320"/>
<param name="image_height" value="240"/>
</node>
<!-- camera visualization markers -->
<node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
<param name="scale" value="3.0"/>
</node>
</launch>

View File

@@ -0,0 +1,83 @@
<launch>
<arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
<arg name="fcu_ip" default="127.0.0.1"/>
<arg name="gcs_bridge" default="tcp"/>
<arg name="viz" default="true"/>
<arg name="respawn" default="true"/>
<arg name="distance_sensor_remap" default="rangefinder/range"/>
<node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
<!-- UART connection -->
<param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
<!-- USB connection -->
<param name="fcu_url" value="/dev/px4fmu" if="$(eval fcu_conn == 'usb')"/>
<!-- sitl before PX4 1.9.0 -->
<param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
<!-- sitl since PX4 1.9.0 -->
<param name="fcu_url" value="udp://@$(arg fcu_ip):14580" if="$(eval fcu_conn == 'sitl')"/>
<!-- gcs bridge -->
<param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
<param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
<param name="gcs_url" value="udp-b://$(env ROS_HOSTNAME):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
<param name="gcs_url" value="udp-pb://$(env ROS_HOSTNAME):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
<param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
<param name="gcs_quiet_mode" value="true"/>
<param name="conn/timeout" value="8"/>
<!-- basic params -->
<rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
<!-- remap rangefinder -->
<remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
<rosparam param="plugin_whitelist">
- altitude
- command
- distance_sensor
- ftp
- global_position
- imu
- local_position
- manual_control
# - mocap_pose_estimate
- param
- px4flow
- rc_io
- setpoint_attitude
- setpoint_position
- setpoint_raw
- setpoint_velocity
- sys_status
- sys_time
- vision_pose_estimate
# - vision_speed_estimate
# - waypoint
</rosparam>
</node>
<!-- remapped distance_sensor config -->
<rosparam param="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))">
subscriber: true
id: 1
orientation: PITCH_270
covariance: 1 # cm
</rosparam>
<!-- Rangefinders frame -->
<node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>
<!-- Copter visualization -->
<node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
<remap to="mavros/local_position/pose" from="local_position"/>
<remap to="mavros/setpoint_position/local" from="local_setpoint"/>
<param name="fixed_frame_id" value="map"/>
<param name="child_frame_id" value="base_link"/>
<param name="marker_scale" value="1"/>
<param name="max_track_size" value="20"/>
<param name="num_rotors" value="4"/>
</node>
</launch>

View File

@@ -0,0 +1,139 @@
# Config file for mavros
# Based on https://raw.githubusercontent.com/mavlink/mavros/master/mavros/launch/px4_config.yaml
startup_px4_usb_quirk: true
conn:
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
timeout: 10.0 # hertbeat timeout in seconds
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)
time:
time_ref_source: "fcu" # time_reference source
timesync_mode: MAVLINK
timesync_avg_alpha: 0.6 # timesync averaging factor
global_position:
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?
frame_id: "map" # TF frame_id
global_frame_id: "earth" # TF earth frame_id
child_frame_id: "base_link" # TF child_frame_id
imu:
frame_id: "base_link"
# need find actual values
linear_acceleration_stdev: 0.0003
angular_velocity_stdev: !degrees 0.02
orientation_stdev: 1.0
magnetic_stdev: 0.0
local_position:
frame_id: "map"
tf:
send: true
send_fcu: false
# setpoint_attitude
setpoint_attitude:
reverse_thrust: false # allow reversed thrust
use_quaternion: true # enable PoseStamped topic subscriber
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "map"
child_frame_id: "target_attitude"
rate_limit: 50.0
setpoint_raw:
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:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "map"
child_frame_id: "target_position"
rate_limit: 50.0
mav_frame: LOCAL_NED
setpoint_velocity:
mav_frame: LOCAL_NED
mission:
pull_after_gcs: true # update mission if gcs updates
distance_sensor:
rangefinder:
id: 0
frame_id: "rangefinder"
orientation: PITCH_270
field_of_view: 0.5
rangefinder_sub:
subscriber: true
# fake_gps
fake_gps:
# 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]
lon: 8.5500 # longitude [degrees]
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
send: false # send TF?
frame_id: "map" # TF frame_id
child_frame_id: "fix" # TF child_frame_id
rate_limit: 10.0 # TF rate
gps_rate: 5.0 # GPS data publishing rate
mocap:
# select mocap source
use_tf: false # ~mocap/tf
use_pose: true # ~mocap/pose
odometry:
in:
frame_id: "odom"
child_frame_id: "base_link"
frame_tf:
local_frame: "local_origin_ned"
body_frame_orientation: "flu"
out:
frame_tf:
# available: check MAV_FRAME odometry local frames in
# https://mavlink.io/en/messages/common.html
local_frame: "vision_ned"
# available: ned, frd or flu (though only the tf to frd is supported)
body_frame_orientation: "frd"
px4flow:
frame_id: "px4flow"
ranger_fov: !degrees 6.8 # 6.8 degreens at 5 meters, 31 degrees at 1 meter
ranger_min_range: 0.3 # meters
ranger_max_range: 5.0 # meters
vision_pose:
tf:
listen: false # enable tf listener (disable topic subscribers)
vision_speed:
listen_twist: true # enable listen to twist topic, else listen to vec3d topic
twist_cov: true # enable listen to twist with covariance topic
vibration:
frame_id: "base_link"

19
clover/launch/sitl.launch Normal file
View File

@@ -0,0 +1,19 @@
<launch>
<!-- clover configuration for testing in sitl -->
<arg name="ip" default="127.0.0.1"/>
<arg name="rosbridge" default="false"/>
<include file="$(find clover)/launch/clover.launch">
<arg name="fcu_conn" value="udp"/>
<arg name="fcu_ip" value="$(arg ip)"/>
<arg name="gcs_bridge" value="false"/>
<arg name="optical_flow" value="false"/>
<arg name="web_video_server" default="false"/>
<arg name="main_camera" default="false"/>
<arg name="rosbridge" value="$(arg rosbridge)"/>
<arg name="aruco" default="false"/>
<arg name="rangefinder_vl53l1x" default="false"/>
<arg name="led" default="false"/>
<arg name="rc" default="false"/>
</include>
</launch>