From d9a618bcb938769174ee66eb928ab68794341cfe Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Wed, 12 Jul 2023 09:11:18 +0800 Subject: [PATCH] Further nav2 work --- src/dcaiti_control/launch/launch_sim.py | 7 ++- src/dcaiti_control/launch/rsp.launch.py | 3 +- src/dcaiti_navigation/launch/start_nav2.py | 57 +------------------ src/dcaiti_navigation/params/nav2_params.yaml | 10 ++-- src/dcaitirobot/config/slam.yml | 4 +- src/dcaitirobot/launch/launch_slam.py | 23 +------- 6 files changed, 16 insertions(+), 88 deletions(-) diff --git a/src/dcaiti_control/launch/launch_sim.py b/src/dcaiti_control/launch/launch_sim.py index 670d343..6a1918e 100644 --- a/src/dcaiti_control/launch/launch_sim.py +++ b/src/dcaiti_control/launch/launch_sim.py @@ -25,7 +25,7 @@ def generate_launch_description(): package_name='dcaiti_control' #<--- CHANGE ME use_ros2_control = LaunchConfiguration('use_ros2_control') - world_path='~/.gazebo/models' + world_path='worlds/cafe.world' rsp = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( @@ -34,6 +34,8 @@ def generate_launch_description(): ) gazebo_params_path = os.path.join(get_package_share_directory(package_name),'config','gazebo_config.yml') # 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')]), @@ -44,7 +46,8 @@ def generate_launch_description(): spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=[ '-topic', 'robot_description', - '-entity', 'dcaiti_control' + '-entity', 'dcaiti_control', + '-z', '0.2', ], output='screen' ) diff --git a/src/dcaiti_control/launch/rsp.launch.py b/src/dcaiti_control/launch/rsp.launch.py index 51f0a35..ef0d48e 100644 --- a/src/dcaiti_control/launch/rsp.launch.py +++ b/src/dcaiti_control/launch/rsp.launch.py @@ -32,7 +32,8 @@ def generate_launch_description(): node_joint_state_publisher = Node( package='joint_state_publisher', - executable='joint_state_publisher' + executable='joint_state_publisher', + parameters=[{'use_sim_time': use_sim_time}] ) diff --git a/src/dcaiti_navigation/launch/start_nav2.py b/src/dcaiti_navigation/launch/start_nav2.py index b346aa2..5a083f0 100644 --- a/src/dcaiti_navigation/launch/start_nav2.py +++ b/src/dcaiti_navigation/launch/start_nav2.py @@ -14,10 +14,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # Set the path to different files and folders. - pkg_share = get_package_share_directory('dcaiti_control') - world_file_name = 'basic_mobile_bot_world/smalltown.world' - world_path = os.path.join(pkg_share, 'worlds', world_file_name) - static_map_path = os.path.join(pkg_share, 'maps', 'smalltown_world.yaml') + pkg_share = get_package_share_directory('dcaiti_navigation') nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml') behavior_tree_xml_path = os.path.join(get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml') @@ -60,65 +57,22 @@ def generate_launch_description(): name='default_bt_xml_filename', default_value=behavior_tree_xml_path, description='Full path to the behavior tree xml file to use') - - declare_map_yaml_cmd = DeclareLaunchArgument( - name='map', - default_value=static_map_path, - description='Full path to map file to load') declare_params_file_cmd = DeclareLaunchArgument( name='params_file', default_value=nav2_params_path, description='Full path to the ROS2 parameters file to use for all launched nodes') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient') - - declare_slam_cmd = DeclareLaunchArgument( - name='slam', - default_value='False', - description='Whether to run SLAM') - - declare_use_robot_state_pub_cmd = DeclareLaunchArgument( - name='use_robot_state_pub', - default_value='True', - description='Whether to start the robot state publisher') - - declare_use_rviz_cmd = DeclareLaunchArgument( - name='use_rviz', - default_value='True', - description='Whether to start RVIZ') declare_use_sim_time_cmd = DeclareLaunchArgument( name='use_sim_time', default_value='True', description='Use simulation (Gazebo) clock if true') - - declare_use_simulator_cmd = DeclareLaunchArgument( - name='use_simulator', - default_value='True', - description='Whether to start the simulator') - - declare_world_cmd = DeclareLaunchArgument( - name='world', - default_value=world_path, - description='Full path to the world model file to load') # Specify the actions # Launch the ROS 2 Navigation Stack start_ros2_navigation_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','bringup_launch.py'), - launch_arguments = {'namespace': namespace, - 'use_namespace': use_namespace, - 'slam': slam, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'default_bt_xml_filename': default_bt_xml_filename, - 'autostart': autostart}.items()) + PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','navigation_launch.py')) # Create the launch description and populate ld = LaunchDescription() @@ -128,15 +82,8 @@ def generate_launch_description(): ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_bt_xml_cmd) - ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_slam_cmd) - ld.add_action(declare_use_robot_state_pub_cmd) - ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) # Add any actions ld.add_action(start_ros2_navigation_cmd) diff --git a/src/dcaiti_navigation/params/nav2_params.yaml b/src/dcaiti_navigation/params/nav2_params.yaml index ea44dea..b138e4c 100644 --- a/src/dcaiti_navigation/params/nav2_params.yaml +++ b/src/dcaiti_navigation/params/nav2_params.yaml @@ -188,7 +188,7 @@ local_costmap: width: 10 height: 10 resolution: 0.05 - robot_radius: 0.75 + footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]" plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" @@ -232,7 +232,7 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: True - robot_radius: 0.75 + robot_radius: 0.21 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] @@ -339,12 +339,10 @@ slam_toolbox: ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None - # ROS Parameters - odom_frame: odom - map_frame: map base_frame: base_link + map_update_interval: 1.0 scan_topic: /scan - mode: mapping #localization + odom_topic: /diff_cont/odom # 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 diff --git a/src/dcaitirobot/config/slam.yml b/src/dcaitirobot/config/slam.yml index f62f36b..f6f8f52 100644 --- a/src/dcaitirobot/config/slam.yml +++ b/src/dcaitirobot/config/slam.yml @@ -2,5 +2,5 @@ slam_toolbox: ros__parameters: base_frame: base_link map_update_interval: 1.0 - scan_topic: /scan_no_nan - odom_topic: /odom + scan_topic: /scan + odom_topic: /diff_cont/odom diff --git a/src/dcaitirobot/launch/launch_slam.py b/src/dcaitirobot/launch/launch_slam.py index a52512c..df41bbf 100644 --- a/src/dcaitirobot/launch/launch_slam.py +++ b/src/dcaitirobot/launch/launch_slam.py @@ -24,27 +24,6 @@ def generate_launch_description(): 'slam_params_file': slam_params}.items() ) - node_fix_lidar = Node( - package="dcaitirobot", - executable="fixlidar", - name="fixlidar", - ) - - node_scale_odom = Node( - package="dcaitirobot", - executable="scaleodom", - name="scaleodom", - ) - - tfs1 = Node(package = "tf2_ros", - executable = "static_transform_publisher", - arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"]) - - tfs2 = Node(package = "tf2_ros", - executable = "static_transform_publisher", - arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"]) - - return LaunchDescription([ - slam_toolbox, tfs1, tfs2, node_fix_lidar, node_scale_odom + slam_toolbox ]) \ No newline at end of file