replace node throttle with NodeletThrottle plugin

tiago_clf_launch/config/joypad_8bitdo.yaml

@@ -9,7 +9,7 @@ teleop:
       -
         axis: 1
         target: linear.x
-        scale: 0.7
+        scale: 0.5
       -
         axis: 3
         target: angular.z

tiago_clf_launch/config/realsense_fixed.yaml

@@ -0,0 +1,18 @@
+l500_depth_sensor:
+  visual_preset: 3
+  laser_power: 80
+  confidence_threshold: 0
+  min_distance: 250
+  receiver_gain: 12
+  post_processing_sharpening: 2
+  pre_processing_sharpening: 3
+  receiver_gain: 12
+  noise_filtering: 2
+  digital_gain: 2
+
+rgb_camera:
+  power_line_frequency: 1
+  global_time_enabled: true
+  exposure: 100
+  gain: 4096
+  enable_auto_exposure: false

tiago_clf_launch/config/tiago_motions.yaml

@@ -20,7 +20,7 @@ play_motion:
         usage: demo
         description: 'Go home'
 
-    handover_long:
+    handover_stretched:
       joints: [arm_1_joint,
       arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
       arm_6_joint, arm_7_joint]
@@ -573,6 +573,13 @@ play_motion:
       - positions: [0.09, 1.02, -3.01, 0.74, -1.54, 1.29, -0.20]
         time_from_start: 2.0
 
+    preplace_after_transport_over_head:
+      joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+      meta: {description: preplace_after_transport_over_head, name: preplace after transport over head, usage: demo}
+      points:
+      - positions: [0.35, 0.76, -3.01, 0.76, -1.54, 0.45, -0.20]
+        time_from_start: 2.0
+
 
     cml_carry_on_back:
           joints: [arm_1_joint,
@@ -630,4 +637,27 @@ play_motion:
           meta:
             name: transport over head ign j7
             usage: demo
-            description: 'transport over head ign j7'        
+            description: 'transport over head ign j7'   
+
+    look_into_cup:
+          joints: [torso_lift_joint, head_1_joint, head_2_joint, arm_1_joint,
+          arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
+          arm_6_joint, arm_7_joint]
+          points:
+          - positions: [0.15,-0.27, -0.87,  0.07, 0.21, -1.70, 2.10, 2.07, -0.72, 0.52]
+            time_from_start: 1.5
+          meta:
+            name: look into cup
+            usage: demo
+            description: 'Moves the gripper so that  the robot can look into the cup'
+    pregrasp_laundry:
+          joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, 'torso_lift_joint']
+          points:
+          - positions: [ 0.8776, -0.3718, -1.5654, 1.4531, -1.57, 1.36, 0.0, 0.35]
+            time_from_start: 1.0
+          - positions: [ 1.56, 0.3361, 0.0207, 1.4705, -1.57, 0.05, 0.0, 0.35]
+            time_from_start: 3.0
+          meta: 
+            description: pregrasp_laundry
+            name: pregrasp_laundry
+            usage: 'Pregrasp pose for grasping laundry out of basket'

tiago_clf_launch/launch/rs_D455_calibration.launch

@@ -3,7 +3,7 @@
   <arg name="publish_transform" default="True"></arg>
 
   <group if="$(arg publish_transform)">
-    <node pkg="tf2_ros" type="static_transform_publisher" name="realsense_calibration" args="0.00020595 0.066965 -0.029624 -0.0088591 -0.028474 0.0058181 0.99954 realsense_link camera_link" />
+    <node pkg="tf2_ros" type="static_transform_publisher" name="realsense_calibration" args="0.0099479 0.054964 -0.045365 0.0094738 -0.040901 0.011308 0.99905 realsense_link camera_link" />
   </group>
 
   <group unless="$(arg publish_transform)"> 

tiago_clf_launch/launch/rs_l515_calibration.launch

@@ -3,7 +3,7 @@
 <arg name="publish_transform" default="True"></arg>
 
 <group if="$(arg publish_transform)">
-  <node pkg="tf2_ros" type="static_transform_publisher" name="realsense_calibration" args="-0.002 0.008 0.0058 -0.00872521 0.0174517 0.000152299 0.99981 realsense_link camera_link" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="realsense_calibration" args="-0.002 0.008 0.0058 -0.0087218 0.00091831 -0.0052281 0.99995 realsense_link camera_link" /> 
   <!--node pkg="tf2_ros" type="static_transform_publisher" name="realsense_calibration" args="1.8626e-9 0.027831 0 -6.0263e-17 0.015707 -6.0263e-17 0.99988 realsense_link camera_link" /-->
   <!--node pkg="tf2_ros" type="static_transform_publisher" name="realsense_calibration" args="-0.00951475 -0.00500572 0.00447848 4.36893e-10 0.00968784 7.24114e-11 0.999953 realsense_link camera_link" /-->
 </group>
@@ -13,4 +13,4 @@
    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="$(arg command_args)" output="log"/>
 </group>
 
-</launch>
+</launch>

tiago_clf_nav/CMakeLists.txt

@@ -10,5 +10,6 @@ install(DIRECTORY launch config maps
 
 install(PROGRAMS 
         scripts/amcl_particle_fan_node
+        scripts/particle_fan_stub_node
         scripts/nav_pose.py
         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

tiago_clf_nav/config/cartographer/tiago.lua

@@ -20,12 +20,12 @@ options = {
   trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
   tracking_frame = "base_footprint",
-  published_frame = "base_footprint",
+  published_frame = "odom",
   odom_frame = "odom",
-  provide_odom_frame = true,
-  publish_frame_projected_to_2d = false,
-  use_pose_extrapolator = true,
-  use_odometry = false,
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = true,
+--  use_pose_extrapolator = true,
+  use_odometry = true,
   use_nav_sat = false,
   use_landmarks = false,
   num_laser_scans = 1,
@@ -34,7 +34,8 @@ options = {
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
-  pose_publish_period_sec = 5e-3,
+  pose_publish_period_sec = 20e-3, --20ms = 50hz
+--  publish_to_tf = true,
   trajectory_publish_period_sec = 30e-3,
   rangefinder_sampling_ratio = 1.,
   odometry_sampling_ratio = 1.,
@@ -45,11 +46,19 @@ options = {
 
 MAP_BUILDER.use_trajectory_builder_2d = true
 
-TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+-- crashes after error:
+-- Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise
 TRAJECTORY_BUILDER_2D.use_imu_data = false
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
 TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
 TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
 
-POSE_GRAPH.optimization_problem.huber_scale = 1e2
+--POSE_GRAPH.constraint_builder.min_score = 0.65
+--POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
+
+--POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+POSE_GRAPH.optimize_every_n_nodes = 30 -- def: 90
 
-return options
+return options

tiago_clf_nav/config/cartographer/tiago_imu.lua

@@ -0,0 +1,66 @@
+-- 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 = "base_imu_link",
+  published_frame = "odom",
+
+  publish_frame_projected_to_2d = true,
+--  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 = 20e-3, --20ms = 50hz
+--  publish_to_tf = true,
+  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.,
+
+  provide_odom_frame = false,
+  odom_frame = "odom",
+
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+-- crashes after error:
+-- Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise
+TRAJECTORY_BUILDER_2D.use_imu_data = true
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+
+POSE_GRAPH.constraint_builder.min_score = 0.65
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
+
+
+--POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+return options

tiago_clf_nav/config/cartographer/tiago_localization.lua

@@ -0,0 +1,32 @@
+-- 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 "tiago.lua"
+
+
+--TRAJECTORY_BUILDER_2D.pure_localization_trimmer = {
+--  max_submaps_to_keep = 3,
+--}
+
+
+TRAJECTORY_BUILDER.pure_localization = true 
+POSE_GRAPH.optimize_every_n_nodes = 20
+
+-- fast localization
+MAP_BUILDER.num_background_threads = 4
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio
+POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio
+POSE_GRAPH.max_num_final_iterations = 1
+
+return options

tiago_clf_nav/config/cartographer/tiago_odom.lua

@@ -0,0 +1,65 @@
+-- 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 = "base_imu_link",
+  published_frame = "odom",
+
+  publish_frame_projected_to_2d = true,
+--  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 = 20e-3, --20ms = 50hz
+--  publish_to_tf = true,
+  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.,
+
+  provide_odom_frame = false,
+  odom_frame = "odom",
+
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+-- crashes after error:
+-- Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise
+TRAJECTORY_BUILDER_2D.use_imu_data = true
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+
+POSE_GRAPH.constraint_builder.min_score = 0.65
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
+
+--POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+return options

tiago_clf_nav/config/gmcl.yaml

@@ -0,0 +1,41 @@
+use_optimal_filter: false
+use_intelligent_filter: false
+use_self_adaptive: false
+use_kld_sampling: true
+N_aux_particles: 10
+crossover_alpha: .5
+mutation_probability: 0.1
+energy_map_resolution_x: 0.2
+energy_map_resolution_y: 0.2
+energy_threshold_value: 0.05
+publish_ser: false
+odom_model_type: diff
+transform_tolerance: 0.1
+gui_publish_rate: 2.0
+laser_max_beams: 30
+min_particles: 500
+max_particles: 2000
+kld_err: 0.01
+kld_z: 0.99
+odom_alpha1: 0.2
+odom_alpha2: 0.2
+odom_alpha3: 0.8
+odom_alpha4: 0.2
+laser_z_hit: 0.5
+laser_z_short: 0.05
+laser_z_max: 0.05
+laser_z_rand: 0.5
+laser_sigma_hit: 0.2
+laser_lambda_short: 0.1
+laser_model_type: likelihood_field
+laser_likelihood_max_dist: 2.5
+update_min_d: 0.2
+update_min_a: 0.5
+odom_frame_id: odom
+resample_interval: 1
+transform_tolerance: 0.1
+recovery_alpha_slow: 0.0
+recovery_alpha_fast: 0.0
+selective_resampling: false
+
+# laser_model_type: beam

tiago_clf_nav/config/move_base/base/global_costmap.yaml

@@ -12,6 +12,8 @@ global_costmap:
   plugins:
     - name: static_layer
       type: 'costmap_2d::StaticLayer'
+    - name: static_layer_obstacles
+      type: 'costmap_2d::StaticLayer'
     - name: current_obstacle_layer
       type: 'costmap_2d::CurrentObstacleLayer'
     - name: inflation_layer
@@ -22,6 +24,12 @@ global_costmap:
     subscribe_to_updates: true
     map_topic: /map
 
+  static_layer_obstacles:
+    enabled  : true
+    use_maximum: true
+    subscribe_to_updates: true
+    map_topic: /map_obstacles
+
   current_obstacle_layer:
     enabled: true
     observation_sources: sensor_scan_clf