diff --git a/toid_navigation/launch/mock_control_launch.py b/toid_navigation/launch/mock_control_launch.py deleted file mode 100644 index 5d4ccfd..0000000 --- a/toid_navigation/launch/mock_control_launch.py +++ /dev/null @@ -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 - ]) \ No newline at end of file diff --git a/toid_navigation/launch/navigation_launch.py b/toid_navigation/launch/navigation_launch.py deleted file mode 100644 index 8193379..0000000 --- a/toid_navigation/launch/navigation_launch.py +++ /dev/null @@ -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 - ]) \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 54dd3bf..fc0776b 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/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 @@ -194,38 +188,4 @@ controller_server: 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 \ No newline at end of file + node_names: ['controller_server','planner_server','behavior_server','map_server','bt_navigator' ] \ No newline at end of file