Add DARPA Virtual Subterranean Challenge example

gerona_examples/cartographer/subt_x.lua

@@ -0,0 +1,58 @@
+-- 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"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "X1",
+  published_frame = "X1",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = true,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  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.,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+MAP_BUILDER.num_background_threads = 7
+
+TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 2
+
+POSE_GRAPH.optimization_problem.huber_scale = 5e2
+POSE_GRAPH.optimize_every_n_nodes = 40
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
+POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
+POSE_GRAPH.constraint_builder.min_score = 0.62
+POSE_GRAPH.constraint_builder.log_matches = true
+
+return options

gerona_examples/launch/include/gerona_gazebo_params_x1.launch

@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<launch>
+
+    <group ns="gerona">
+        <param name="world_frame" value="world" />
+        <param name="odom_frame" value="world" />
+        <param name="robot_frame" value="X1" />
+    </group>
+
+    <group ns="highlevel_dummy">
+        <param name="target_speed" value="1.5" />
+        <param name="obstacle_radius" value="0.45" />
+
+        <group ns="planner">
+            <param name="reversed" value="false" />
+            <param name="goal_dist_threshold" value="0.15" />
+            <param name="goal_angle_threshold" value="45" />
+            <param name="allow_forward" value="true" />
+            <param name="allow_backward" value="true" />
+            <param name="ackermann_la" value="1.2" />
+            <param name="ackermann_steer_steps" value="3" />
+            <param name="ackermann_max_steer_angle" value="60" />
+            <param name="ackermann_steer_delta" value="20" />
+        </group>
+    </group>
+
+    <group ns="path_follower">
+        <param name="controller_type" value="kinematic_hbz" />
+        <group ns="controller">
+            <group ns="kinematic_hbz">
+                <param name="k1" value="1.0" />
+                <param name="k2" value="200" />
+
+                <param name="lambda" value="2" />
+                <param name="theta_a" value="2.0*0.78539816339" />
+            </group>
+        </group>
+
+        <param name="collision_avoider_type" value="ackermann" />
+        <group ns="collision_avoider">
+            <group ns="collision_box">
+                <param name="width" value="0.5" />
+                <param name="crit_length" value="0.4" />
+                <param name="min_length" value="0.6" />
+                <param name="max_length" value="1.0" />
+                <param name="velocity_factor" value="1.0" />
+                <param name="velocity_saturation" value="1.0" />
+            </group>
+        </group>
+
+        <group ns="local_planner">
+            <param name="algorithm" value="NULL" />
+        </group>
+
+        <group ns="supervisor">
+            <param name="use_path_lookout" value="true" />
+            <param name="use_waypoint_timeout" value="false" />
+            <param name="use_distance_to_path" value="true" />
+        </group>
+    </group>
+
+    <param name="highlevel_dummy/grow_obstacles" value="true" />
+    <param name="highlevel_dummy/obstacle_radius" value="0.75" />
+    <param name="highlevel_dummy/target_speed" value="1.0" />
+
+
+    <group ns="path_planner">
+        <param name="algorithm" value="generic" />
+    </group>
+
+    <group ns="obstacle_cloud">
+        <param name="baseFrame" value="X1" />
+    </group>
+
+</launch>

gerona_examples/launch/subt_tunnel_cartographer.launch

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- GeRoNa -->
+    <group ns="X1">
+        <remap from="cmd_vel" to="cmd_vel" />
+        <remap from="scan/front/filtered" to="front_scan" />
+        <remap from="odom" to="odom" />
+
+        <include file="$(find navigation_launch)/launch/rviz_controlled.launch">
+            <arg name="use_hector" value="false" />
+            <arg name="use_planner_default" value="true" />
+            <arg name="use_laser_obstacles" value="true" />
+        </include>
+
+        <!-- GeRoNa parameters for this stage robot -->
+        <include file="$(find gerona_examples)/launch/include/gerona_gazebo_params_x1.launch" />
+
+        <node pkg="tf" type="static_transform_publisher" name="odom_to_world_link" args="0 0 0 0 0 0 /map /world 10" />
+
+        <node name="cartographer_node" pkg="cartographer_ros"
+            type="cartographer_node" args="
+          -configuration_directory $(find gerona_examples)/cartographer
+          -configuration_basename subt_x.lua"
+            output="screen">
+            <remap from="scan" to="front_scan" />
+            <remap from="imu" to="imu/data" />
+        </node>
+
+        <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+            type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
+    </group>
+
+    <!-- ing -->
+    <!-- ign launch -v 4 tunnel_circuit_practice.ign worldName:=tunnel_circuit_practice_01 robotName1:=X1 robotConfig1:=X1_SENSOR_CONFIG_1 -->
+
+    <!-- RViz -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gerona_examples)/rviz/subt_tunnel_cartographer.rviz"/>
+
+    <!-- Logging -->
+    <!--  <node name="rqt_console" pkg="rqt_console" type="rqt_console" /> -->
+
+</launch>

gerona_examples/launch/subt_tunnel_odom.launch

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- GeRoNa -->
+    <group ns="X1">
+        <remap from="cmd_vel" to="cmd_vel" />
+        <remap from="scan/front/filtered" to="front_scan" />
+        <remap from="odom" to="odom" />
+
+        <include file="$(find navigation_launch)/launch/rviz_controlled.launch">
+            <arg name="use_hector" value="false" />
+            <arg name="use_planner_default" value="true" />
+            <arg name="use_laser_obstacles" value="true" />
+        </include>
+
+        <!-- GeRoNa parameters for this stage robot -->
+        <include file="$(find gerona_examples)/launch/include/gerona_gazebo_params_x1.launch" />
+
+        <node name="odom2tf" pkg="odom2tf" type="odom2tf_node" output="screen">
+            <param name="base_link_frame" value="X1" />
+            <param name="odom_frame" value="X1/odom" />
+        </node>
+
+        <node pkg="tf" type="static_transform_publisher" name="odom_to_world_link" args="0 0 0 0 0 0 /X1/odom /world 10" />
+
+    </group>
+
+    <!-- subt challenge launch -->
+    <!-- ign launch -v 4 tunnel_circuit_practice.ign worldName:=tunnel_circuit_practice_01 robotName1:=X1 robotConfig1:=X1_SENSOR_CONFIG_1 -->
+
+    <!-- RViz -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gerona_examples)/rviz/subt_tunnel_odom.rviz"/>
+
+</launch>