[scripts] Add startup for exploration

scripts/startup/.gitignore

@@ -0,0 +1 @@
+.tmuxinator.yml

scripts/startup/custom_config/apm_config.yaml

@@ -0,0 +1,231 @@
+# Common configuration for APM2 autopilot
+#
+# node:
+startup_px4_usb_quirk: false
+
+# --- system plugins ---
+
+# sys_status & sys_time connection options
+conn:
+  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  heartbeat_mav_type: "ONBOARD_CONTROLLER"
+  timeout: 10.0          # hertbeat timeout in seconds
+  timesync_rate: 0.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
+  system_time_rate: 0.0  # send system time to FCU rate in Hertz (disabled if 0.0)
+
+# sys_status
+sys:
+  min_voltage: 10.0   # diagnostics min voltage
+  disable_diag: false # disable all sys_status diagnostics, except heartbeat
+
+# sys_time
+time:
+  time_ref_source: "fcu"  # time_reference source
+  timesync_mode: MAVLINK
+  timesync_avg_alpha: 0.6 # timesync averaging factor
+
+# --- mavros plugins (alphabetical order) ---
+
+# 3dr_radio
+tdr_radio:
+  low_rssi: 40  # raw rssi lower level for diagnostics
+
+# actuator_control
+# None
+
+# command
+cmd:
+  use_comp_id_system_control: false # quirk for some old FCUs
+
+# dummy
+# None
+
+# ftp
+# None
+
+# global_position
+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_pub
+imu:
+  frame_id: "red/map/base_link"
+  # need find actual values
+  linear_acceleration_stdev: 0.0003
+  angular_velocity_stdev: 0.0003490659
+  orientation_stdev: 1.0
+  magnetic_stdev: 0.0
+
+# local_position
+local_position:
+  frame_id: "map"
+  tf:
+    send: false
+    frame_id: "map"
+    child_frame_id: "base_link"
+    send_fcu: false
+
+# param
+# None, used for FCU params
+
+# rc_io
+# None
+
+# safety_area
+safety_area:
+  p1: {x:  1.0, y:  1.0, z:  1.0}
+  p2: {x: -1.0, y: -1.0, z: -1.0}
+
+# setpoint_accel
+setpoint_accel:
+  send_force: 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_position
+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
+setpoint_velocity:
+  mav_frame: LOCAL_NED
+
+setpoint_raw:
+  thrust_scaling: 1.0
+
+thrust_scaling: 1.0
+
+# vfr_hud
+# None
+
+# waypoint
+mission:
+  pull_after_gcs: true  # update mission if gcs updates
+
+# --- mavros extras plugins (same order) ---
+
+# adsb
+# None
+
+# debug_value
+# None
+
+# distance_sensor
+## Currently available orientations:
+#    Check http://wiki.ros.org/mavros/Enumerations
+##
+distance_sensor:
+  rangefinder_pub:
+    id: 0
+    frame_id: "lidar"
+    #orientation: PITCH_270 # sended by FCU
+    field_of_view: 0.0  # XXX TODO
+    send_tf: false
+    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
+  rangefinder_sub:
+    subscriber: true
+    id: 1
+    orientation: PITCH_270  # only that orientation are supported by APM 3.4+
+
+# image_pub
+image:
+  frame_id: "px4flow"
+
+# 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_pose_estimate
+mocap:
+  # select mocap source
+  use_tf: false   # ~mocap/tf
+  use_pose: true  # ~mocap/pose
+
+# odom
+odometry:
+  frame_tf:
+    desired_frame: "ned"
+  estimator_type: 3 # check enum MAV_ESTIMATOR_TYPE in <http://mavlink.org/messages/common>
+
+# px4flow
+px4flow:
+  frame_id: "px4flow"
+  ranger_fov: 0.118682      # 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_estimate
+vision_pose:
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+    frame_id: "map"
+    child_frame_id: "vision_estimate"
+    rate_limit: 10.0
+
+# vision_speed_estimate
+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
+vibration:
+  frame_id: "base_link"
+
+# wheel_odometry
+wheel_odometry:
+  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: "map"             # origin frame
+  child_frame_id: "base_link" # body-fixed frame
+  vel_error: 0.1              # wheel velocity measurement error 1-std (m/s)
+  tf:
+    send: true
+    frame_id: "map"
+    child_frame_id: "base_link"
+
+# vim:set ts=2 sw=2 et:

scripts/startup/custom_config/exploration.lua

@@ -0,0 +1,89 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "map_builder.lua"
+include "trajectory_builder.lua"
+namespace = os.getenv("UAV_NAMESPACE")
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame =  namespace.."/map/map",
+  tracking_frame =  namespace.."/map/base_link",
+  published_frame =  namespace.."/map/base_link",
+  odom_frame = namespace.."/map/odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  nav_sat_use_predefined_enu_frame = false,
+  nav_sat_predefined_enu_frame_lat_deg = 45.813902,
+  nav_sat_predefined_enu_frame_lon_deg = 16.038766,
+  nav_sat_predefined_enu_frame_alt_m = 168.259294525,
+  nav_sat_translation_weight = 1.,
+  nav_sat_inverse_covariance_weight = 0.,
+  nav_sat_inverse_covariance_bias = 1.,
+  use_landmarks = false,
+  num_laser_scans = 0,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 1,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+TRAJECTORY_BUILDER.collate_fixed_frame = false
+
+TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation_weight = 10.
+TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight = 600
+TRAJECTORY_BUILDER_3D.submaps.high_resolution = 0.05
+TRAJECTORY_BUILDER_3D.submaps.high_resolution_max_range = 40.
+TRAJECTORY_BUILDER_3D.submaps.num_range_data = 50.
+-- TRAJECTORY_BUILDER_3D.submaps.num_range_data = 90.
+TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter = {
+    max_length = 2.,
+    min_num_points = 150,
+    max_range = 50.,
+}
+
+TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter = {
+    max_length = 4.,
+    min_num_points = 200,
+    max_range = 70,
+}
+
+MAP_BUILDER.use_trajectory_builder_3d = true
+MAP_BUILDER.num_background_threads = 3
+POSE_GRAPH.optimization_problem.huber_scale = 5e2
+POSE_GRAPH.optimization_problem.enable_nav_sat_huber_loss = false
+POSE_GRAPH.optimize_every_n_nodes = 0
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.02
+POSE_GRAPH.constraint_builder.max_constraint_distance = 30.
+POSE_GRAPH.optimization_problem.nav_sat_huber_scale = 5e2
+POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 200
+POSE_GRAPH.constraint_builder.min_score = 0.4
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
+POSE_GRAPH.optimization_problem.rotation_weight = 0
+POSE_GRAPH.optimization_problem.acceleration_weight = 1e2
+
+return options

scripts/startup/session.yml

@@ -0,0 +1,38 @@
+name: exploration
+root: ./
+startup_window: roscore
+pre_window: export UAV_NAMESPACE=red
+windows:
+  - roscore:
+      layout: tiled
+      panes:
+        - roscore
+        - export SITL_RITW_TERMINAL="tmux new-window -n:ardupilot1"; waitForRos; roslaunch ardupilot_gazebo sim_vehicle.launch enable_console:=false streamrate:=15
+        - waitForRos; rosparam set use_sim_time true; roslaunch ardupilot_gazebo mavros.launch config_file:=custom_config/apm_config.yaml
+  - gazebo:
+      layout: tiled
+      panes:
+        - waitForRos; roslaunch ardupilot_gazebo kopterworx.launch use_sim_time:=true  enable_velodyne:=true max_range:=20 world:=$(rospack find uav_frontier_exploration_3d)/worlds/house.world tf_prefix:="$UAV_NAMESPACE/map" x:=-11 y:=-7
+  - arm&takeof:
+      layout: tiled
+      panes:
+        - waitForRos; waitForOdometry; roslaunch uav_ros_control pid_carrot_carto.launch manual_takeoff:=false odometry:=/$UAV_NAMESPACE/uav/cartographer/odometry_filtered_acc
+        - waitForRos; waitForSLAM; rosrun ardupilot_gazebo automatic_takeoff.sh
+  - slam:
+      layout: tiled
+      panes: 
+        - waitForRos; waitForOdometry; rosparam set use_sim_time true; roslaunch uav_ros_general cartographer.launch simulation:=true configuration_directory:=$(pwd)/custom_config configuration_basename:=exploration.lua
+        - waitForRos; waitForOdometry; roslaunch uav_ros_general velocity_estimation_acc.launch
+  - exploration:
+      layout: tiled
+      panes:
+        - waitForRos; waitForOdometry; waitForSLAM; roslaunch uav_frontier_exploration_3d planner.launch
+        - waitForRos; waitForOdometry; waitForSLAM; roslaunch uav_frontier_exploration_3d execute_trajectory.launch
+        - waitForRos; waitForOdometry; waitForSLAM; roslaunch uav_frontier_exploration_3d frontier_server.launch odometry:=/$UAV_NAMESPACE/uav/cartographer/odometry_filtered_acc config_filename:=$(rospack find uav_frontier_exploration_3d)/config/kopterworx_exploration_sim_house.yaml
+        - "#waitForRos; waitForOdometry; waitForSLAM; rosservice call /$UAV_NAMESPACE/exploration/toggle \"data: true\""
+        - "#rosservice call /$UAV_NAMESPACE/confirm_trajectory \"data: true\""
+  - rviz:
+      layout: tiled
+      panes:
+        - waitForRos; waitForOdometry; waitForSLAM; rosrun rviz rviz -d ~/uav_ws/src/uav_frontier_exploration_3d/rviz/cartographer_and_frontier.rviz
+        - "#rosservice call /$UAV_NAMESPACE/exploration/save_octomap \"{}\""

scripts/startup/start.sh

@@ -0,0 +1,16 @@
+#!/bin/zsh
+
+# Absolute path to this script. /home/user/bin/foo.sh
+SCRIPT=$(readlink -f $0)
+# Absolute path this script is in. /home/user/bin
+SCRIPTPATH=`dirname $SCRIPT`
+cd "$SCRIPTPATH"
+
+# remove the old link
+rm .tmuxinator.yml
+
+# link the session file to .tmuxinator.yml
+ln session.yml .tmuxinator.yml
+
+# start tmuxinator
+tmuxinator