Simulation running 3D object detection

.gitignore

@@ -45,5 +45,11 @@ qtcreator-*
 # Emacs
 .#*
 
+#vscode
+.vscode
+
+
 # Catkin custom files
 CATKIN_IGNORE
+
+/gazebo/.vscode

control/CMakeLists.txt → gazebo/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(trailbot_control)
+project(trailbot_gazebo)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -15,4 +15,9 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+install(
+  DIRECTORY config urdf meshes launch worlds maps
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()

gazebo/README.md

@@ -0,0 +1,43 @@
+# trailbot_gazebo
+
+Husky Gazebo simulation package
+
+## Installation
+
+Install ros2-foxy as per the [website](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
+
+Install slam_toolbox and nav2
+
+```bash
+sudo apt install ros-humble-slam-toolbox
+sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup
+sudo apt-get install ros-humble-turtlebot3*
+sudo apt install ros-humble-twist-mux
+sudo apt install ros-humble-gazebo-ros2-control
+
+```
+
+## Building and sourcing
+Assumes starting from a structure "... /{work directory}/src/TRAILbot/". From the TRAILBot directory, run in a terminal  
+```bash
+cd ../..
+colcon build --symlink-install
+. install/setup.bash
+```
+
+## Usage
+For simulation only, run in a terminal window
+
+```bash
+ros2 launch trailbot_gazebo gazebo_sim.launch.py
+```
+For simulation with slam and navigation
+```bash
+ros2 launch trailbot_gazebo nav_sim.launch.py
+```
+
+## Keyboard control
+To control the robot using the keyboard, in a separate terminal run. The terminal window must be the currently active window.
+```bash
+ros2 run teleop_twist_keyboard teleop_twist_keyboard
+```

gazebo/config/control.yaml

@@ -0,0 +1,71 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 30  # Hz
+    use_sim_time: true
+
+    joint_broad:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    diff_cont:
+      type: diff_drive_controller/DiffDriveController
+
+diff_cont:
+  ros__parameters:
+    # base_frame_id: base_link
+
+    left_wheel_names: [ "front_left_wheel_joint", "rear_left_wheel_joint" ]
+    right_wheel_names: [ "front_right_wheel_joint", "rear_right_wheel_joint" ]
+
+    wheel_separation: 0.512  #0.1  # 0.256  # 0.512
+    wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.1651  # 0.015
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_link
+    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+    open_loop: false
+    # enable_odom_tf: false
+    # open_loop: true
+    enable_odom_tf: true
+
+    # cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    use_stamped_vel: false
+    #velocity_rolling_window_size: 10
+
+    # Preserve turning radius when limiting speed/acceleration/jerk
+    # preserve_turning_radius: true
+
+    # Publish limited velocity
+    # publish_cmd: true
+
+    # Publish wheel data
+    # publish_wheel_data: true
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0

gazebo/config/joystick.yaml

@@ -0,0 +1,27 @@
+joy_node:
+  ros__parameters:
+    device_id: 0
+    deadzone: 0.05
+    autorepeat_rate: 20.0
+
+teleop_node:
+  ros__parameters:
+    axis_linear:
+      x: 1
+    scale_linear:
+      x: 0.5
+    scale_linear_turbo:
+      x: 1.0
+
+    axis_angular:
+      yaw: 0
+    scale_angular:
+      yaw: 0.5
+    scale_angular_turbo:
+      yaw: 1.0
+
+
+    enable_button: 6
+    enable_turbo_button: 7
+
+    require_enable_button: true

gazebo/config/mapper_params_online_async.yaml

@@ -0,0 +1,73 @@
+slam_toolbox:
+  ros__parameters:
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+    mode: localization
+
+    # if you'd like to immediately start continuing a map at a given pose
+    # or at the dock, but they are mutually exclusive, if pose is given
+    # will use pose
+    map_file_name: /home/dev/dev_ws/my_map_serial
+    # map_start_pose: [0.0, 0.0, 0.0]
+    map_start_at_dock: true
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    max_laser_range: 20.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10           
+    loop_match_maximum_variance_coarse: 3.0  
+    loop_match_minimum_response_coarse: 0.35    
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5      
+    angle_variance_penalty: 1.0    
+
+    fine_search_angle_offset: 0.00349     
+    coarse_search_angle_offset: 0.349   
+    coarse_angle_resolution: 0.0349        
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true