toid_navigation/launch/mock_control_launch.py

@@ -1,93 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition, UnlessCondition 
-from launch.substitutions import Command, LaunchConfiguration
-from launch_ros.actions import Node, LifecycleNode
-from launch_ros.substitutions import FindPackageShare
-import os
-
-def generate_launch_description():
-    pkg_share = FindPackageShare("").find('toid_navigation')
-    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
-    map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
-    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz')
-    
-
-    description_pkg_share = FindPackageShare("").find('toid_bot_description')
-    default_model_path = os.path.join(
-        description_pkg_share, 
-        'src', 
-        'toid_bot_description.urdf'
-    )
-
-    visualize = LaunchConfiguration("visualize")
-
-    visualize_arg = DeclareLaunchArgument(
-        'visualize',
-        default_value='True',
-        description="Whether to launch rviz2" 
-    )
-
-    odom_broadcast = Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        name='map_to_odom_broadcaster',
-        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
-        condition=IfCondition(LaunchConfiguration('visualize'))
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='screen',
-        parameters=[{'robot_description': Command(['xacro ', default_model_path])}]
-    )
-
-    controller_manager = Node(
-        package='controller_manager',
-        executable='ros2_control_node',
-        output='screen',
-        parameters=[params]
-    )
-
-    joint_state_broadcaster = Node(
-        package='controller_manager',
-        executable='spawner',
-        output='screen',
-        arguments=["joint_state_broadcaster"]
-    )
-
-    diffbot_base_controller = Node(
-        package='controller_manager',
-        executable='spawner',
-        output='both',
-        arguments=[
-            "diffbot_base_controller",
-            "-p",
-            params,
-            "--controller-ros-args",
-            "-r diffbot_base_controller/cmd_vel:=/cmd_vel",
-            "--controller-ros-args",
-            "-r diffbot_base_controller/odom:=/odom"
-        ]
-    )
-
-    rviz_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        output='screen',
-        arguments=['-d', default_rviz_config_path],
-        condition=IfCondition(visualize)
-    )
-
-    return LaunchDescription([
-        visualize_arg,
-        odom_broadcast,
-        robot_state_publisher,
-        controller_manager,
-        joint_state_broadcaster,
-        diffbot_base_controller,
-        rviz_node
-    ])

toid_navigation/launch/navigation_launch.py

@@ -1,116 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.conditions import IfCondition, UnlessCondition 
-from launch.substitutions import  LaunchConfiguration
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-import os
-
-def generate_launch_description():
-    pkg_share = FindPackageShare("").find('toid_navigation')
-    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
-    map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
-    launch_dir = os.path.join(pkg_share, 'launch')
-    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz')
-    bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
-    lattice_path = os.path.join(pkg_share, "params", "output.json")
-
-    position = "0.756 0.225 0 0 0 0".split(' ')
-    #position = "0 0 0 0 0 0".split(' ')
-    
-    visualize = LaunchConfiguration("visualize")
-    visualize_arg = DeclareLaunchArgument(
-        'visualize',
-        default_value='True',
-        description="Whether to launch rviz2" 
-    )
-    
-    toid_control = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_dir, 'mock_control_launch.py')
-        ),
-        launch_arguments={
-            'visualize': 'False'
-        }.items()
-    )
-
-    set_position = Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        name='map_to_odom_broadcaster',
-        arguments=position + ['map', 'odom'],
-        condition=IfCondition(visualize)
-    )
-
-    map_server = Node(
-        package='nav2_map_server',
-        executable='map_server',
-        name='map_server',
-        output='screen',
-        parameters=[{'yaml_filename': map}]
-    )
-
-    planner_server = Node(
-        package='nav2_planner',
-        executable='planner_server',
-        name='planner_server',
-        output='screen',
-        parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params]
-    )
-
-    controller_server = Node(
-        package='nav2_controller',
-        executable='controller_server',
-        name='controller_server',
-        output='screen',
-        parameters=[params]
-    )
-
-    bt_navigator = Node(
-        package='nav2_bt_navigator',
-        executable='bt_navigator',
-        name='bt_navigator',
-        output='screen',
-        parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params]
-    )
-
-# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml"
-
-    behavior_server = Node(
-        package='nav2_behaviors',
-        executable='behavior_server',
-        name='behavior_server',
-        output='screen',
-        parameters=[params]
-    )
-
-    lifecycle_manager_node = Node(
-        package='nav2_lifecycle_manager',
-        executable='lifecycle_manager',
-        name='lifecycle_manager',
-        output='screen',
-        parameters=[params]
-    )
-
-    rviz_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        output='screen',
-        arguments=['-d', default_rviz_config_path],
-        condition=IfCondition(visualize)
-    )
-
-    return LaunchDescription([
-        visualize_arg,
-        set_position,
-        rviz_node,
-        map_server,
-        bt_navigator,
-        behavior_server,
-        planner_server,
-        controller_server,
-        lifecycle_manager_node,
-        toid_control
-    ])

toid_navigation/params/toid_general_params.yaml

@@ -22,17 +22,11 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 50.0
-    behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX", "moveCoords"]
+    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords"]
     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"
     rotate:
       plugin: "toid::SimpleRotateBehavior"
       max_angular_accel: 4.0
@@ -195,37 +189,3 @@ lifecycle_manager:
   ros__parameters:
     autostart: true
     node_names: ['controller_server','planner_server','behavior_server','map_server','bt_navigator' ]
-
-
-controller_manager:
-  ros__parameters:
-    update_rate: 20  # Hz
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
-diffbot_base_controller:
-  ros__parameters:
-    type: diff_drive_controller/DiffDriveController
-
-    left_wheel_names: ["drivewhl_l_joint"]
-    right_wheel_names: ["drivewhl_r_joint"]
-
-    wheel_separation: 0.29
-
-    wheel_radius: 0.04
-    use_stamped_vel: false
-
-    wheel_separation_multiplier: 1.0
-    left_wheel_radius_multiplier: 1.0
-    right_wheel_radius_multiplier: 1.0
-
-    publish_rate: 50.0
-    odom_frame_id: odom
-    base_frame_id: base_footprint
-
-    open_loop: true
-    enable_odom_tf: true
-    publish_limited_velocity: true
-
-    cmd_vel_timeout: 1.0