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 100% rename from launch/joy_teleop.launch.py rename to launch/robot_joy_teleop.launch.py 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 100% rename from launch/mapper.launch.py rename to launch/robot_localization.launch.py 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/twist_mux.launch.py b/launch/robot_twist_mux.launch.py similarity index 100% rename from launch/twist_mux.launch.py rename to launch/robot_twist_mux.launch.py 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/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