From 351b1e526fd3141abb9f7237a9d35ac42430b612 Mon Sep 17 00:00:00 2001 From: Fastroof Date: Fri, 24 Jan 2025 21:04:54 +0000 Subject: [PATCH] Add GPS follower with ROS 2 control integration; EKF adjustments needed for improved performance on curve paths and turns (#10) --- CMakeLists.txt | 2 +- README.md | 58 +++++ config/dual_ekf_navsat_params.yaml | 127 +++++++++++ config/gps_wpf_demo.mvc | 61 ++++++ config/my_controllers.yaml | 9 +- config/nav2_no_map_params.yaml | 334 +++++++++++++++++++++++++++++ config/waypoints.yaml | 13 ++ description/imu.xacro | 18 ++ description/lidar.xacro | 46 ++++ description/navsat.xacro | 19 ++ description/robot.urdf.xacro | 3 + description/ros2_control.xacro | 4 +- executables/example_diff_drive.cpp | 2 +- launch/dual_ekf_navsat.launch.py | 54 +++++ launch/launch_robot.launch.py | 2 +- launch/launch_sim.launch.py | 73 ++++++- launch/mapviz.launch.py | 34 +++ launch/rsp.launch.py | 2 +- launch/spawn_robot.launch.py | 2 +- package.xml | 9 +- params/robot_bridge.yaml | 27 +++ worlds/empty.world | 22 +- 22 files changed, 901 insertions(+), 20 deletions(-) create mode 100644 README.md create mode 100644 config/dual_ekf_navsat_params.yaml create mode 100644 config/gps_wpf_demo.mvc create mode 100644 config/nav2_no_map_params.yaml create mode 100644 config/waypoints.yaml create mode 100644 description/imu.xacro create mode 100644 description/lidar.xacro create mode 100644 description/navsat.xacro create mode 100644 launch/dual_ekf_navsat.launch.py create mode 100644 launch/mapviz.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cfbb94..fea5ae3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(apricotka-robot-car) +project(ros_sw) # Default to C99 if(NOT CMAKE_C_STANDARD) diff --git a/README.md b/README.md new file mode 100644 index 0000000..7093d86 --- /dev/null +++ b/README.md @@ -0,0 +1,58 @@ +# ROS Setup Instructions + +## Install Required Packages + +```bash +sudo apt install ros-jazzy-navigation2 +sudo apt install ros-jazzy-nav2-bringup +sudo apt install ros-jazzy-robot-localization +sudo apt install ros-jazzy-mapviz +sudo apt install ros-jazzy-mapviz-plugins +sudo apt install ros-jazzy-tile-map +``` + +## Setup ROS Environment + +```bash +source /opt/ros/jazzy/setup.bash +``` + +## Create Workspace and Clone Repositories + +```bash +mkdir gps_ws +cd gps_ws +git clone -b 10-gps-navigation https://github.com/KPI-Rover/ros_sw.git +git clone -b jazzy https://github.com/ros-navigation/navigation2_tutorials.git +``` + +## Build the Workspace + +```bash +colcon build --symlink-install +source install/setup.bash +``` + +## Launch Simulation + +```bash +ros2 launch ros_sw launch_sim.launch.py use_rviz:=True use_mapviz:=True +``` + +## Run GPS Waypoint Follower Demos + +```bash +ros2 run nav2_gps_waypoint_follower_demo interactive_waypoint_follower +ros2 run nav2_gps_waypoint_follower_demo gps_waypoint_logger +ros2 run nav2_gps_waypoint_follower_demo logged_waypoint_follower +``` + +## Run Teleop Twist Keyboard + +```bash +ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_drive_controller/cmd_vel -p stamped:=True +``` + +## More Information + +For more information, please visit [Navigation2 with GPS](https://docs.nav2.org/tutorials/docs/navigation2_with_gps.html). diff --git a/config/dual_ekf_navsat_params.yaml b/config/dual_ekf_navsat_params.yaml new file mode 100644 index 0000000..447a374 --- /dev/null +++ b/config/dual_ekf_navsat_params.yaml @@ -0,0 +1,127 @@ +# For parameter descriptions, please refer to the template parameter files for each node. + +ekf_filter_node_odom: + ros__parameters: + frequency: 50.0 # Increased from 30.0 for better update rate + two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin + world_frame: odom + + odom0: diff_drive_controller/odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: true # Changed from false to improve accuracy during turns + odom0_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + true, true, true, # Added linear acceleration + false, false, false, + false, false, false] + imu0_differential: true # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1] + +ekf_filter_node_map: + ros__parameters: + frequency: 50.0 # Increased from 30.0 for better update rate + two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin + world_frame: map + + odom0: diff_drive_controller/odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: true # Changed from false to improve accuracy during turns + odom0_relative: false + + odom1: odometry/gps + odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_queue_size: 10 + odom1_differential: false + odom1_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + true, true, true, # Added linear acceleration + false, false, false, + false, false, false] + imu0_differential: true # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1] + +navsat_transform: + ros__parameters: + frequency: 50.0 # Increased from 30.0 for better update rate + delay: 3.0 + magnetic_declination_radians: 0.0 + yaw_offset: 0.0 + zero_altitude: true + broadcast_cartesian_transform: true + publish_filtered_gps: true + use_odometry_yaw: true + wait_for_datum: false diff --git a/config/gps_wpf_demo.mvc b/config/gps_wpf_demo.mvc new file mode 100644 index 0000000..be78621 --- /dev/null +++ b/config/gps_wpf_demo.mvc @@ -0,0 +1,61 @@ +capture_directory: "~" +fixed_frame: map +target_frame: map +fix_orientation: false +rotate_90: false +enable_antialiasing: true +show_displays: true +show_status_bar: true +show_capture_tools: true +view_scale: 0.564473748 +offset_x: -55.0469894 +offset_y: -4.65194941 +use_latest_transforms: true +background: "#a0a0a4" +image_transport: raw +displays: + - type: mapviz_plugins/tile_map + name: new display + config: + visible: true + collapsed: false + custom_sources: + - base_url: https://api.maptiler.com/tiles/satellite-v2/{level}/{x}/{y}.jpg?key=prOIJC29PpyOVqzLxuuJ + max_zoom: 23 + name: mt + type: wmts + - base_url: http://tile.openstreetmap.org/{level}/{x}/{y}.png + max_zoom: 21 + name: osm + type: wmts + bing_api_key: "" + source: mt + - type: mapviz_plugins/point_click_publisher + name: new display + config: + visible: true + collapsed: false + topic: clicked_point + output_frame: wgs84 + - type: mapviz_plugins/tf_frame + name: new display + config: + visible: true + collapsed: false + frame: base_link + color: "#00ff00" + draw_style: arrows + position_tolerance: 0 + buffer_size: 1 + static_arrow_sizes: true + arrow_size: 53 + - type: mapviz_plugins/navsat + name: new display + config: + visible: true + collapsed: false + topic: /gps/fix + color: "#55aaff" + draw_style: points + position_tolerance: 0 + buffer_size: 1 \ No newline at end of file diff --git a/config/my_controllers.yaml b/config/my_controllers.yaml index 5e059de..bd94120 100644 --- a/config/my_controllers.yaml +++ b/config/my_controllers.yaml @@ -5,10 +5,10 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - diff_drive_base_controller: + diff_drive_controller: type: diff_drive_controller/DiffDriveController -diff_drive_base_controller: +diff_drive_controller: ros__parameters: left_wheel_names: ["front_left_wheel_joint", "rear_left_wheel_joint"] right_wheel_names: ["front_right_wheel_joint", "rear_right_wheel_joint"] @@ -17,6 +17,7 @@ diff_drive_base_controller: wheel_radius: 0.033 publish_rate: 50.0 - # odom_frame_id: odom + odom_frame_id: odom base_frame_id: base_link - use_stamped_vel: false \ No newline at end of file + tf_frame_prefix_enable: false + # open_loop: true \ No newline at end of file diff --git a/config/nav2_no_map_params.yaml b/config/nav2_no_map_params.yaml new file mode 100644 index 0000000..9d330d6 --- /dev/null +++ b/config/nav2_no_map_params.yaml @@ -0,0 +1,334 @@ +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + transform_tolerance: 0.1 + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + enable_stamped_cmd_vel: True # if control stamped + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + odom_topic: /odom + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + 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.5 # Increased from 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.5 # Increased from 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.5 # Increased from 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: 50.0 # Increased from 32.0 to prioritize path alignment + PathAlign.forward_point_distance: 0.2 # Increased from 0.1 to look further ahead + GoalAlign.scale: 10.0 # Decreased from 24.0 to reduce goal alignment influence + 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 + +# GPS WPF CHANGE: Remove static layer +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: laser_scan_sensor + laser_scan_sensor: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + always_send_full_costmap: True + +# GPS WPF CHANGE: Remove static layer +# GPS WPF CHANGE: Set rolling global costmap with 50x50 size. See note below +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.1 + # When using GPS navigation you will potentially traverse huge environments which are not practical to + # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to + # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 + rolling_window: True + width: 50 + height: 50 + track_unknown_space: true + # no static map + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: laser_scan_sensor + laser_scan_sensor: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + # outdoors there will probably be more inf points + inf_is_valid: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# 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: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + 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 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + 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" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + enable_stamped_cmd_vel: True # if control stamped + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + 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: + enable_stamped_cmd_vel: True # if control stamped + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "/odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True # if control stamped + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "/cmd_vel_smoothed" + cmd_vel_out_topic: "/cmd_vel" + transform_tolerance: 0.5 + source_timeout: 5.0 + stop_pub_timeout: 2.0 + polygons: ["PolygonStop", "PolygonSlow"] + PolygonStop: + type: "circle" + radius: 0.1 + action_type: "stop" + min_points: 4 + visualize: True + polygon_pub_topic: "polygon_stop" + PolygonSlow: + type: "polygon" + points: "[[1.0, 1.0], [1.0, -1.0], [-0.5, -1.0], [-0.5, 1.0]]" + action_type: "slowdown" + min_points: 4 + slowdown_ratio: 0.3 + visualize: True + polygon_pub_topic: "polygon_slowdown" + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True # if control stamped + controller_frequency: 20.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: ['nova_carter_dock'] + nova_carter_dock: + plugin: 'opennav_docking::SimpleChargingDock' # Also 'opennav_docking::SimpleNonChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false + + 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 + docks: ['home_dock'] + home_dock: + type: 'nova_carter_dock' + frame: map + pose: [0.0, 0.0, 0.0] + id: 'c67f50cb-e152-4720-85cc-5eb20bd85ce8' + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 \ No newline at end of file diff --git a/config/waypoints.yaml b/config/waypoints.yaml new file mode 100644 index 0000000..367eb37 --- /dev/null +++ b/config/waypoints.yaml @@ -0,0 +1,13 @@ +waypoints: +- latitude: 38.161491054181276 + longitude: -122.45464431092836 + yaw: 0.0 +- latitude: 38.161587576524845 + longitude: -122.4547994038464 + yaw: 1.57 +- latitude: 38.161708040316405 + longitude: -122.45499603070951 + yaw: 3.14 +- latitude: 38.16180165780551 + longitude: -122.45515645020123 + yaw: 4.71 diff --git a/description/imu.xacro b/description/imu.xacro new file mode 100644 index 0000000..05f0337 --- /dev/null +++ b/description/imu.xacro @@ -0,0 +1,18 @@ + + + + + + + + + + + + + 1 + false + imu/data + + + \ No newline at end of file diff --git a/description/lidar.xacro b/description/lidar.xacro new file mode 100644 index 0000000..d9a67f9 --- /dev/null +++ b/description/lidar.xacro @@ -0,0 +1,46 @@ + + + + + + + + + + + + 0 0 0.2 0 0 0 + + + + + 0 0 0 0 0 0 + 10 + scan + lidar_link + + + + 360 + 1 + 0 + 6.28 + + + 1 + 0.01 + 0 + 0 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + \ No newline at end of file diff --git a/description/navsat.xacro b/description/navsat.xacro new file mode 100644 index 0000000..d52f2d4 --- /dev/null +++ b/description/navsat.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + 1 + 1 + /gps/fix + navsat_link + + + \ No newline at end of file diff --git a/description/robot.urdf.xacro b/description/robot.urdf.xacro index b503a90..cc20543 100644 --- a/description/robot.urdf.xacro +++ b/description/robot.urdf.xacro @@ -6,5 +6,8 @@ + + + \ No newline at end of file diff --git a/description/ros2_control.xacro b/description/ros2_control.xacro index 61202bf..86b9535 100644 --- a/description/ros2_control.xacro +++ b/description/ros2_control.xacro @@ -46,8 +46,8 @@ - $(find apricotka-robot-car)/config/my_controllers.yaml + $(find ros_sw)/config/my_controllers.yaml - + \ No newline at end of file diff --git a/executables/example_diff_drive.cpp b/executables/example_diff_drive.cpp index 203abc7..678fb5e 100644 --- a/executables/example_diff_drive.cpp +++ b/executables/example_diff_drive.cpp @@ -14,7 +14,7 @@ int main(int argc, char * argv[]) std::make_shared("diff_drive_test_node"); auto publisher = node->create_publisher( - "/diff_drive_base_controller/cmd_vel", 10); + "/diff_drive_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); diff --git a/launch/dual_ekf_navsat.launch.py b/launch/dual_ekf_navsat.launch.py new file mode 100644 index 0000000..d9edeea --- /dev/null +++ b/launch/dual_ekf_navsat.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +import launch_ros.actions +import os +import launch.actions + + +def generate_launch_description(): + rl_params_file = os.path.join( + get_package_share_directory('ros_sw'), + "config", + "dual_ekf_navsat_params.yaml" + ) + + return LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + "output_final_position", default_value="false" + ), + launch.actions.DeclareLaunchArgument( + "output_location", default_value="false" + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_odom", + output="screen", + parameters=[rl_params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/local")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_map", + output="screen", + parameters=[rl_params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/global")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="navsat_transform_node", + name="navsat_transform", + output="screen", + parameters=[rl_params_file, {"use_sim_time": True}], + remappings=[ + ("imu/data", "imu/data"), + ("gps/fix", "gps/fix"), + ("gps/filtered", "gps/filtered"), + ("odometry/gps", "odometry/gps"), + ("odometry/filtered", "odometry/global"), + ], + ), + ] + ) diff --git a/launch/launch_robot.launch.py b/launch/launch_robot.launch.py index f4773ff..6aa1675 100644 --- a/launch/launch_robot.launch.py +++ b/launch/launch_robot.launch.py @@ -11,7 +11,7 @@ from launch_ros.actions import Node def generate_launch_description(): - package_name='apricotka-robot-car' + package_name='ros_sw' rsp_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( diff --git a/launch/launch_sim.launch.py b/launch/launch_sim.launch.py index 2e70cee..f41a877 100644 --- a/launch/launch_sim.launch.py +++ b/launch/launch_sim.launch.py @@ -4,19 +4,22 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, ExecuteProcess +from launch.actions import IncludeLaunchDescription, ExecuteProcess, DeclareLaunchArgument, GroupAction +from launch_ros.actions import SetRemap from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from nav2_common.launch import RewrittenYaml def generate_launch_description(): - package_name='apricotka-robot-car' + package = get_package_share_directory('ros_sw') ros_gz_sim = get_package_share_directory('ros_gz_sim') x_pose = LaunchConfiguration('x_pose', default='0.0') y_pose = LaunchConfiguration('y_pose', default='0.0') world = os.path.join( - get_package_share_directory(package_name), + package, 'worlds', 'empty.world' ) @@ -35,13 +38,13 @@ def generate_launch_description(): ) rsp_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory(package_name),'launch','rsp.launch.py')] + package,'launch','rsp.launch.py')] ), launch_arguments={'use_sim_time': 'true', 'use_ros2_control': 'true'}.items() ) spawn_robot_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory(package_name),'launch', 'spawn_robot.launch.py') + os.path.join(package,'launch', 'spawn_robot.launch.py') ), launch_arguments={ 'x_pose': x_pose, @@ -57,10 +60,61 @@ def generate_launch_description(): load_diff_drive_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'diff_drive_base_controller'], + 'diff_drive_controller'], output='screen' ) + robot_localization_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(os.path.join(package, 'launch'), 'dual_ekf_navsat.launch.py')) + ) + + nav2_bringup = get_package_share_directory('nav2_bringup') + configured_params = RewrittenYaml( + source_file=os.path.join(os.path.join(package, "config"), "nav2_no_map_params.yaml"), + root_key="", param_rewrites="", convert_types=True + ) + navigation2_cmd = GroupAction( + actions=[ + SetRemap(src='/cmd_vel', dst='/diff_drive_controller/cmd_vel'), + SetRemap(src='/odom', dst='/diff_drive_controller/odom'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup, "launch", "navigation_launch.py") + ), + launch_arguments={ + "use_sim_time": "True", + "params_file": configured_params, + "autostart": "True", + }.items() + ) + ] + ) + + use_rviz = LaunchConfiguration('use_rviz') + use_mapviz = LaunchConfiguration('use_mapviz') + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='False', + description='Whether to start RVIZ') + + declare_use_mapviz_cmd = DeclareLaunchArgument( + 'use_mapviz', + default_value='False', + description='Whether to start mapviz') + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup, "launch", 'rviz_launch.py')), + condition=IfCondition(use_rviz) + ) + + mapviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(os.path.join(package, 'launch'), 'mapviz.launch.py')), + condition=IfCondition(use_mapviz) + ) + # Launch them all! ld = LaunchDescription() @@ -71,4 +125,11 @@ def generate_launch_description(): ld.add_action(load_joint_state_broadcaster) ld.add_action(load_diff_drive_controller) + ld.add_action(robot_localization_cmd) + ld.add_action(navigation2_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(rviz_cmd) + ld.add_action(declare_use_mapviz_cmd) + ld.add_action(mapviz_cmd) + return ld \ No newline at end of file diff --git a/launch/mapviz.launch.py b/launch/mapviz.launch.py new file mode 100644 index 0000000..1b74153 --- /dev/null +++ b/launch/mapviz.launch.py @@ -0,0 +1,34 @@ +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return launch.LaunchDescription([ + launch_ros.actions.Node( + package="mapviz", + executable="mapviz", + name="mapviz", + parameters=[{"config": os.path.join(get_package_share_directory('ros_sw'), + "config", "gps_wpf_demo.mvc")}] + ), + launch_ros.actions.Node( + package="swri_transform_util", + executable="initialize_origin.py", + name="initialize_origin", + remappings=[ + ("fix", "gps/fix"), + ], + ), + launch_ros.actions.Node( + package="tf2_ros", + executable="static_transform_publisher", + name="swri_transform", + arguments=["--x", "0", "--y", "0", "--z", "0", + "--roll", "0", "--pitch", "0", "--yaw", "0", + "--frame-id", "map", "--child-frame-id", "origin"] + ) + ]) diff --git a/launch/rsp.launch.py b/launch/rsp.launch.py index 4246449..6991deb 100644 --- a/launch/rsp.launch.py +++ b/launch/rsp.launch.py @@ -13,7 +13,7 @@ def generate_launch_description(): use_ros2_control = LaunchConfiguration('use_ros2_control') # Process the URDF file - pkg_path = os.path.join(get_package_share_directory('apricotka-robot-car')) + pkg_path = os.path.join(get_package_share_directory('ros_sw')) xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro') robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' sim_mode:=', use_sim_time]) diff --git a/launch/spawn_robot.launch.py b/launch/spawn_robot.launch.py index 60622ad..f6a046c 100644 --- a/launch/spawn_robot.launch.py +++ b/launch/spawn_robot.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): - package_name='apricotka-robot-car' + package_name='ros_sw' # Launch configuration variables specific to simulation x_pose = LaunchConfiguration('x_pose', default='0.0') diff --git a/package.xml b/package.xml index 107eb98..30204b2 100644 --- a/package.xml +++ b/package.xml @@ -1,9 +1,9 @@ - apricotka-robot-car + ros_sw 0.0.0 - Apricotka Robot Car + ros_sw fastroof TODO: License declaration @@ -12,6 +12,11 @@ ament_lint_auto ament_lint_common + navigation2 + robot_localization + mapviz + mapviz_plugins + tile_map ros2_control diff --git a/params/robot_bridge.yaml b/params/robot_bridge.yaml index eca6010..2a0dfb0 100644 --- a/params/robot_bridge.yaml +++ b/params/robot_bridge.yaml @@ -1,6 +1,33 @@ +# gz topic published by the simulator core +- 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 + +# gz topic published by IMU plugin +- ros_topic_name: "imu/data" + gz_topic_name: "imu/data" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + # gz topic published by Sensors plugin (Camera) - 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 + +# gz topic published by NavSat plugin +- ros_topic_name: "/gps/fix" + gz_topic_name: "/gps/fix" + ros_type_name: "sensor_msgs/msg/NavSatFix" + gz_type_name: "gz.msgs.NavSat" + 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 \ No newline at end of file diff --git a/worlds/empty.world b/worlds/empty.world index e923bd3..7d5effa 100644 --- a/worlds/empty.world +++ b/worlds/empty.world @@ -18,7 +18,27 @@ name="gz::sim::systems::Sensors"> ogre2 - + + + + + + + + + EARTH_WGS84 + ENU + 38.161491054181276 + -122.45464431092836 + 0 + 0 + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane