From 902bed9961bf571a1fe84fc680bc4c3af4638dca Mon Sep 17 00:00:00 2001 From: Matt Williamson Date: Tue, 28 Mar 2023 08:14:05 -0400 Subject: [PATCH] Porting spotmicro to ros2 --- configs/spotmicro_config/CMakeLists.txt | 11 +- configs/spotmicro_config/README.md | 27 +- .../config/autonomy/navigation.yaml | 383 ++++ .../config/autonomy/slam.yaml | 73 + .../spotmicro_config/config/gait/gait.yaml | 11 - .../config/joints/joints.yaml | 23 - .../spotmicro_config/config/links/links.yaml | 24 - .../base_local_planner_holonomic_params.yaml | 46 - .../move_base/costmap_common_params.yaml | 28 - .../move_base/global_costmap_params.yaml | 13 - .../move_base/local_costmap_params.yaml | 16 - .../config/move_base/move_base_params.yaml | 15 - .../config/ros_control/ros_control.yaml | 22 - .../spotmicro_config/launch/bringup.launch | 49 - .../spotmicro_config/launch/bringup.launch.py | 83 + configs/spotmicro_config/launch/gazebo.launch | 35 - .../spotmicro_config/launch/gazebo.launch.py | 125 ++ .../launch/include/amcl.launch | 38 - .../launch/include/gmapping.launch | 41 - .../launch/include/move_base.launch | 23 - .../spotmicro_config/launch/navigate.launch | 30 - .../launch/navigate.launch.py | 72 + configs/spotmicro_config/launch/slam.launch | 23 - .../spotmicro_config/launch/slam.launch.py | 61 + .../launch/spawn_robot.launch | 32 - configs/spotmicro_config/maps/playground.yaml | 7 + configs/spotmicro_config/package.xml | 18 +- .../spotmicro_config/worlds/playground.world | 1564 +++++++++++++++++ 28 files changed, 2400 insertions(+), 493 deletions(-) create mode 100644 configs/spotmicro_config/config/autonomy/navigation.yaml create mode 100644 configs/spotmicro_config/config/autonomy/slam.yaml delete mode 100644 configs/spotmicro_config/config/gait/gait.yaml delete mode 100644 configs/spotmicro_config/config/joints/joints.yaml delete mode 100644 configs/spotmicro_config/config/links/links.yaml delete mode 100644 configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml delete mode 100644 configs/spotmicro_config/config/move_base/costmap_common_params.yaml delete mode 100644 configs/spotmicro_config/config/move_base/global_costmap_params.yaml delete mode 100644 configs/spotmicro_config/config/move_base/local_costmap_params.yaml delete mode 100644 configs/spotmicro_config/config/move_base/move_base_params.yaml delete mode 100644 configs/spotmicro_config/config/ros_control/ros_control.yaml delete mode 100644 configs/spotmicro_config/launch/bringup.launch create mode 100644 configs/spotmicro_config/launch/bringup.launch.py delete mode 100644 configs/spotmicro_config/launch/gazebo.launch create mode 100644 configs/spotmicro_config/launch/gazebo.launch.py delete mode 100644 configs/spotmicro_config/launch/include/amcl.launch delete mode 100644 configs/spotmicro_config/launch/include/gmapping.launch delete mode 100644 configs/spotmicro_config/launch/include/move_base.launch delete mode 100644 configs/spotmicro_config/launch/navigate.launch create mode 100644 configs/spotmicro_config/launch/navigate.launch.py delete mode 100644 configs/spotmicro_config/launch/slam.launch create mode 100644 configs/spotmicro_config/launch/slam.launch.py delete mode 100644 configs/spotmicro_config/launch/spawn_robot.launch create mode 100644 configs/spotmicro_config/maps/playground.yaml create mode 100644 configs/spotmicro_config/worlds/playground.world diff --git a/configs/spotmicro_config/CMakeLists.txt b/configs/spotmicro_config/CMakeLists.txt index 010f26d..ac96358 100644 --- a/configs/spotmicro_config/CMakeLists.txt +++ b/configs/spotmicro_config/CMakeLists.txt @@ -1,6 +1,11 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(spotmicro_config) -find_package(catkin REQUIRED COMPONENTS) +find_package(ament_cmake REQUIRED) -catkin_package() \ No newline at end of file +install( + DIRECTORY launch config maps worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/configs/spotmicro_config/README.md b/configs/spotmicro_config/README.md index 32aa0b4..4216529 100644 --- a/configs/spotmicro_config/README.md +++ b/configs/spotmicro_config/README.md @@ -7,24 +7,20 @@ You don't need a physical robot to run the following demos. #### 1.1.1. Run the base driver: - roslaunch spotmicro_config bringup.launch rviz:=true + ros2 launch spotmicro_config bringup.launch.py rviz:=true -#### 1.1.2. Run the teleop node: - - roslaunch champ_teleop teleop.launch -If you want to use a [joystick](https://www.logitechg.com/en-hk/products/gamepads/f710-wireless-gamepad.html) add joy:=true as an argument. +#### 1.1.2. Run the teleop node: + ros2 launch champ_teleop teleop.launch.py ### 1.2. SLAM demo: #### 1.2.1. Run the Gazebo environment: - roslaunch spotmicro_config gazebo.launch + ros2 launch spotmicro_config gazebo.launch.py -#### 1.2.2. Run gmapping package and move_base: - - roslaunch spotmicro_config slam.launch rviz:=true +#### 1.2.2 Run [Nav2](https://navigation.ros.org/)'s navigation and [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox): To start mapping: @@ -35,18 +31,21 @@ To start mapping: - Save the map by running: - roscd spotmicro_config/maps - rosrun map_server map_saver + cd spotmicro_config/maps + ros2 run nav2_map_server map_saver_cli -f new_map + +After this, you can use the new_map to do pure navigation. + ### 1.3. Autonomous Navigation: #### 1.3.1. Run the Gazebo environment: - roslaunch spotmicro_config gazebo.launch + ros2 launch spotmicro_config gazebo.launch.py -#### 1.3.2. Run amcl and move_base: +#### 1.3.2 Run [Nav2](https://navigation.ros.org/): - roslaunch spotmicro_config navigate.launch rviz:=true + ros2 launch spotmicro_config navigate.launch.py rviz:=true To navigate: diff --git a/configs/spotmicro_config/config/autonomy/navigation.yaml b/configs/spotmicro_config/config/autonomy/navigation.yaml new file mode 100644 index 0000000..d5d81f6 --- /dev/null +++ b/configs/spotmicro_config/config/autonomy/navigation.yaml @@ -0,0 +1,383 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: True + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.6 + + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + + # Goal checker parameters + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.4 + max_vel_y: 0.0 + max_vel_theta: 0.75 + min_speed_xy: 0.0 + max_speed_xy: 0.4 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel2d_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan base_scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + base_scan: + topic: /base/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + voxel3d_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + mark_threshold: 1 + observation_sources: realsense zed + min_obstacle_height: 0.00 + max_obstacle_height: 2.0 + realsense: + topic: /camera/depth/color/points + raytrace_min_range: 0.5 + min_obstacle_height: 0.0 + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + zed: + topic: /zed/point_cloud/cloud_registered + raytrace_min_range: 0.5 + min_obstacle_height: 0.0 + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: False + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"] + voxel2d_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan base_scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + base_scan: + topic: /base/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + voxel3d_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + mark_threshold: 1 + observation_sources: realsense zed + min_obstacle_height: 0.00 + max_obstacle_height: 2.0 + realsense: + topic: /camera/depth/color/points + raytrace_min_range: 0.5 + min_obstacle_height: 0.0 + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + zed: + topic: /zed/point_cloud/cloud_registered + raytrace_min_range: 0.5 + min_obstacle_height: 0.0 + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "PointCloud2" + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "map.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/configs/spotmicro_config/config/autonomy/slam.yaml b/configs/spotmicro_config/config/autonomy/slam.yaml new file mode 100644 index 0000000..d783cb6 --- /dev/null +++ b/configs/spotmicro_config/config/autonomy/slam.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: mapping #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: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 5.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 diff --git a/configs/spotmicro_config/config/gait/gait.yaml b/configs/spotmicro_config/config/gait/gait.yaml deleted file mode 100644 index 8bc0cc4..0000000 --- a/configs/spotmicro_config/config/gait/gait.yaml +++ /dev/null @@ -1,11 +0,0 @@ -knee_orientation : ">>" -pantograph_leg : false -odom_scaler: 1.0 -max_linear_velocity_x : 0.5 -max_linear_velocity_y : 0.25 -max_angular_velocity_z : 1.0 -com_x_translation : 0.0 -swing_height : 0.02 -stance_depth : 0.0 -stance_duration : 0.25 -nominal_height : 0.15 \ No newline at end of file diff --git a/configs/spotmicro_config/config/joints/joints.yaml b/configs/spotmicro_config/config/joints/joints.yaml deleted file mode 100644 index 3827519..0000000 --- a/configs/spotmicro_config/config/joints/joints.yaml +++ /dev/null @@ -1,23 +0,0 @@ -left_front: - - front_left_shoulder - - front_left_leg - - front_left_foot - - front_left_toe - -right_front: - - front_right_shoulder - - front_right_leg - - front_right_foot - - front_right_toe - -left_hind: - - rear_left_shoulder - - rear_left_leg - - rear_left_foot - - rear_left_toe - -right_hind: - - rear_right_shoulder - - rear_right_leg - - rear_right_foot - - rear_right_toe \ No newline at end of file diff --git a/configs/spotmicro_config/config/links/links.yaml b/configs/spotmicro_config/config/links/links.yaml deleted file mode 100644 index aa82b04..0000000 --- a/configs/spotmicro_config/config/links/links.yaml +++ /dev/null @@ -1,24 +0,0 @@ -base: base_link -left_front: - - front_left_shoulder_link - - front_left_leg_link - - front_left_foot_link - - front_left_toe_link - -right_front: - - front_right_shoulder_link - - front_right_leg_link - - front_right_foot_link - - front_right_toe_link - -left_hind: - - rear_left_shoulder_link - - rear_left_leg_link - - rear_left_foot_link - - rear_left_toe_link - -right_hind: - - rear_right_shoulder_link - - rear_right_leg_link - - rear_right_foot_link - - rear_right_toe_link \ No newline at end of file diff --git a/configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml b/configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml deleted file mode 100644 index d05c9d3..0000000 --- a/configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml +++ /dev/null @@ -1,46 +0,0 @@ -DWAPlannerROS: - #http://wiki.ros.org/dwa_local_planner - - min_vel_trans: 0.01 - max_vel_trans: 0.5 - - min_vel_x: -0.025 - max_vel_x: 0.5 - - min_vel_y: 0.0 - max_vel_y: 0.0 - - max_vel_rot: 1.0 - min_vel_rot: -1.0 - - acc_lim_trans: 1.7 - acc_lim_x: 1.7 - acc_lim_y: 0.0 - acc_lim_theta: 3 - - trans_stopped_vel: 0.1 - theta_stopped_vel: 0.1 - - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.34 - - sim_time: 3.5 - sim_granularity: 0.1 - vx_samples: 20 - vy_samples: 0 - vth_samples: 40 - - path_distance_bias: 34.0 - goal_distance_bias: 24.0 - occdist_scale: 0.05 - - forward_point_distance: 0.2 - stop_time_buffer: 0.5 - scaling_speed: 0.25 - max_scaling_factor: 0.2 - - oscillation_reset_dist: 0.05 - - use_dwa: true - - prune_plan: false diff --git a/configs/spotmicro_config/config/move_base/costmap_common_params.yaml b/configs/spotmicro_config/config/move_base/costmap_common_params.yaml deleted file mode 100644 index d24a93f..0000000 --- a/configs/spotmicro_config/config/move_base/costmap_common_params.yaml +++ /dev/null @@ -1,28 +0,0 @@ -obstacle_range: 2.5 -raytrace_range: 3.0 -footprint: [[-0.25, -0.145], [-0.25, 0.145], [0.25, 0.145], [0.25, -0.145]] - -transform_tolerance: 0.5 -resolution: 0.05 - -static_map_layer: - map_topic: map - subscribe_to_updates: true - -2d_obstacles_layer: - observation_sources: scan - scan: {data_type: LaserScan, - topic: scan, - marking: true, - clearing: true} - -3d_obstacles_layer: - observation_sources: depth - depth: {data_type: PointCloud2, - topic: camera/depth/points, - min_obstacle_height: 0.1, - marking: true, - clearing: true} - -inflation_layer: - inflation_radius: 2.0 \ No newline at end of file diff --git a/configs/spotmicro_config/config/move_base/global_costmap_params.yaml b/configs/spotmicro_config/config/move_base/global_costmap_params.yaml deleted file mode 100644 index 107b26a..0000000 --- a/configs/spotmicro_config/config/move_base/global_costmap_params.yaml +++ /dev/null @@ -1,13 +0,0 @@ -global_costmap: - global_frame: map - robot_base_frame: base_footprint - update_frequency: 1.0 - publish_frequency: 0.5 - static_map: true - cost_scaling_factor: 10.0 - - plugins: - - {name: static_map_layer, type: "costmap_2d::StaticLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} - - {name: 2d_obstacles_layer, type: "costmap_2d::ObstacleLayer"} - - {name: 3d_obstacles_layer, type: "costmap_2d::VoxelLayer"} diff --git a/configs/spotmicro_config/config/move_base/local_costmap_params.yaml b/configs/spotmicro_config/config/move_base/local_costmap_params.yaml deleted file mode 100644 index 0be7cb3..0000000 --- a/configs/spotmicro_config/config/move_base/local_costmap_params.yaml +++ /dev/null @@ -1,16 +0,0 @@ -local_costmap: - global_frame: odom - robot_base_frame: base_footprint - update_frequency: 1.0 - publish_frequency: 2.0 - - static_map: false - rolling_window: true - width: 2.5 - height: 2.5 - cost_scaling_factor: 5 - - plugins: - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} - - {name: 2d_obstacles_layer, type: "costmap_2d::ObstacleLayer"} - - {name: 3d_obstacles_layer, type: "costmap_2d::VoxelLayer"} diff --git a/configs/spotmicro_config/config/move_base/move_base_params.yaml b/configs/spotmicro_config/config/move_base/move_base_params.yaml deleted file mode 100644 index f975528..0000000 --- a/configs/spotmicro_config/config/move_base/move_base_params.yaml +++ /dev/null @@ -1,15 +0,0 @@ -base_global_planner: global_planner/GlobalPlanner -base_local_planner: dwa_local_planner/DWAPlannerROS - -shutdown_costmaps: false - -controller_frequency: 5.0 -controller_patience: 3.0 - -planner_frequency: 0.5 -planner_patience: 5.0 - -oscillation_timeout: 10.0 -oscillation_distance: 0.2 - -conservative_reset_dist: 0.1 \ No newline at end of file diff --git a/configs/spotmicro_config/config/ros_control/ros_control.yaml b/configs/spotmicro_config/config/ros_control/ros_control.yaml deleted file mode 100644 index 6b84d10..0000000 --- a/configs/spotmicro_config/config/ros_control/ros_control.yaml +++ /dev/null @@ -1,22 +0,0 @@ -"": - joint_states_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - - joint_group_position_controller: - type: position_controllers/JointTrajectoryController - joints: - - front_left_shoulder - - front_left_leg - - front_left_foot - - front_right_shoulder - - front_right_leg - - front_right_foot - - rear_left_shoulder - - rear_left_leg - - rear_left_foot - - rear_right_shoulder - - rear_right_leg - - rear_right_foot - - # Servomotors have inner control loops so PIDs are not needed diff --git a/configs/spotmicro_config/launch/bringup.launch b/configs/spotmicro_config/launch/bringup.launch deleted file mode 100644 index e5b53d3..0000000 --- a/configs/spotmicro_config/launch/bringup.launch +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/configs/spotmicro_config/launch/bringup.launch.py b/configs/spotmicro_config/launch/bringup.launch.py new file mode 100644 index 0000000..383d4b1 --- /dev/null +++ b/configs/spotmicro_config/launch/bringup.launch.py @@ -0,0 +1,83 @@ +# Copyright (c) 2021 Juan Miguel Jimeno +# +# 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. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + description_package = FindPackageShare('spotmicro_description') + + joints_config = PathJoinSubstitution( + [description_package, 'config', 'champ', 'joints.yaml'] + ) + links_config = PathJoinSubstitution( + [description_package, 'config', 'champ', 'links.yaml'] + ) + gait_config = PathJoinSubstitution( + [description_package, 'config', 'champ', 'gait.yaml'] + ) + description_path = PathJoinSubstitution( + [description_package, 'urdf', 'spotmicro_description.urdf.xacro'] + ) + bringup_launch_path = PathJoinSubstitution( + [FindPackageShare('champ_bringup'), 'launch', 'bringup.launch.py'] + ) + + return LaunchDescription([ + DeclareLaunchArgument( + name='robot_name', + default_value='', + description='Set robot name for multi robot' + ), + + DeclareLaunchArgument( + name='sim', + default_value='false', + description='Enable use_sime_time to true' + ), + + DeclareLaunchArgument( + name='rviz', + default_value='false', + description='Run rviz' + ), + + DeclareLaunchArgument( + name='hardware_connected', + default_value='false', + description='Set to true if connected to a physical robot' + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(bringup_launch_path), + launch_arguments={ + "use_sim_time": LaunchConfiguration("sim"), + "robot_name": LaunchConfiguration("robot_name"), + "gazebo": LaunchConfiguration("sim"), + "rviz": LaunchConfiguration("rviz"), + "hardware_connected": LaunchConfiguration("hardware_connected"), + "publish_foot_contacts": "true", + "close_loop_odom": "true", + "joint_controller_topic": "joint_group_effort_controller/joint_trajectory", + "joints_map_path": joints_config, + "links_map_path": links_config, + "gait_config_path": gait_config, + "description_path": description_path + }.items(), + ) + ]) diff --git a/configs/spotmicro_config/launch/gazebo.launch b/configs/spotmicro_config/launch/gazebo.launch deleted file mode 100644 index 94f7363..0000000 --- a/configs/spotmicro_config/launch/gazebo.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/configs/spotmicro_config/launch/gazebo.launch.py b/configs/spotmicro_config/launch/gazebo.launch.py new file mode 100644 index 0000000..762549f --- /dev/null +++ b/configs/spotmicro_config/launch/gazebo.launch.py @@ -0,0 +1,125 @@ +import os + +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration("use_sim_time") + description_path = LaunchConfiguration("description_path") + base_frame = "base_link" + + config_pkg_share = launch_ros.substitutions.FindPackageShare( + package="spotmicro_config" + ).find("spotmicro_config") + descr_pkg_share = launch_ros.substitutions.FindPackageShare( + package="spotmicro_description" + ).find("spotmicro_description") + joints_config = os.path.join(descr_pkg_share, "config", "champ", "joints.yaml") + gait_config = os.path.join(descr_pkg_share, "config", "champ", "gait.yaml") + links_config = os.path.join(descr_pkg_share, "config", "champ", "links.yaml") + default_model_path = os.path.join(descr_pkg_share, "urdf", "spotmicro_description.urdf.xacro") + default_world_path = os.path.join(config_pkg_share, "worlds", "outdoor.world") + + declare_use_sim_time = DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + declare_rviz = DeclareLaunchArgument( + "rviz", default_value="false", description="Launch rviz" + ) + declare_robot_name = DeclareLaunchArgument( + "robot_name", default_value="spotmicro", description="Robot name" + ) + declare_lite = DeclareLaunchArgument( + "lite", default_value="false", description="Lite" + ) + + declare_gazebo_world = DeclareLaunchArgument( + "world", default_value=default_world_path, description="Gazebo world name" + ) + + declare_gui = DeclareLaunchArgument( + "gui", default_value="true", description="Use gui" + ) + declare_world_init_x = DeclareLaunchArgument("world_init_x", default_value="0.0") + declare_world_init_y = DeclareLaunchArgument("world_init_y", default_value="0.0") + declare_world_init_heading = DeclareLaunchArgument( + "world_init_heading", default_value="0.6" + ) + + bringup_ld = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("champ_bringup"), + "launch", + "bringup.launch.py", + ) + ), + launch_arguments={ + "description_path": default_model_path, + "joints_map_path": joints_config, + "links_map_path": links_config, + "gait_config_path": gait_config, + "use_sim_time": LaunchConfiguration("use_sim_time"), + "robot_name": LaunchConfiguration("robot_name"), + "gazebo": "true", + "lite": LaunchConfiguration("lite"), + "rviz": LaunchConfiguration("rviz"), + "joint_controller_topic": "joint_group_effort_controller/joint_trajectory", + "hardware_connected": "false", + "publish_foot_contacts": "false", + "close_loop_odom": "true", + }.items(), + ) + + gazebo_ld = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("champ_gazebo"), + "launch", + "gazebo.launch.py", + ) + ), + launch_arguments={ + "use_sim_time": LaunchConfiguration("use_sim_time"), + "robot_name": LaunchConfiguration("robot_name"), + "world": LaunchConfiguration("world"), + "lite": LaunchConfiguration("lite"), + "world_init_x": LaunchConfiguration("world_init_x"), + "world_init_y": LaunchConfiguration("world_init_y"), + "world_init_heading": LaunchConfiguration("world_init_heading"), + "gui": LaunchConfiguration("gui"), + "close_loop_odom": "true", + }.items(), + ) + + return LaunchDescription( + [ + declare_use_sim_time, + declare_rviz, + declare_robot_name, + declare_lite, + declare_gazebo_world, + declare_gui, + declare_world_init_x, + declare_world_init_y, + declare_world_init_heading, + bringup_ld, + gazebo_ld + + ] + ) diff --git a/configs/spotmicro_config/launch/include/amcl.launch b/configs/spotmicro_config/launch/include/amcl.launch deleted file mode 100644 index 635bd34..0000000 --- a/configs/spotmicro_config/launch/include/amcl.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/configs/spotmicro_config/launch/include/gmapping.launch b/configs/spotmicro_config/launch/include/gmapping.launch deleted file mode 100644 index 8bbf58b..0000000 --- a/configs/spotmicro_config/launch/include/gmapping.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/configs/spotmicro_config/launch/include/move_base.launch b/configs/spotmicro_config/launch/include/move_base.launch deleted file mode 100644 index 44235d0..0000000 --- a/configs/spotmicro_config/launch/include/move_base.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/configs/spotmicro_config/launch/navigate.launch b/configs/spotmicro_config/launch/navigate.launch deleted file mode 100644 index 4e1fa3f..0000000 --- a/configs/spotmicro_config/launch/navigate.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/configs/spotmicro_config/launch/navigate.launch.py b/configs/spotmicro_config/launch/navigate.launch.py new file mode 100644 index 0000000..ef964bc --- /dev/null +++ b/configs/spotmicro_config/launch/navigate.launch.py @@ -0,0 +1,72 @@ +# Copyright (c) 2021 Juan Miguel Jimeno +# +# 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. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + this_package = FindPackageShare('spotmicro_config') + + default_map_path = PathJoinSubstitution( + [this_package, 'maps', 'map.yaml'] + ) + + default_params_file_path = PathJoinSubstitution( + [this_package, 'config', 'autonomy', 'navigation.yaml'] + ) + + nav2_launch_path = PathJoinSubstitution( + [FindPackageShare('champ_navigation'), 'launch', 'navigate.launch.py'] + ) + + return LaunchDescription([ + DeclareLaunchArgument( + name='map', + default_value=default_map_path, + description='Navigation map path' + ), + + DeclareLaunchArgument( + name='params_file', + default_value=default_params_file_path, + description='Navigation2 params file' + ), + + DeclareLaunchArgument( + name='sim', + default_value='false', + description='Enable use_sime_time to true' + ), + + DeclareLaunchArgument( + name='rviz', + default_value='false', + description='Run rviz' + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav2_launch_path), + launch_arguments={ + 'params_file': LaunchConfiguration("params_file"), + 'map': LaunchConfiguration("map"), + 'sim': LaunchConfiguration("sim"), + 'rviz': LaunchConfiguration("rviz") + }.items() + ) + ]) diff --git a/configs/spotmicro_config/launch/slam.launch b/configs/spotmicro_config/launch/slam.launch deleted file mode 100644 index 4cc5ebd..0000000 --- a/configs/spotmicro_config/launch/slam.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/configs/spotmicro_config/launch/slam.launch.py b/configs/spotmicro_config/launch/slam.launch.py new file mode 100644 index 0000000..49be441 --- /dev/null +++ b/configs/spotmicro_config/launch/slam.launch.py @@ -0,0 +1,61 @@ +# Copyright (c) 2021 Juan Miguel Jimeno +# +# 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. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + this_package = FindPackageShare('mini_pupper_config') + + default_params_file_path = PathJoinSubstitution( + [this_package, 'config', 'autonomy', 'slam.yaml'] + ) + + slam_launch_path = PathJoinSubstitution( + [FindPackageShare('champ_navigation'), 'launch', 'slam.launch.py'] + ) + + return LaunchDescription([ + DeclareLaunchArgument( + name='slam_params_file', + default_value=default_params_file_path, + description='Navigation2 slam params file' + ), + + DeclareLaunchArgument( + name='sim', + default_value='false', + description='Enable use_sime_time to true' + ), + + DeclareLaunchArgument( + name='rviz', + default_value='false', + description='Run rviz' + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_path), + launch_arguments={ + 'slam_params_file': LaunchConfiguration("slam_params_file"), + 'sim': LaunchConfiguration("sim"), + 'rviz': LaunchConfiguration("rviz") + }.items() + ) + ]) diff --git a/configs/spotmicro_config/launch/spawn_robot.launch b/configs/spotmicro_config/launch/spawn_robot.launch deleted file mode 100644 index b2bb246..0000000 --- a/configs/spotmicro_config/launch/spawn_robot.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/configs/spotmicro_config/maps/playground.yaml b/configs/spotmicro_config/maps/playground.yaml new file mode 100644 index 0000000..a0e63f9 --- /dev/null +++ b/configs/spotmicro_config/maps/playground.yaml @@ -0,0 +1,7 @@ +image: playground.pgm +mode: trinary +resolution: 0.05 +origin: [-10.3, -10.5, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/configs/spotmicro_config/package.xml b/configs/spotmicro_config/package.xml index 8badc9a..ff2a429 100644 --- a/configs/spotmicro_config/package.xml +++ b/configs/spotmicro_config/package.xml @@ -1,18 +1,22 @@ - + spotmicro_config 0.1.0 - spotmicro Champ Config Package + CHAMP Config Package Juan Miguel Jimeno Juan Miguel Jimeno BSD - catkin + ament_cmake champ_base - ros_controllers - roslaunch - rviz - + launch + launch_ros + rviz2 + gazebo_plugins + + + ament_cmake + diff --git a/configs/spotmicro_config/worlds/playground.world b/configs/spotmicro_config/worlds/playground.world new file mode 100644 index 0000000..d199a8c --- /dev/null +++ b/configs/spotmicro_config/worlds/playground.world @@ -0,0 +1,1564 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + + + + + 20 20 0.1 + + + 10 + + + + + + + + + + + + + + + 0 + + + 20 20 0.1 + + + + + + + 0 + 0 + 0 + + -0.066287 0.086676 0 0 -0 0 + + + 1 + + + + + model://playground/meshes/playground.dae + + + 10 + + + + + + + + + + + + + + + + + model://playground/meshes/playground.dae + + + + 0 + 0 + 0 + + 0.556672 -0.365718 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + -8.74428 -9.11584 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + -9.17408 -6.9914 0 0 -0 0 + + + 1 + + + + + model://pine_tree/meshes/pine_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Branch + + + + + + + + + + + model://pine_tree/meshes/pine_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + -5.79941 -9.32885 0 0 -0 0 + + + 1 + + + + + model://lamp_post/meshes/lamp_post.dae + 3 3 3 + + + 10 + + + + + + + + + + + + + + + + + model://lamp_post/meshes/lamp_post.dae + 3 3 3 + + + + 0 + 0 + 0 + + 8.73716 -8.92816 0 0 -0 0 + + + 1 + + + + + model://lamp_post/meshes/lamp_post.dae + 3 3 3 + + + 10 + + + + + + + + + + + + + + + + + model://lamp_post/meshes/lamp_post.dae + 3 3 3 + + + + 0 + 0 + 0 + + 8.76856 8.53222 0 0 0 -2.10022 + + + 1 + + + + + model://oak_tree/meshes/oak_tree.dae + + + 10 + + + + + + + + + + + + + + + + + model://oak_tree/meshes/oak_tree.dae + + Branch + + + + + + + + + + + model://oak_tree/meshes/oak_tree.dae + + Bark + + + + + + + + 0 + 0 + 0 + + -7.84166 8.24695 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + 10.9243 2.40595 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + 10.0573 -0.353256 0 0 -0 1.54836 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + 10.0327 -6.98111 0 0 -0 1.54836 + + + 1 + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + 10 + + + + + + + + + + + + + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + + 0 + 0 + 0 + + 8.1439 -9.70032 0 0 -0 0 + + + 1 + + 0 0 1.5 0 -0 0 + + + + 3 1 3 + + + 10 + + + + + + + + + + + + + + + + + model://brick_box_3x1x3/meshes/simple_box.dae + 3 1 3 + + + + + + + 0 + 0 + 0 + + -3.10709 9.8237 0 0 -0 0 + + + 1 + + 0 0 1.5 0 -0 0 + + + + 3 1 3 + + + 10 + + + + + + + + + + + + + + + + + model://brick_box_3x1x3/meshes/simple_box.dae + 3 1 3 + + + + + + + 0 + 0 + 0 + + 3.64111 9.85455 0 0 -0 0 + + + 0 + 0 + 0 + 0 + + 4.68902 -9.79948 0.049992 0 -8e-06 0.014447 + 1 1 1 + + 4.68902 -9.79948 0.049992 0 -8e-06 0.014447 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 8.73716 -8.92816 0 0 0 -2.10022 + 1 1 1 + + 8.73716 -8.92816 0 0 0 -2.10022 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 8.76856 8.53222 0 0 0 -1.04217 + 1 1 1 + + 8.76856 8.53222 0 0 0 -1.04217 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.089672 0.059574 0 0 -0 0 + 1 1 1 + + -0.089672 0.059574 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3.10709 9.8237 0 0 -0 0 + 1 1 1 + + -3.10709 9.8237 1.5 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 3.64111 9.85455 0 0 -0 0 + 1 1 1 + + 3.64111 9.85455 1.5 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1.1899 -9.96513 0 0 -0 1.35112 + 1 1 1 + + -1.1899 -9.96513 0 0 -0 1.35112 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2.38207 -10.1405 0 0 -0 1.60373 + 1 1 1 + + 2.38207 -10.1405 0 0 -0 1.60373 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 8.1439 -9.70032 0 0 -0 0 + 1 1 1 + + 8.1439 -9.70032 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -10.1561 -5.99504 0 0 -0 1.58739 + 1 1 1 + + -10.1561 -5.99504 1.4 0 -0 1.58739 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -10.2657 1.37707 0 0 -0 1.58739 + 1 1 1 + + -10.2657 1.37707 1.4 0 -0 1.58739 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -10.3624 6.45785 0 0 -0 1.58739 + 1 1 1 + + -10.3624 6.45785 1.4 0 -0 1.58739 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.0511 5.66451 0 0 -0 1.54836 + 1 1 1 + + 10.0511 5.66451 0 0 -0 1.54836 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.0573 -0.353256 0 0 -0 1.54836 + 1 1 1 + + 10.0573 -0.353256 0 0 -0 1.54836 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.0327 -6.98111 0 0 -0 1.54836 + 1 1 1 + + 10.0327 -6.98111 0 0 -0 1.54836 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -7.84166 8.24695 0 0 -0 0 + 1 1 1 + + -7.84166 8.24695 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.74428 -9.11584 0 0 -0 0 + 1 1 1 + + -8.74428 -9.11584 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -9.17408 -6.9914 0 0 -0 0 + 1 1 1 + + -9.17408 -6.9914 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -5.79941 -9.32885 0 0 -0 0 + 1 1 1 + + -5.79941 -9.32885 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.556672 -0.365718 0 0 -0 0.836001 + 1 1 1 + + 0.556672 -0.365718 0 0 -0 0.836001 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + -5.63054 -11.2657 1 0 -0 0 + + + 10.4762 9.60881 1 0 -0 0 + + + + 1 + + + + + model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae + + + + + + + model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + -2.30016 -10.1505 0 0 -0 0 + + + 1 + + + + + model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae + + + + + + + model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae + + + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + 0.387818 -10.2088 0 0 -0 1.35112 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + 4.68291 -9.79023 0 0 -0 0 + + + 1 + + 0 0 1.4 0 -0 0 + + + + 7.5 0.2 2.8 + + + 10 + + + + + + + + + + + + + + + 0 + + + 7.5 0.2 2.8 + + + + + + + 0 + 0 + 0 + + -17.584 -9.25495 0 0 -0 0 + + + 1 + + 0 0 1.4 0 -0 0 + + + + 7.5 0.2 2.8 + + + 10 + + + + + + + + + + + + + + + 0 + + + 7.5 0.2 2.8 + + + + + + + 0 + 0 + 0 + + -10.3087 1.37636 0 0 -0 1.58739 + + + 1 + + 0 0 1.4 0 -0 0 + + + + 7.5 0.2 2.8 + + + 10 + + + + + + + + + + + + + + + 0 + + + 7.5 0.2 2.8 + + + + + + + 0 + 0 + 0 + + -10.3624 6.45785 0 0 -0 1.58739 + + + -5.63054 -11.2657 1 0 -0 0 + 0.5 0.5 0.5 1 + 0.1 0.1 0.1 1 + 0.1 0.1 -0.9 + + 20 + 0.5 + 0.01 + 0.001 + + 1 + + + 10.4762 9.60881 1 0 -0 0 + 0.5 0.5 0.5 1 + 0.1 0.1 0.1 1 + 0.1 0.1 -0.9 + + 20 + 0.5 + 0.01 + 0.001 + + 1 + + + + 18.2797 -9.50323 6.40337 0 0.311643 2.68019 + orbit + perspective + + + +