diff --git a/config/diffbot_controllers.yaml b/config/diffbot_controllers.yaml index 6e51218..842acf8 100644 --- a/config/diffbot_controllers.yaml +++ b/config/diffbot_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 30 # Hz was 100 vefore joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -24,7 +24,7 @@ diffbot_base_controller: left_wheel_radius_multiplier: -1.0 right_wheel_radius_multiplier: 1.0 - publish_rate: 100.0 + publish_rate: 50.0 # was 100 before odom_frame_id: odom base_frame_id: base_link # pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index f6cad1e..c50ffda 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -26,7 +26,7 @@ slam_toolbox: debug_logging: false throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry - map_update_interval: 5.0 + map_update_interval: 1.0 #was 5.0 resolution: 0.05 max_laser_range: 200.0 #for rastering images, standard was 20 minimum_time_interval: 0.5 diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml new file mode 100644 index 0000000..14fe6e4 --- /dev/null +++ b/config/nav2_params.yaml @@ -0,0 +1,348 @@ +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 + +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 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_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 + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_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 + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + 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.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + 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 + +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: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + 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 + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +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 + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 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: + use_sim_time: True + 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 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/config/twist_mux.yaml b/config/twist_mux.yaml index 97411fc..375a961 100644 --- a/config/twist_mux.yaml +++ b/config/twist_mux.yaml @@ -12,4 +12,4 @@ twist_mux: joystick: topic : cmd_vel_joy timeout : 0.5 - priority: 100 \ No newline at end of file + priority: 5 \ No newline at end of file diff --git a/launch/nav2.launch.py b/launch/nav2.launch.py new file mode 100644 index 0000000..f016df9 --- /dev/null +++ b/launch/nav2.launch.py @@ -0,0 +1,270 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('bot_mini_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld \ No newline at end of file diff --git a/launch/cam.launch.py b/launch/robot_cam.launch.py similarity index 100% rename from launch/cam.launch.py rename to launch/robot_cam.launch.py diff --git a/launch/controller.launch.py b/launch/robot_controller.launch.py similarity index 100% rename from launch/controller.launch.py rename to launch/robot_controller.launch.py diff --git a/launch/joy_teleop.launch.py b/launch/robot_joy_teleop.launch.py similarity index 85% rename from launch/joy_teleop.launch.py rename to launch/robot_joy_teleop.launch.py index 46a45b4..747444b 100644 --- a/launch/joy_teleop.launch.py +++ b/launch/robot_joy_teleop.launch.py @@ -79,7 +79,8 @@ def generate_launch_description(): teleop_spawner = Node( package="rmp220_teleop", - executable="rmp220_teleop" + executable="rmp220_teleop", + remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] ) cam_node = Node( @@ -98,6 +99,14 @@ def generate_launch_description(): parameters=[lidar_dir], ) + twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml') + twist_mux = Node( + package="twist_mux", + executable="twist_mux", + parameters=[twist_mux_params, {'use_sim_time': False}], + remappings=[('/cmd_vel_out','/diffbot_base_controller/cmd_vel_unstamped')] + ) + return LaunchDescription([ #control_node, #robot_state_pub_node, @@ -106,5 +115,6 @@ def generate_launch_description(): joystick_spawner, teleop_spawner, #cam_node, - #lidar_node + #lidar_node, + #twist_mux ]) \ No newline at end of file diff --git a/launch/lidar.launch.py b/launch/robot_lidar.launch.py similarity index 100% rename from launch/lidar.launch.py rename to launch/robot_lidar.launch.py diff --git a/launch/mapper.launch.py b/launch/robot_localization.launch.py similarity index 99% rename from launch/mapper.launch.py rename to launch/robot_localization.launch.py index 293c970..36c61eb 100644 --- a/launch/mapper.launch.py +++ b/launch/robot_localization.launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): executable="cam_node" ) - use_sim_time = True + use_sim_time = False slam_params_file = PathJoinSubstitution( [ FindPackageShare("bot_mini_bringup"), diff --git a/launch/robot_mapper.launch.py b/launch/robot_mapper.launch.py new file mode 100644 index 0000000..36c61eb --- /dev/null +++ b/launch/robot_mapper.launch.py @@ -0,0 +1,131 @@ +# Copyright 2022 Factor Robotics +# +# 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.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + use_sim_time = False + slam_params_file = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "mapper_params_online_async.yaml" + ] + ) + mapper_node = Node( + package="slam_toolbox", + executable="async_slam_toolbox_node", + name='slam_toolbox_node', + output='screen', + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time} + ], + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + #lidar_node, + mapper_node + ]) \ No newline at end of file diff --git a/launch/robot_navigation.launch.py b/launch/robot_navigation.launch.py new file mode 100644 index 0000000..f016df9 --- /dev/null +++ b/launch/robot_navigation.launch.py @@ -0,0 +1,270 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('bot_mini_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld \ No newline at end of file diff --git a/launch/robot_twist_mux.launch.py b/launch/robot_twist_mux.launch.py new file mode 100644 index 0000000..33c1b01 --- /dev/null +++ b/launch/robot_twist_mux.launch.py @@ -0,0 +1,120 @@ +# Copyright 2022 Factor Robotics +# +# 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.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop", + remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml') + twist_mux = Node( + package="twist_mux", + executable="twist_mux", + parameters=[twist_mux_params, {'use_sim_time': False}], + remappings=[('/cmd_vel_out','/diffbot_base_controller/cmd_vel_unstamped')] + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + #lidar_node, + twist_mux + ]) \ No newline at end of file diff --git a/launch/sim.launch.py b/launch/sim.launch.py new file mode 100644 index 0000000..a8dcfa6 --- /dev/null +++ b/launch/sim.launch.py @@ -0,0 +1,160 @@ +# Copyright 2022 Factor Robotics +# +# 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 IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop", + remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] + ) + + joystick_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('bot_mini_bringup'),'joy_teleop.launch.py' + )]), launch_arguments={'use_sim_time': 'true'}.items() + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml') + twist_mux = Node( + package="twist_mux", + executable="twist_mux", + parameters=[twist_mux_params, {'use_sim_time': False}], + remappings=[('/cmd_vel_out','/diffbot_base_controller/cmd_vel_unstamped')] + ) + + gazebo_params_file = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','gazebo_params.yaml') + # Include the Gazebo launch file, provided by the gazebo_ros package + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), + launch_arguments={'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_file}.items() + ) + + # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot. + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'my_bot'], + output='screen') + + diff_drive_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diff_cont"], + ) + + joint_broad_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_broad"], + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + #lidar_node, + twist_mux, + gazebo, + spawn_entity, + joystick_launch, + diff_drive_spawner, + joint_broad_spawner + ]) \ No newline at end of file diff --git a/maps/cps_save_map.pgm b/maps/cps_save_map.pgm new file mode 100644 index 0000000..0365447 Binary files /dev/null and b/maps/cps_save_map.pgm differ diff --git a/maps/cps_save_map.yaml b/maps/cps_save_map.yaml new file mode 100644 index 0000000..14cd9e1 --- /dev/null +++ b/maps/cps_save_map.yaml @@ -0,0 +1,7 @@ +image: ganz_save_map.pgm +mode: trinary +resolution: 0.05 +origin: [0.192, 31.9, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/maps/cps_ser_map.data b/maps/cps_ser_map.data new file mode 100644 index 0000000..1360ccb Binary files /dev/null and b/maps/cps_ser_map.data differ diff --git a/maps/cps_ser_map.posegraph b/maps/cps_ser_map.posegraph new file mode 100644 index 0000000..43dfbda Binary files /dev/null and b/maps/cps_ser_map.posegraph differ diff --git a/maps/default_map_save.pgm b/maps/default_map_save.pgm new file mode 100644 index 0000000..745e26f Binary files /dev/null and b/maps/default_map_save.pgm differ diff --git a/maps/default_map_save.yaml b/maps/default_map_save.yaml new file mode 100644 index 0000000..55d3ba9 --- /dev/null +++ b/maps/default_map_save.yaml @@ -0,0 +1,7 @@ +image: office1_wb_konrad_map_save.pgm +mode: trinary +resolution: 0.05 +origin: [-4.41, -17.2, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/maps/default_ser_save.data b/maps/default_ser_save.data new file mode 100644 index 0000000..c59391e Binary files /dev/null and b/maps/default_ser_save.data differ diff --git a/maps/default_ser_save.posegraph b/maps/default_ser_save.posegraph new file mode 100644 index 0000000..717b302 Binary files /dev/null and b/maps/default_ser_save.posegraph differ diff --git a/maps/office1_map_save.pgm b/maps/office1_map_save.pgm new file mode 100644 index 0000000..e699d50 Binary files /dev/null and b/maps/office1_map_save.pgm differ diff --git a/maps/office1_map_save.yaml b/maps/office1_map_save.yaml new file mode 100644 index 0000000..233f8b9 --- /dev/null +++ b/maps/office1_map_save.yaml @@ -0,0 +1,7 @@ +image: office1_map_save.pgm +mode: trinary +resolution: 0.05 +origin: [1.87, -8.61, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/maps/office1_map_serial.data b/maps/office1_map_serial.data new file mode 100644 index 0000000..c0a1b21 Binary files /dev/null and b/maps/office1_map_serial.data differ diff --git a/maps/office1_map_serial.posegraph b/maps/office1_map_serial.posegraph new file mode 100644 index 0000000..9a5953c Binary files /dev/null and b/maps/office1_map_serial.posegraph differ diff --git a/maps/office1_wb_konrad_map_save.pgm b/maps/office1_wb_konrad_map_save.pgm new file mode 100644 index 0000000..745e26f Binary files /dev/null and b/maps/office1_wb_konrad_map_save.pgm differ diff --git a/maps/office1_wb_konrad_map_save.yaml b/maps/office1_wb_konrad_map_save.yaml new file mode 100644 index 0000000..55d3ba9 --- /dev/null +++ b/maps/office1_wb_konrad_map_save.yaml @@ -0,0 +1,7 @@ +image: office1_wb_konrad_map_save.pgm +mode: trinary +resolution: 0.05 +origin: [-4.41, -17.2, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/maps/office1_wb_konrad_ser_save.data b/maps/office1_wb_konrad_ser_save.data new file mode 100644 index 0000000..c59391e Binary files /dev/null and b/maps/office1_wb_konrad_ser_save.data differ diff --git a/maps/office1_wb_konrad_ser_save.posegraph b/maps/office1_wb_konrad_ser_save.posegraph new file mode 100644 index 0000000..717b302 Binary files /dev/null and b/maps/office1_wb_konrad_ser_save.posegraph differ diff --git a/setup.py b/setup.py index 49457ac..0ce3dbe 100644 --- a/setup.py +++ b/setup.py @@ -13,7 +13,8 @@ setup( ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name), glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml'))#[1:] + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), #[1:] + (os.path.join('share', package_name, 'maps'), glob('maps/*')) ], install_requires=['setuptools'], zip_safe=True,