diff --git a/sam_bot_description/config/bridge_config.yaml b/sam_bot_description/config/bridge_config.yaml new file mode 100644 index 0000000..d539047 --- /dev/null +++ b/sam_bot_description/config/bridge_config.yaml @@ -0,0 +1,57 @@ +--- +- 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: "/demo/imu" + gz_topic_name: "/demo/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + +# Topic published by DiffDrive plugin +- ros_topic_name: "/demo/odom" + gz_topic_name: "/demo/odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS + +# Topic published by JointStatePublisher plugin +- 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 + +# Topic subscribed to by DiffDrive plugin +- ros_topic_name: "/demo/cmd_vel" + gz_topic_name: "/demo/cmd_vel" + ros_type_name: "geometry_msgs/msg/TwistStamped" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ + +- 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: "/scan/points" + gz_topic_name: "/scan/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "gz.msgs.PointCloudPacked" + direction: GZ_TO_ROS + +- ros_topic_name: "/depth_camera/camera_info" + gz_topic_name: "/depth_camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS + +- ros_topic_name: "/depth_camera/points" + gz_topic_name: "/depth_camera/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "gz.msgs.PointCloudPacked" + direction: GZ_TO_ROS diff --git a/sam_bot_description/config/ekf.yaml b/sam_bot_description/config/ekf.yaml index 89e7b2c..1df05e2 100644 --- a/sam_bot_description/config/ekf.yaml +++ b/sam_bot_description/config/ekf.yaml @@ -1,5 +1,5 @@ ### ekf config file ### -ekf_filter_node: +ekf_node: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin # computation until it receives at least one message from one of the inputs. It will then run continuously at the @@ -33,15 +33,15 @@ ekf_filter_node: world_frame: odom # Defaults to the value of odom_frame if unspecified odom0: demo/odom - odom0_config: [true, true, true, - false, false, false, + odom0_config: [false, false, false, false, false, false, + true, true, false, false, false, true, false, false, false] imu0: demo/imu imu0_config: [false, false, false, - true, true, true, false, false, false, false, false, false, + false, false, true, false, false, false] diff --git a/sam_bot_description/config/nav2_params.yaml b/sam_bot_description/config/nav2_params.yaml index 1d3c6e4..d4141f2 100644 --- a/sam_bot_description/config/nav2_params.yaml +++ b/sam_bot_description/config/nav2_params.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -26,7 +25,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -39,78 +38,47 @@ amcl: z_short: 0.05 scan_topic: scan -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True - bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /demo/odom bt_loop_duration: 10 default_server_timeout: 20 - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node -bt_navigator_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] odom_topic: /demo/odom + use_realtime_priority: false + enable_stamped_cmd_vel: true # Progress checker parameters progress_checker: @@ -128,61 +96,94 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: - [ - "RotateToGoal", - "Oscillation", - "BaseObstacle", - "GoalAlign", - "PathAlign", - "PathDist", - "GoalDist", - ] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -191,7 +192,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -213,7 +213,13 @@ local_costmap: mark_threshold: 0 observation_sources: scan scan: - topic: /scan + # A relative topic will be appended to the parent of the local_costmap namespace. + # For example: + # * User chosen namespace is `tb4`. + # * User chosen topic is `scan`. + # * Topic will be remapped to `/tb4/scan` without `local_costmap`. + # * Use global topic `/scan` if you do not wish the node namespace to apply + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -223,14 +229,9 @@ local_costmap: obstacle_max_range: 2.5 obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True global_costmap: global_costmap: @@ -239,7 +240,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.3 resolution: 0.05 track_unknown_space: true @@ -249,7 +249,13 @@ global_costmap: enabled: True observation_sources: scan scan: - topic: /scan + # A relative topic will be appended to the parent of the global_costmap namespace. + # For example: + # * User chosen namespace is `tb4`. + # * User chosen topic is `scan`. + # * Topic will be remapped to `/tb4/scan` without `global_costmap`. + # * Use global topic `/scan` if you do not wish the node namespace to apply + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -266,21 +272,16 @@ global_costmap: cost_scaling_factor: 3.0 inflation_radius: 0.55 always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True -map_server: - ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" map_saver: ros__parameters: - use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -289,49 +290,167 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true -planner_server_rclcpp_node: +smoother_server: ros__parameters: - use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True -recoveries_server: +behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_recoveries/Wait" - global_frame: odom + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: true + transform_tolerance: 0.1 simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 - -robot_state_publisher: - ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: true waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "/demo/odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + enable_stamped_cmd_vel: true + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "/demo/cmd_vel" + enable_stamped_cmd_vel: true + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "/local_costmap/costmap_raw" + footprint_topic: "/local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + enable_stamped_cmd_vel: true diff --git a/sam_bot_description/launch/display.launch.py b/sam_bot_description/launch/display.launch.py index 1c39303..4d47f7f 100644 --- a/sam_bot_description/launch/display.launch.py +++ b/sam_bot_description/launch/display.launch.py @@ -1,60 +1,93 @@ -import launch -from launch.substitutions import Command, LaunchConfiguration -import launch_ros import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node +from ros_gz_bridge.actions import RosGzBridge +from ros_gz_sim.actions import GzServer + + def generate_launch_description(): - pkg_share = launch_ros.substitutions.FindPackageShare(package='sam_bot_description').find('sam_bot_description') - default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf') - default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz') - world_path=os.path.join(pkg_share, 'world/my_world.sdf') - - robot_state_publisher_node = launch_ros.actions.Node( + pkg_share = get_package_share_directory('sam_bot_description') + ros_gz_sim_share = get_package_share_directory('ros_gz_sim') + gz_spawn_model_launch_source = os.path.join(ros_gz_sim_share, "launch", "gz_spawn_model.launch.py") + default_model_path = os.path.join(pkg_share, 'src', 'description', 'sam_bot_description.sdf') + default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'config.rviz') + world_path = os.path.join(pkg_share, 'world', 'my_world.sdf') + bridge_config_path = os.path.join(pkg_share, 'config', 'bridge_config.yaml') + + robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}, {'use_sim_time': LaunchConfiguration('use_sim_time')}] ) - joint_state_publisher_node = launch_ros.actions.Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')), - parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}] - ) - rviz_node = launch_ros.actions.Node( + rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', output='screen', arguments=['-d', LaunchConfiguration('rvizconfig')], ) - spawn_entity = launch_ros.actions.Node( - package='gazebo_ros', - executable='spawn_entity.py', - arguments=['-entity', 'sam_bot', '-topic', 'robot_description'], - output='screen' + gz_server = GzServer( + world_sdf_file=world_path, + container_name='ros_gz_container', + create_own_container='True', + use_composition='True', + ) + ros_gz_bridge = RosGzBridge( + bridge_name='ros_gz_bridge', + config_file=bridge_config_path, + container_name='ros_gz_container', + create_own_container='False', + use_composition='True', ) - robot_localization_node = launch_ros.actions.Node( - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node', - output='screen', - parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}] + camera_bridge_image = Node( + package='ros_gz_image', + executable='image_bridge', + name='bridge_gz_ros_camera_image', + output='screen', + parameters=[{'use_sim_time': True}], + arguments=['/depth_camera/image'], + ) + camera_bridge_depth = Node( + package='ros_gz_image', + executable='image_bridge', + name='bridge_gz_ros_camera_depth', + output='screen', + parameters=[{'use_sim_time': True}], + arguments=['/depth_camera/depth_image'], + ) + spawn_entity = IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_spawn_model_launch_source), + launch_arguments={ + 'world': 'my_world', + 'topic': '/robot_description', + 'entity_name': 'sam_bot', + 'z': '0.65', + }.items(), + ) + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_node', + output='screen', + parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}], ) - return launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument(name='gui', default_value='True', - description='Flag to enable joint_state_publisher_gui'), - launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path, - description='Absolute path to robot urdf file'), - launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, - description='Absolute path to rviz config file'), - launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True', - description='Flag to enable use_sim_time'), - launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'), - joint_state_publisher_node, + return LaunchDescription([ + DeclareLaunchArgument(name='model', default_value=default_model_path, description='Absolute path to robot model file'), + DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, description='Absolute path to rviz config file'), + DeclareLaunchArgument(name='use_sim_time', default_value='True', description='Flag to enable use_sim_time'), + ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen'), robot_state_publisher_node, + rviz_node, + gz_server, + ros_gz_bridge, + camera_bridge_image, + camera_bridge_depth, spawn_entity, robot_localization_node, - rviz_node ]) diff --git a/sam_bot_description/package.xml b/sam_bot_description/package.xml index 94e6977..113996e 100644 --- a/sam_bot_description/package.xml +++ b/sam_bot_description/package.xml @@ -9,11 +9,13 @@ ament_cmake - joint_state_publisher + robot_localization robot_state_publisher + ros_gz_bridge + ros_gz_sim rviz2 + sdformat_urdf xacro - robot_localization ament_lint_auto ament_lint_common diff --git a/sam_bot_description/rviz/urdf_config.rviz b/sam_bot_description/rviz/config.rviz similarity index 100% rename from sam_bot_description/rviz/urdf_config.rviz rename to sam_bot_description/rviz/config.rviz diff --git a/sam_bot_description/src/description/sam_bot_description.sdf b/sam_bot_description/src/description/sam_bot_description.sdf new file mode 100644 index 0000000..68f5597 --- /dev/null +++ b/sam_bot_description/src/description/sam_bot_description.sdf @@ -0,0 +1,420 @@ + + + + + + + + + + + + + + + + + + + + + 0 0 0 ${pi/2} 0 ${pi/2} + ${m} + + ${(m/12) * (h*h + d*d)} + 0.0 + 0.0 + ${(m/12) * (w*w + d*d)} + 0.0 + ${(m/12) * (w*w + h*h)} + + + + + + + 0 0 0 ${pi/2} 0 0 + ${m} + + ${(m/12) * (3*r*r + h*h)} + 0.0 + 0.0 + ${(m/12) * (3*r*r + h*h)} + 0.0 + ${(m/2) * (r*r)} + + + + + + + + ${m} + + ${(2/5) * m * (r*r)} + 0.0 + 0.0 + ${(2/5) * m * (r*r)} + 0.0 + ${(2/5) * m * (r*r)} + + + + + + + true + + + + ${base_length} ${base_width} ${base_height} + + + + 0 1 1 1 + 0 1 1 1 + + + + + + + ${base_length} ${base_width} ${base_height} + + + + + + + + + + + + + base_link + base_footprint + 0.0 0.0 ${-(wheel_radius+wheel_zoff)} 0 0 0 + + + + + + + + + + 0 0 0 ${pi/2} 0 0 + + + ${wheel_radius} + ${wheel_width} + + + + 0.3 0.3 0.3 1.0 + 0.7 0.7 0.7 1.0 + + + + + 0 0 0 ${pi/2} 0 0 + + + ${wheel_radius} + ${wheel_width} + + + + + + + + + base_link + ${prefix}_link + ${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff} 0 0 0 + + 0 1 0 + + -inf + inf + + + + + + + + + + + + + + + ${(wheel_radius+wheel_zoff-(base_height/2))} + + + + 0 1 1 1 + 0 1 1 1 + + + + + + + ${(wheel_radius+wheel_zoff-(base_height/2))} + + + + 0.001 + 0.001 + + + + + + + + base_link + front_caster + ${caster_xoff} 0.0 ${-(base_height/2)} 0 0 0 + + + + base_link + imu_link + 0.0 0.0 0.01 0 0 0 + + + + + + + + 0.1 0.1 0.1 + + + + + + + + 0.1 0.1 0.1 + + + + + + + + true + 100 + true + demo/imu + imu_link + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + + drivewhl_l_joint + drivewhl_r_joint + + + 0.4 + ${wheel_radius} + + + 0.1 + + + /demo/cmd_vel + + + /demo/odom + /tf + + odom + base_link + + + + joint_states + + + + base_link + lidar_link + 0.0 0.0 0.12 0 0 0 + + + + + + + + 0.0508 + 0.055 + + + + + + + + 0.0508 + 0.055 + + + + + + + + true + true + 5 + scan + lidar_link + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + + base_link + camera_link + 0.215 0 0.05 0 0 0 + + + + + + + + 0.015 0.130 0.0222 + + + + + + + + 0.015 0.130 0.0222 + + + + + + + + true + true + 5.0 + depth_camera + camera_link + + 1.047198 + + 640 + 480 + + + 0.05 + 3 + + + 0.2 + 0.5 + 3.0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + diff --git a/sam_bot_description/src/description/sam_bot_description.urdf b/sam_bot_description/src/description/sam_bot_description.urdf index eda0f2f..8f41842 100644 --- a/sam_bot_description/src/description/sam_bot_description.urdf +++ b/sam_bot_description/src/description/sam_bot_description.urdf @@ -55,13 +55,11 @@ - - - + @@ -152,104 +150,6 @@ - - - - - /demo - ~/out:=imu - - false - - true - 100 - true - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - - - - - /demo - - - - - drivewhl_l_joint - drivewhl_r_joint - - - 0.4 - 0.2 - - - 20 - 1.0 - - - true - false - true - - odom - base_link - - - @@ -278,41 +178,6 @@ - - - true - true - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - ~/out:=scan - - sensor_msgs/LaserScan - lidar_link - - - - @@ -340,49 +205,4 @@ - - - - - - - - - - - - true - 30.0 - - 1.047198 - - 640 - 480 - R8G8B8 - - - 0.05 - 3 - - - - 0.2 - true - 0.0 - camera_depth_frame - 0.5 - 3.0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - - - diff --git a/sam_bot_description/world/my_world.sdf b/sam_bot_description/world/my_world.sdf index fe1c0ea..66ce5a0 100644 --- a/sam_bot_description/world/my_world.sdf +++ b/sam_bot_description/world/my_world.sdf @@ -1,5 +1,29 @@ - + + + 0.001 + 1.0 + + + + + + + + + + + ogre2 + 1 0 0 10 0 -0 0 @@ -239,4 +263,4 @@ - \ No newline at end of file +