diff --git a/tortoisebot_bringup/launch/autobringup_ign.launch.py b/tortoisebot_bringup/launch/autobringup_ign.launch.py new file mode 100644 index 0000000..ec0922d --- /dev/null +++ b/tortoisebot_bringup/launch/autobringup_ign.launch.py @@ -0,0 +1,150 @@ +import os +import launch +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, PythonExpression,Command +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable,IncludeLaunchDescription +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition +import launch_ros +from launch_ros.descriptions import ParameterValue + +def generate_launch_description(): + pkg_share = launch_ros.substitutions.FindPackageShare(package='tortoisebot_description').find('tortoisebot_description') + navigation_dir = os.path.join(get_package_share_directory('tortoisebot_navigation'), 'launch') + rviz_launch_dir=os.path.join(get_package_share_directory('tortoisebot_description'), 'launch') + gazebo_launch_dir=os.path.join(get_package_share_directory('tortoisebot_gazebo'), 'launch') + ydlidar_launch_dir=os.path.join(get_package_share_directory('ydlidar_ros2_driver'), 'launch') + camera_launch_dir=os.path.join(get_package_share_directory('v4l2_camera'), 'launch') + cartographer_launch_dir=os.path.join(get_package_share_directory('tortoisebot_slam'), 'launch') + prefix_address = get_package_share_directory('tortoisebot_navigation') + default_model_path = os.path.join(pkg_share, 'models/urdf/tortoisebot_ign.xacro') + default_rviz_config_path = os.path.join(get_package_share_directory('tortoisebot_description'), 'rviz/tortoisebot_sensor_display.rviz') + + + params_file_sim = os.path.join(prefix_address, 'config', 'nav2_params_simulation.yaml') + params_file_robot = os.path.join(prefix_address, 'config', 'nav2_params_robot.yaml') + + map_file=LaunchConfiguration('map') + map_directory = os.path.join(get_package_share_directory( + 'tortoisebot_bringup'), 'maps','second.yaml') + use_sim_time=LaunchConfiguration('use_sim_time') + exploration=LaunchConfiguration('exploration') + + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', LaunchConfiguration('rvizconfig')], + parameters= [{'use_sim_time': use_sim_time}], + + ) + + state_publisher_launch_cmd=IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(rviz_launch_dir, 'state_publisher_ign.launch.py')), + launch_arguments={'use_sim_time':use_sim_time}.items()) + + gazebo_launch_cmd=IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(gazebo_launch_dir, 'gazebo_ign.launch.py')), + condition=IfCondition(use_sim_time), + launch_arguments={'use_sim_time':use_sim_time}.items()) + + navigation_launch_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(navigation_dir, 'navigation.launch.py')), + launch_arguments={'params_file': params_file_robot}.items()) + + cartographer_launch_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(cartographer_launch_dir, 'cartographer.launch.py')), + launch_arguments={'params_file': params_file_robot, + 'exploration': exploration, + 'use_sim_time': use_sim_time}.items()) + + ydlidar_launch_cmd=IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ydlidar_launch_dir, 'ydlidar_launch.py')), + condition=IfCondition(PythonExpression(['not ', use_sim_time])), + launch_arguments={'use_sim_time':use_sim_time}.items()) + + differential_drive_node = Node( + package='tortoisebot_firmware', + condition=IfCondition(PythonExpression(['not ', use_sim_time])), + executable='differential.py', + name ='differential_drive_publisher', + ) + camera_drive_node = Node( + package='v4l2_camera', + condition=IfCondition(PythonExpression(['not ', use_sim_time])), + executable='v4l2_camera_node', + name ='camera_publisher', + ) + camera_node = Node( + package='camera_ros', + condition=IfCondition(PythonExpression(['not ', use_sim_time])), + executable='camera_node', + name ='pi_camera', + parameters= [{'height': 360},{'width': 480}], + ) + robot_state_publisher_node = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'use_sim_time': use_sim_time},{'robot_description': ParameterValue(Command(['xacro ', LaunchConfiguration('model')]),value_type=str)}] + ) + + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters= [{'use_sim_time': use_sim_time}], + ) + return LaunchDescription([ + + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='False', + description='Flag to enable use_sim_time'), + launch.actions.DeclareLaunchArgument(name='exploration', default_value='True', + description='Flag to enable use_sim_time'), + launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path, + description='Absolute path to robot urdf file'), + launch.actions.DeclareLaunchArgument(name='map',default_value=map_directory, + description='Map to be used'), + launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, + description='Absolute path to rviz config file'), + Node( + package='nav2_map_server', + condition=IfCondition(PythonExpression(['not ', exploration])), + executable='map_server', + name='map_server', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'yaml_filename': map_file} + ]), + Node( + package='nav2_lifecycle_manager', + condition=IfCondition(PythonExpression(['not ', exploration])), + executable='lifecycle_manager', + name='lifecycle_manager_mapper', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': True}, + {'node_names': ['map_server']}]), + + rviz_node, + state_publisher_launch_cmd, + robot_state_publisher_node, + joint_state_publisher_node, + ydlidar_launch_cmd, + differential_drive_node, + camera_drive_node, + gazebo_launch_cmd, + navigation_launch_cmd, + cartographer_launch_cmd + + ] +) + diff --git a/tortoisebot_bringup/maps/second.pgm b/tortoisebot_bringup/maps/second.pgm new file mode 100644 index 0000000..a7f7d80 Binary files /dev/null and b/tortoisebot_bringup/maps/second.pgm differ diff --git a/tortoisebot_bringup/maps/second.yaml b/tortoisebot_bringup/maps/second.yaml new file mode 100644 index 0000000..1ea2f40 --- /dev/null +++ b/tortoisebot_bringup/maps/second.yaml @@ -0,0 +1,7 @@ +image: second.pgm +mode: trinary +resolution: 0.05 +origin: [-2.25, -2.27, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/tortoisebot_description/launch/state_publisher_ign.launch.py b/tortoisebot_description/launch/state_publisher_ign.launch.py new file mode 100644 index 0000000..4afbb28 --- /dev/null +++ b/tortoisebot_description/launch/state_publisher_ign.launch.py @@ -0,0 +1,58 @@ +from ament_index_python import get_package_share_directory +import launch +import os +from launch.substitutions import Command, LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import PythonExpression +import launch_ros +from launch_ros.descriptions import ParameterValue + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') + default_model_path = os.path.join(get_package_share_directory('tortoisebot_description'), 'models/urdf/tortoisebot_ign.xacro') + real_robot_model_path = os.path.join(get_package_share_directory('tortoisebot_description'), 'models/urdf/tortoisebotreal.xacro') + + # Robot state publisher for simulation (when use_sim_time is True) + robot_state_publisher_sim = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{ + 'use_sim_time': use_sim_time, + 'robot_description': ParameterValue(Command(['xacro ', LaunchConfiguration('model')]), value_type=str) + }], + condition=IfCondition(use_sim_time) + ) + + # Robot state publisher for real robot (when use_sim_time is False) + robot_state_publisher_real = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{ + 'use_sim_time': use_sim_time, + 'robot_description': ParameterValue(Command(['xacro ', real_robot_model_path]), value_type=str) + }], + condition=UnlessCondition(use_sim_time) + ) + + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'use_sim_time': use_sim_time}], + ) + + return launch.LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='use_sim_time', + default_value='False', + description='Flag to enable use_sim_time' + ), + launch.actions.DeclareLaunchArgument( + name='model', + default_value=default_model_path, + description='Absolute path to robot urdf file (used only when use_sim_time is True)' + ), + robot_state_publisher_sim, + robot_state_publisher_real, + joint_state_publisher_node + ]) \ No newline at end of file diff --git a/tortoisebot_description/models/urdf/tortoisebot_ign.xacro b/tortoisebot_description/models/urdf/tortoisebot_ign.xacro new file mode 100644 index 0000000..fd3d478 --- /dev/null +++ b/tortoisebot_description/models/urdf/tortoisebot_ign.xacro @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tortoisebot_gazebo/CMakeLists.txt b/tortoisebot_gazebo/CMakeLists.txt index 8f6e0d4..13ce709 100644 --- a/tortoisebot_gazebo/CMakeLists.txt +++ b/tortoisebot_gazebo/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) install( - DIRECTORY gazebo launch worlds + DIRECTORY gazebo launch worlds config DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) diff --git a/tortoisebot_gazebo/config/gz_bridge.yaml b/tortoisebot_gazebo/config/gz_bridge.yaml new file mode 100644 index 0000000..edac429 --- /dev/null +++ b/tortoisebot_gazebo/config/gz_bridge.yaml @@ -0,0 +1,35 @@ +- ros_topic_name: "clock" + gz_topic_name: "clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS + +- ros_topic_name: "scan" + gz_topic_name: "scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS + +- ros_topic_name: "camera/camera_info" + gz_topic_name: "camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS + +- ros_topic_name: "tf" + gz_topic_name: "tf" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS + +- ros_topic_name: "cmd_vel" + gz_topic_name: "cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ + +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS diff --git a/tortoisebot_gazebo/gazebo/tortoisebot_plugins_ign.gazebo b/tortoisebot_gazebo/gazebo/tortoisebot_plugins_ign.gazebo new file mode 100644 index 0000000..257b22a --- /dev/null +++ b/tortoisebot_gazebo/gazebo/tortoisebot_plugins_ign.gazebo @@ -0,0 +1,88 @@ + + + + + + + + + + wheel_left_joint + wheel_right_joint + + + 0.17186 + 0.033 + + + + 0.20 + + cmd_vel + + + + + + + + + + + + + + false + 10 + + + + 360 + -3.14 + 3.14 + + + + 0.3 + 12 + + + scan + lidar + + + + + + + + + + true + 10 + + camera/camera_info + 1.089 + + R8G8B8 + 640 + 480 + + + 0.05 + 8.0 + + + camera/image_raw + camera_link + + + + + diff --git a/tortoisebot_gazebo/launch/gazebo_ign.launch.py b/tortoisebot_gazebo/launch/gazebo_ign.launch.py new file mode 100644 index 0000000..5c2ceef --- /dev/null +++ b/tortoisebot_gazebo/launch/gazebo_ign.launch.py @@ -0,0 +1,71 @@ +from ament_index_python import get_package_share_directory +import launch +from launch.substitutions import Command, LaunchConfiguration +import launch_ros +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch_ros.actions import Node +import os + + +def generate_launch_description(): + # world_path= os.path.join(get_package_share_directory('ttb_description'), 'models/worlds/house_env.world'), + package_name='tortoisebot_gazebo' + world_path=os.path.join(get_package_share_directory('tortoisebot_gazebo'), 'worlds/second.sdf'), + world = LaunchConfiguration('world') + + + + default_world = os.path.join( + get_package_share_directory(package_name), + 'worlds', + 'empty.world' + ) + + world_arg = DeclareLaunchArgument( + 'world', + default_value=world_path , + description='World to load' + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]), + launch_arguments={'gz_args': ['-r -v4 ', world], 'on_exit_shutdown': 'true'}.items() + ) + + # Run the spawner node from the ros_gz_sim package. The entity name doesn't really matter if you only have a single robot. + spawn_entity = Node(package='ros_gz_sim', executable='create', + arguments=['-topic', 'robot_description', + '-name', 'my_bot', + '-z', '0.1'], + output='screen') + + + bridge_params = os.path.join(get_package_share_directory(package_name),'config','gz_bridge.yaml') + ros_gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + '--ros-args', + '-p', + f'config_file:={bridge_params}', + ] + ) + + ros_gz_image_bridge = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/camera/image_raw"] + ) + + use_sim_time = LaunchConfiguration('use_sim_time') + + return launch.LaunchDescription([ + world_arg, + gazebo, + spawn_entity, + ros_gz_bridge, + ros_gz_image_bridge + ]) + \ No newline at end of file diff --git a/tortoisebot_gazebo/worlds/second.sdf b/tortoisebot_gazebo/worlds/second.sdf new file mode 100644 index 0000000..9f7534d --- /dev/null +++ b/tortoisebot_gazebo/worlds/second.sdf @@ -0,0 +1,415 @@ + + + + 0.001 + 1 + 1000 + + + + + + ogre2 + + + 0 0 -9.8000000000000007 + 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 + + + 0.400000006 0.400000006 0.400000006 1 + 0.699999988 0.699999988 0.699999988 1 + true + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.800000012 0.800000012 0.800000012 1 + 0.800000012 0.800000012 0.800000012 1 + 0.800000012 0.800000012 0.800000012 1 + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 0 0 + false + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + true + true + 3.0021046306236707 0.51356120539538497 0 0 0 0 + false + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + true + true + 0.50742614269256592 -1.961226224899292 0 0 0 -1.5849199372922735 + false + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + true + true + 0.48567593097686768 3.0232474803924561 0 0 0 -1.5941000150300249 + false + + + + + + + 0.050000000000000003 1.45 0.82999999999999996 + + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + true + true + 2.883137924324064 0.52975129719305913 0 0 0 0 + false + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + 0.10000000000000001 5 2 + + + + + + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + true + true + -1.9785492973417345 0.53221742781935566 0 0 0 0 + false + + + + + 0 0 0.94999999999999996 0 0 0 + 80 + + 27.82 + 0 + 0 + 24.879999999999999 + 0 + 4.5700000000000003 + + + + 0 0 0.01 0 0 0 + + + 0.34999999999999998 0.75 0.02 + + + + + + + + + + + + 0 0 -0.02 0 0 0 + + + https://fuel.gazebosim.org/1.0/gazebomstrling/models/walking person/1/files/meshes/walking.dae + + + + + 0 0 -0.02 0 0 0 + + + https://fuel.gazebosim.org/1.0/gazebomstrling/models/walking person/1/files/meshes/walking.dae + + + + 0 0 0 0 0 0 + false + + 1.2797878457102103 1.412421202326092 -7.3271239603434067e-11 1.0720906839495214e-21 -1.3292261019567409e-22 3.2859880983158194e-22 + false + false + + + true + + 0 0 0 0 0 0 + + + + https://fuel.gazebosim.org/1.0/will0993/models/woodenchair/1/files/meshes/WoodenChair.obj + + + + + + + https://fuel.gazebosim.org/1.0/will0993/models/woodenchair/1/files/meshes/WoodenChair_Col.obj + + + + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + -0.60016833108321821 1.5965244603890154 0 0 0 0 + false + + + 0 0 10 0 0 0 + true + 1 + -0.5 0.10000000000000001 -0.90000000000000002 + 0.800000012 0.800000012 0.800000012 1 + 0.200000003 0.200000003 0.200000003 1 + + 1000 + 0.01 + 0.90000000000000002 + 0.001 + + + 0 + 0 + 0 + + + + diff --git a/tortoisebot_gazebo/worlds/test.sdf b/tortoisebot_gazebo/worlds/test.sdf new file mode 100644 index 0000000..4f08432 --- /dev/null +++ b/tortoisebot_gazebo/worlds/test.sdf @@ -0,0 +1,89 @@ + + + + 0.001 + 1 + 1000 + + + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 -0 0 + false + + + 0 0 10 0 -0 0 + true + 1 + -0.5 0.1 -0.9 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.01 + 0.90000000000000002 + 0.001 + + + 0 + 0 + 0 + + + +