diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index b88bcd7..458e0fa 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/src/rove_bringup/launch/common.launch.py @@ -45,23 +45,24 @@ def generate_launch_description(): # used tutorial: https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html robot_localization_node_local = Node( - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node_local', - output='screen', - parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), - {'use_sim_time': True}] - ) + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_local', + output='screen', + parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), + {'use_sim_time': True}], + remappings=[('odometry/filtered', 'odometry/local')]) + robot_localization_node_global = Node( - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node_global', - output='screen', - parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), - {'use_sim_time': True}], - remappings=[("odometry/filtered", "odometry/filtered/global")], - ) + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_global', + output='screen', + parameters=[os.path.join(pkg_rove_slam, 'config/ekf.yaml'), + {'use_sim_time': True}], + remappings=[('odometry/filtered', 'odometry/global')]) + navsat_transform = Node( package="robot_localization", @@ -74,10 +75,7 @@ def generate_launch_description(): # Subscribed Topics ("imu/data", "imu"), ("gps/fix", "gps"), - ("odometry/filtered", "odometry/filtered/global"), - # Published Topics - ("gps/filtered", "gps/filtered"), - ("odometry/gps", "odometry/gps"), + ('odometry/filtered', 'odometry/global'), ], ) diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 2609407..6334030 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -4,12 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /RobotModel1 - - /TF1/Frames1 - - /Map1 + - /RobotModel1/Status1 Splitter Ratio: 0.5 - Tree Height: 523 + Tree Height: 517 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -49,36 +46,66 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_footprint: + base_link: Alpha: 1 Show Axes: false Show Trail: false - base_link: + Value: true + flipper_fl: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_scan: + flipper_fr: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_fl: + flipper_rl: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_fr: + flipper_rr: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_rl: + rove_end_effector: + Alpha: 1 + Show Axes: false + Show Trail: false + rove_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - flipper_rr: + rove_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rove_link_base: Alpha: 1 Show Axes: false Show Trail: false @@ -98,6 +125,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + velodyne_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -163,11 +200,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - base_footprint: - Value: true base_link: - Value: false - base_scan: Value: true flipper_fl: Value: true @@ -177,16 +210,36 @@ Visualization Manager: Value: true flipper_rr: Value: true - sensor_link: - Value: true map: Value: true odom: Value: true + rove_end_effector: + Value: true + rove_link_1: + Value: true + rove_link_2: + Value: true + rove_link_3: + Value: true + rove_link_4: + Value: true + rove_link_5: + Value: true + rove_link_6: + Value: true + rove_link_base: + Value: true + sensor_link: + Value: true track_l: Value: true track_r: Value: true + velodyne_laser: + Value: true + velodyne_link: + Value: true Marker Scale: 1 Name: TF Show Arrows: true @@ -195,23 +248,32 @@ Visualization Manager: Tree: map: odom: - base_footprint: - base_link: - base_scan: - {} - flipper_fl: - {} - flipper_fr: - {} - flipper_rl: - {} - flipper_rr: - {} - sensor_link: - {} - track_l: - {} - track_r: + base_link: + flipper_fl: + {} + flipper_fr: + {} + flipper_rl: + {} + flipper_rr: + {} + rove_link_base: + rove_link_1: + rove_link_2: + rove_link_3: + rove_link_4: + rove_link_5: + rove_link_6: + rove_end_effector: + {} + sensor_link: + {} + track_l: + {} + track_r: + {} + velodyne_link: + velodyne_laser: {} Update Interval: 0 Value: true @@ -281,46 +343,7 @@ Visualization Manager: Scale: 1 Value: true Value: false - Enabled: false - Keep: 1 - Name: raw odom - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /odom - Value: false - - Angle Tolerance: 0.10000000149011612 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: false - Enabled: false + Enabled: true Keep: 1 Name: filtered position Position Tolerance: 0.10000000149011612 @@ -341,7 +364,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /odometry/filtered - Value: false + Value: true - Acceleration properties: Acc. vector alpha: 1 Acc. vector color: 255; 0; 0 @@ -416,25 +439,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 86.56747436523438 + Distance: 15.358924865722656 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 11.144978523254395 - Y: -2.5526533126831055 - Z: -4.807077407836914 + X: -0.7862069010734558 + Y: 0.829606294631958 + Z: -1.3278541564941406 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.1853981018066406 + Pitch: 0.900398313999176 Target Frame: Value: Orbit (rviz) - Yaw: 6.198585033416748 + Yaw: 5.728583812713623 Saved: ~ Window Geometry: Displays: @@ -442,7 +465,7 @@ Window Geometry: Height: 814 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000294fc020000000afb000000100044006900730070006c006100790073010000003b00000294000000c700fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000afb000000100044006900730070006c006100790073010000003d00000290000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -452,5 +475,5 @@ Window Geometry: Views: collapsed: false Width: 1103 - X: 2030 - Y: 119 + X: 817 + Y: 109 diff --git a/src/rove_description/config/navsat_transform.yaml b/src/rove_description/config/navsat_transform.yaml index a6faa62..a7669e2 100644 --- a/src/rove_description/config/navsat_transform.yaml +++ b/src/rove_description/config/navsat_transform.yaml @@ -4,9 +4,10 @@ navsat_transform: frequency: 30.0 delay: 3.0 magnetic_declination_radians: 0.0 - yaw_offset: 0.0 + yaw_offset: pi zero_altitude: true broadcast_utm_transform: true publish_filtered_gps: true use_odometry_yaw: true - wait_for_datum: false \ No newline at end of file + wait_for_datum: false + broadcast_utm_transform_as_parent_frame: false \ No newline at end of file diff --git a/src/rove_description/urdf/fake_wheels.urdf.xacro b/src/rove_description/urdf/fake_wheels.urdf.xacro new file mode 100644 index 0000000..21531e3 --- /dev/null +++ b/src/rove_description/urdf/fake_wheels.urdf.xacro @@ -0,0 +1,53 @@ + + + + + + + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + + + diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro index 9c33f2d..290568c 100644 --- a/src/rove_description/urdf/gazebo.urdf.xacro +++ b/src/rove_description/urdf/gazebo.urdf.xacro @@ -28,16 +28,7 @@ 1 cmd_vel - odom - odom - base_footprint - 30 - - false - false - - /tf - @@ -42,11 +41,6 @@ - - - - - diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index 50b08b4..7f92b0b 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -31,23 +31,10 @@ Gazebo/Black - imu_link + sensor_link true - 200 + 50 imu - - - gaussian - - 0.0 - 0.01 - - - 0.0 - 0.01 - - - diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro index bb8c100..00685ee 100644 --- a/src/rove_description/urdf/track.urdf.xacro +++ b/src/rove_description/urdf/track.urdf.xacro @@ -35,8 +35,8 @@ - 0.65 - 0.85 + 1 + 1 @@ -53,7 +53,7 @@ - + track_${suffix} -3.0 3.0 diff --git a/src/rove_description/urdf/velodyne.urdf.xacro b/src/rove_description/urdf/velodyne.urdf.xacro index 7c4cb8b..bb72928 100644 --- a/src/rove_description/urdf/velodyne.urdf.xacro +++ b/src/rove_description/urdf/velodyne.urdf.xacro @@ -64,10 +64,9 @@ - ${update_rate} true true - 5 + 50 scan ${name}_laser diff --git a/src/rove_description/worlds/base_world.world b/src/rove_description/worlds/base_world.world index 813dfbd..ae1681a 100644 --- a/src/rove_description/worlds/base_world.world +++ b/src/rove_description/worlds/base_world.world @@ -98,10 +98,7 @@ - + 1 1 1 1 @@ -127,10 +124,7 @@ - + 1 1 1 1 @@ -156,10 +150,7 @@ - + 1 1 1 1 @@ -185,10 +176,7 @@ - + 1 1 1 1 @@ -214,10 +202,7 @@ - + 1 1 1 1 @@ -243,10 +228,7 @@ - + 1 1 1 1 @@ -272,10 +254,7 @@ - + 1 1 1 1 @@ -301,10 +280,7 @@ - + 1 1 1 1 @@ -330,10 +306,7 @@ - + 1 1 1 1 @@ -359,10 +332,7 @@ - + 1 1 1 1 @@ -388,10 +358,7 @@ - + 1 1 1 1 @@ -417,10 +384,7 @@ - + 1 1 1 1 @@ -446,10 +410,7 @@ - + 1 1 1 1 @@ -475,10 +436,7 @@ - + 1 1 1 1 @@ -504,10 +462,7 @@ - + 1 1 1 1 @@ -533,10 +488,7 @@ - + 1 1 1 1 @@ -562,10 +514,7 @@ - + 1 1 1 1 @@ -591,10 +540,7 @@ - + 1 1 1 1 @@ -620,10 +566,7 @@ - + 1 1 1 1 @@ -649,10 +592,7 @@ - + 1 1 1 1 @@ -678,10 +618,7 @@ - + 1 1 1 1 @@ -707,10 +644,7 @@ - + 1 1 1 1 @@ -736,10 +670,7 @@ - + 1 1 1 1 @@ -765,10 +696,7 @@ - + 1 1 1 1 @@ -794,10 +722,7 @@ - + 1 1 1 1 @@ -823,10 +748,7 @@ - + 1 1 1 1 @@ -852,10 +774,7 @@ - + 1 1 1 1 @@ -881,10 +800,7 @@ - + 1 1 1 1 @@ -910,10 +826,7 @@ - + 1 1 1 1 @@ -939,10 +852,7 @@ - + 1 1 1 1 @@ -968,10 +878,7 @@ - + 1 1 1 1 @@ -997,10 +904,7 @@ - + 1 1 1 1 @@ -1026,10 +930,7 @@ - + 1 1 1 1 diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index af906f7..c290fad 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/src/rove_navigation/config/rove_nav_params.yaml @@ -6,7 +6,7 @@ amcl: alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "base_footprint" + base_frame_id: "base_link" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -51,7 +51,7 @@ bt_navigator: ros__parameters: use_sim_time: true global_frame: map - robot_base_frame: base_footprint + robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 @@ -180,7 +180,7 @@ global_costmap: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map - robot_base_frame: base_footprint + robot_base_frame: base_link robot_radius: 0.22 # radius set and used, so no footprint points resolution: 0.05 plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] @@ -262,7 +262,7 @@ local_costmap: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom - robot_base_frame: base_footprint + robot_base_frame: base_link rolling_window: true width: 3 height: 3 @@ -375,7 +375,7 @@ recoveries_server: wait: plugin: "nav2_recoveries/Wait" global_frame: odom - robot_base_frame: base_footprint + robot_base_frame: base_link transform_timeout: 0.1 use_sim_time: true simulate_ahead_time: 2.0 diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml index 4e7371e..ddfefe3 100644 --- a/src/rove_slam/config/ekf.yaml +++ b/src/rove_slam/config/ekf.yaml @@ -3,53 +3,51 @@ ekf_filter_node_local: ros__parameters: frequency: 30.0 two_d_mode: false - publish_acceleration: true publish_tf: true - map_frame: map odom_frame: odom - base_link_frame: base_footprint + base_link_frame: base_link world_frame: odom odom0: /odom - odom0_config: [true, true, true, - false, false, true, + odom0_config: [true, true, false, + false, false, false, + true, true, false, false, false, false, - false, false, true, false, false, false] imu0: /imu - imu0_config: [false, false, false, - true, true, true, - false, false, false, - false, false, false, - false, false, false] + imu0_config: [false, false, false, # x y z + false, false, true, # roll pitch yaw + false, false, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + true, true, false] # ax ay az + imu0_queue_size: 20 ekf_filter_node_global: ros__parameters: frequency: 30.0 two_d_mode: false - publish_acceleration: true publish_tf: true - map_frame: map odom_frame: odom - base_link_frame: base_footprint - world_frame: map + base_link_frame: base_link + world_frame: gps odom0: /odom - odom0_config: [true, true, true, - false, false, true, + odom0_config: [true, true, false, + false, false, false, + true, true, false, false, false, false, - false, false, true, false, false, false] imu0: /imu - imu0_config: [false, false, false, - true, true, true, - false, false, false, - false, false, false, - false, false, false] + imu0_config: [false, false, false, # x y z + false, false, true, # roll pitch yaw + false, false, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + true, true, false] # ax ay az + imu0_queue_size: 20 # description from https://github.com/cra-ros-pkg/robot_localization/blob/ros2/params/ekf.yaml # Each sensor reading updates some or all of the filter's state. These options give you greater control over which diff --git a/src/rove_slam/config/slam_config.yaml b/src/rove_slam/config/slam_config.yaml index 22b9c99..b49b173 100644 --- a/src/rove_slam/config/slam_config.yaml +++ b/src/rove_slam/config/slam_config.yaml @@ -10,7 +10,7 @@ slam_toolbox: # ROS Parameters odom_frame: odom map_frame: map - base_frame: base_footprint + base_frame: base_link scan_topic: /scan mode: mapping #localization