From 27df96e4976b49d4fa2e318b6d47bddeafc47ac2 Mon Sep 17 00:00:00 2001 From: John Vial Date: Tue, 26 Sep 2023 11:36:58 +0800 Subject: [PATCH] Fixed Launch Bug --- .vscode/keybindings.json | 19 ++++- build.sh => .vscode/scripts/build.sh | 0 .vscode/scripts/day2.sh | 5 ++ .vscode/scripts/day3.sh | 6 ++ .vscode/scripts/day4.sh | 5 ++ .vscode/scripts/day5.sh | 5 ++ .vscode/tasks.json | 25 ++++++- .../config/gazebo_bridge.yaml | 32 -------- .../launch/gazebo.launch.py | 73 ++++++++----------- .../launch/mapping.launch.py | 72 ++++++++++++++---- .../launch/navigation.launch.py | 10 ++- src/start_creating_robots/setup.py | 1 + .../worlds_and_models/krytn/krytn.urdf.xacro | 25 ++++--- 13 files changed, 172 insertions(+), 106 deletions(-) rename build.sh => .vscode/scripts/build.sh (100%) create mode 100644 .vscode/scripts/day2.sh create mode 100644 .vscode/scripts/day3.sh create mode 100644 .vscode/scripts/day4.sh create mode 100644 .vscode/scripts/day5.sh delete mode 100644 src/start_creating_robots/config/gazebo_bridge.yaml diff --git a/.vscode/keybindings.json b/.vscode/keybindings.json index 0c4fef7..dc846f0 100644 --- a/.vscode/keybindings.json +++ b/.vscode/keybindings.json @@ -1,7 +1,22 @@ [ { - "key": "ctrl+shift+alt+g", + "key": "ctrl+shift+alt+2", "command": "workbench.action.tasks.runTask", - "args": "Start Gazebo" + "args": "Day 2" + }, + { + "key": "ctrl+shift+alt+3", + "command": "workbench.action.tasks.runTask", + "args": "Day 3" + }, + { + "key": "ctrl+shift+alt+4", + "command": "workbench.action.tasks.runTask", + "args": "Day 4" + }, + { + "key": "ctrl+shift+alt+5", + "command": "workbench.action.tasks.runTask", + "args": "Day 5" } ] \ No newline at end of file diff --git a/build.sh b/.vscode/scripts/build.sh similarity index 100% rename from build.sh rename to .vscode/scripts/build.sh diff --git a/.vscode/scripts/day2.sh b/.vscode/scripts/day2.sh new file mode 100644 index 0000000..1063bcb --- /dev/null +++ b/.vscode/scripts/day2.sh @@ -0,0 +1,5 @@ +#!/bin/bash +bash .vscode/scripts/build.sh + +source install/setup.bash +ign gazebo shapes.sdf \ No newline at end of file diff --git a/.vscode/scripts/day3.sh b/.vscode/scripts/day3.sh new file mode 100644 index 0000000..8879387 --- /dev/null +++ b/.vscode/scripts/day3.sh @@ -0,0 +1,6 @@ +#!/bin/bash +bash .vscode/scripts/build.sh + +source install/setup.bash +ros2 launch start_creating_robots gazebo.launch.py + diff --git a/.vscode/scripts/day4.sh b/.vscode/scripts/day4.sh new file mode 100644 index 0000000..6f221b9 --- /dev/null +++ b/.vscode/scripts/day4.sh @@ -0,0 +1,5 @@ +#!/bin/bash +bash .vscode/scripts/build.sh + +source install/setup.bash +ros2 launch start_creating_robots mapping.launch.py diff --git a/.vscode/scripts/day5.sh b/.vscode/scripts/day5.sh new file mode 100644 index 0000000..9687346 --- /dev/null +++ b/.vscode/scripts/day5.sh @@ -0,0 +1,5 @@ +#!/bin/bash +bash .vscode/scripts/build.sh + +source install/setup.bash +ros2 launch start_creating_robots navigation.launch.py diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 161531d..3a371f9 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -6,7 +6,7 @@ "label": "build", "detail": "Build workspace (default)", "type": "shell", - "command": "bash build.sh", + "command": "bash .vscode/scripts/build.sh", "group": { "kind": "build", "isDefault": true @@ -22,9 +22,28 @@ } }, { - "label": "Start Gazebo", + "label": "Day 2", + "detail": "Test that Gazebo works!", "type": "shell", - "command": "ign gazebo shapes.sdf" + "command": "bash .vscode/scripts/day2.sh" + }, + { + "label": "Day 3", + "detail": "Teleoperate a robot", + "type": "shell", + "command": "bash .vscode/scripts/day3.sh" + }, + { + "label": "Day 4", + "detail": "Create your first map", + "type": "shell", + "command": "bash .vscode/scripts/day4.sh" + }, + { + "label": "Day 5", + "detail": "Let your robot drive itself", + "type": "shell", + "command": "bash .vscode/scripts/day5.sh" } ] } \ No newline at end of file diff --git a/src/start_creating_robots/config/gazebo_bridge.yaml b/src/start_creating_robots/config/gazebo_bridge.yaml deleted file mode 100644 index 6ffcce7..0000000 --- a/src/start_creating_robots/config/gazebo_bridge.yaml +++ /dev/null @@ -1,32 +0,0 @@ -# Configuration to connect Gazebo simulation topics to ROS topics -- topic_name: "/model/magni/cmd_vel" - ros_type_name: "geometry_msgs/msg/Twist" - gz_type_name: "gz.msgs.Twist" - -- topic_name: "/model/magni/odometry" - ros_type_name: "nav_msgs/msg/Odometry" - gz_type_name: "gz.msgs.Odometry" - -- topic_name: "/world/cafe_world/model/robot/model/lidar_2d_v1/link/link/sensor/lidar_2d_v1/scan" - ros_type_name: "sensor_msgs/msg/LaserScan" - gz_type_name: "gz.msgs.LaserScan" - -- topic_name: "/world/cafe_world/model/robot/model/realsense_d435/link/sensor/realsense_d435/image" - ros_type_name: "sensor_msgs/msg/Image" - gz_type_name: "gz.msgs.Image" - -- topic_name: "/world/cafe_world/model/robot/model/realsense_d435/link/sensor/realsense_d435/depth" - ros_type_name: "sensor_msgs/msg/Image" - gz_type_name: "gz.msgs.Image" - -- topic_name: "/world/cafe_world/model/robot/model/realsense_d435/link/sensor/realsense_d435/points" - ros_type_name: "sensor_msgs/msg/PointCloud2" - gz_type_name: "gz.msgs.PointCloudPacked" - -- topic_name: "/clock" - ros_type_name: "rosgraph_msgs/msg/Clock" - gz_type_name: "gz.msgs.Clock" - -- topic_name: "/model/robot/model/magni/pose" - ros_type_name: "tf2_msgs/msg/TFMessage" - gz_type_name: "gz.msgs.Pose_V" diff --git a/src/start_creating_robots/launch/gazebo.launch.py b/src/start_creating_robots/launch/gazebo.launch.py index 6fa6a12..8d48f3f 100644 --- a/src/start_creating_robots/launch/gazebo.launch.py +++ b/src/start_creating_robots/launch/gazebo.launch.py @@ -1,81 +1,72 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.actions import IncludeLaunchDescription, ExecuteProcess, DeclareLaunchArgument from launch.launch_description_sources import get_launch_description_from_python_launch_file from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory from os.path import join +from launch.substitutions import LaunchConfiguration, Command import xacro def generate_launch_description(): + # This allows us to have the with_sensors as an argument on the command line + with_sensors_arg = DeclareLaunchArgument( + 'with_sensors', default_value="false" + ) + # This allows us to use the with_sensors variable in substitutions in this launch description. + with_sensors = LaunchConfiguration('with_sensors', default="false") + + models_path = join(get_package_share_directory("start_creating_robots"), "worlds_and_models") + + # Start a simulation with the cafe world cafe_world_uri = join(models_path,"cafe.sdf") path = join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py") + + gazebo_sim = IncludeLaunchDescription(path, + launch_arguments=[("gz_args", '-r ' + cafe_world_uri)]) + + # Create a robot in the world. + # Steps: + # 1. Process a file using the xacro tool to get an xml file containing the robot description. + # 2. Publish this robot description using a ros topic so all nodes can know about the joints of the robot. + # 3. Spawn a simulated robot in the gazebo simulation using the published robot description topic. + # Step 1. Process robot file. robot_file = join(models_path, "krytn","krytn.urdf.xacro") - robot_description_config = xacro.process_file(robot_file) - robot_description = {'robot_description': robot_description_config.toxml()} - - # Robot state publisher + robot_xml = Command(["xacro ",robot_file," with_sensors:=",with_sensors]) + + #Step 2. Publish robot file to ros topic /robot_description robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='both', - parameters=[robot_description], + parameters=[{'robot_description':robot_xml}], ) - gazebo_sim = IncludeLaunchDescription(path, - launch_arguments=[("gz_args", '-r ' + cafe_world_uri)]) - - + # Step 3. Spawn a robot in gazebo by listening to the published topic. robot = ExecuteProcess( cmd=["ros2", "run", "ros_gz_sim", "create", "-topic", "robot_description", "-z", "0.5"], name="spawn robot", output="both" ) - - # Gazebo Bridge: This allows communication from the Gazebo world to the ROS system. - + # Gazebo Bridge: This allows ROS to send messages to drive the robot in simulation. bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', - arguments=['/model/krytn/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist', - '/model/krytn/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry', - '/model/krytn/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', - '/lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan', - '/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked', - '/realsense/image@sensor_msgs/msg/Image[gz.msgs.Image', - '/realsense/depth@sensor_msgs/msg/Image[gz.msgs.Image', - '/realsense/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked', - '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', - '/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model'], + arguments=['/model/krytn/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist'], output='screen', - remappings=[('/model/krytn/odometry','/odom'), - ('/model/krytn/tf','/tf'), - ('/model/krytn/cmd_vel','/cmd_vel')] + remappings=[('/model/krytn/cmd_vel','/cmd_vel')] ) - # Gazebo fortress has a bug that won't respect our frame_id tags. So we have to publish a transform - depth_cam_link_tf = Node(package='tf2_ros', - executable='static_transform_publisher', - name='depthCamLinkTF', - output='log', - arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'realsense_d435', 'krytn/base_footprint/realsense_d435']) - - krytn_base_fp_link_tf = Node(package='tf2_ros', - executable='static_transform_publisher', - name='base_fp_linkTF', - output='log', - arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'krytn/base_footprint', 'base_footprint']) - - + # A gui tool for easy tele-operation. robot_steering = Node( package="rqt_robot_steering", executable="rqt_robot_steering", ) - return LaunchDescription([gazebo_sim, bridge, robot, robot_steering, robot_state_publisher, depth_cam_link_tf, krytn_base_fp_link_tf]) \ No newline at end of file + return LaunchDescription([gazebo_sim, bridge, robot, robot_steering, robot_state_publisher, with_sensors_arg]) \ No newline at end of file diff --git a/src/start_creating_robots/launch/mapping.launch.py b/src/start_creating_robots/launch/mapping.launch.py index ade1d3f..d2a2d31 100644 --- a/src/start_creating_robots/launch/mapping.launch.py +++ b/src/start_creating_robots/launch/mapping.launch.py @@ -1,28 +1,68 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, 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.actions import DeclareLaunchArgument -from launch.substitutions import PathJoinSubstitution, TextSubstitution -from launch.substitutions import LaunchConfiguration -import os +from os.path import join def generate_launch_description(): - # ... your existing nodes ... + # This allows us to have the with_sensors as an argument on the command line + rviz_config_arg = DeclareLaunchArgument( + 'rviz_config', default_value="mapping.yaml" + ) + # This allows us to use the with_sensors variable in substitutions in this launch description. + rviz_config = LaunchConfiguration('rviz_config', default="vis.rviz") + + base_path = get_package_share_directory("start_creating_robots") + + # We will include everything from the gazebo launch file, making sure that sensors are now enabled however. + gazebo = IncludeLaunchDescription(join(base_path, "launch","gazebo.launch.py"), + launch_arguments=[("with_sensors","true")]) + + # Extended Gazebo Bridge: To do mapping requires alot more info from the simulation. We need sensor data and estimates of the robot joint positions. + extended_bridge = Node( package='ros_gz_bridge', name="extended_gazebo_bridge", executable='parameter_bridge', arguments=['/model/krytn/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry', + '/model/krytn/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', + + '/lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan', + '/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked', + + '/realsense/image@sensor_msgs/msg/Image[gz.msgs.Image', + '/realsense/depth@sensor_msgs/msg/Image[gz.msgs.Image', + '/realsense/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked', + + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', + + '/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model'], output='screen', remappings=[('/model/krytn/odometry','/odom'), + ('/model/krytn/tf','/tf')] + ) + + # Gazebo fortress has a bug that won't respect our frame_id tags. So we have to publish a transform + depth_cam_link_tf = Node(package='tf2_ros', executable='static_transform_publisher', name='depthCamLinkTF', output='log', arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'realsense_d435', 'krytn/base_footprint/realsense_d435']) + + krytn_base_fp_link_tf = Node(package='tf2_ros', executable='static_transform_publisher', name='base_fp_linkTF', output='log', arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'krytn/base_footprint', 'base_footprint']) # SLAM Toolbox for mapping - slam_toolbox = Node( - package='slam_toolbox', - executable='async_slam_toolbox_node', - parameters=[ + slam_toolbox = Node( package='slam_toolbox', executable='async_slam_toolbox_node', parameters=[ get_package_share_directory('start_creating_robots') + '/config/mapping.yaml' - ], - output='screen' + ], output='screen' + ) + + rviz = Node( + package='rviz2', + executable='rviz2', + arguments=[ + '-d', + PathJoinSubstitution([base_path, 'config', rviz_config]) + ] ) - return LaunchDescription([ - slam_toolbox - ]) + return LaunchDescription([gazebo, + extended_bridge, + depth_cam_link_tf, + krytn_base_fp_link_tf, + slam_toolbox, + rviz, + rviz_config_arg]) \ No newline at end of file diff --git a/src/start_creating_robots/launch/navigation.launch.py b/src/start_creating_robots/launch/navigation.launch.py index d933132..e61207a 100644 --- a/src/start_creating_robots/launch/navigation.launch.py +++ b/src/start_creating_robots/launch/navigation.launch.py @@ -9,10 +9,16 @@ from ament_index_python.packages import get_package_share_directory from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource - +from launch.actions import IncludeLaunchDescription +from os.path import join def generate_launch_description(): + base_path = get_package_share_directory("start_creating_robots") + + # We will include everything from the mapping launch file, making sure that sensors are now enabled and setting up RVIZ for navigation. + gazebo_and_mapping = IncludeLaunchDescription(join(base_path, "launch","mapping.launch.py"), + launch_arguments=[("with_sensors","true"), ("rviz_config","navigation.yaml")]) # Nav2 bringup for navigation navigation = IncludeLaunchDescription( @@ -25,5 +31,5 @@ def generate_launch_description(): ) return LaunchDescription([ - navigation + gazebo_and_mapping, navigation ]) \ No newline at end of file diff --git a/src/start_creating_robots/setup.py b/src/start_creating_robots/setup.py index 42b1ee8..b2687d1 100644 --- a/src/start_creating_robots/setup.py +++ b/src/start_creating_robots/setup.py @@ -16,6 +16,7 @@ (join('share', package_name, 'launch'), glob('launch/*launch.py')), (join('share', package_name, 'config'), glob('config/*.yaml')), + (join('share', package_name, 'config'), glob('config/*.rviz')), (join('share', package_name, 'worlds_and_models'), glob('worlds_and_models/*.*')), diff --git a/src/start_creating_robots/worlds_and_models/krytn/krytn.urdf.xacro b/src/start_creating_robots/worlds_and_models/krytn/krytn.urdf.xacro index 393b0ee..0ed11fc 100644 --- a/src/start_creating_robots/worlds_and_models/krytn/krytn.urdf.xacro +++ b/src/start_creating_robots/worlds_and_models/krytn/krytn.urdf.xacro @@ -1,24 +1,29 @@ + + - - - - + + + + + + - - ogre2 - - + + + ogre2 + +