diff --git a/waypoint_saver/launch/waypoint_saver.launch b/waypoint_saver/launch/waypoint_saver.launch
index bfdb74f..332d94d 100644
--- a/waypoint_saver/launch/waypoint_saver.launch
+++ b/waypoint_saver/launch/waypoint_saver.launch
@@ -3,7 +3,7 @@
-
+
diff --git a/waypoint_saver/rviz_cfg/saver.rviz b/waypoint_saver/rviz_cfg/saver.rviz
new file mode 100644
index 0000000..c337ca9
--- /dev/null
+++ b/waypoint_saver/rviz_cfg/saver.rviz
@@ -0,0 +1,405 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /TF1/Frames1
+ Splitter Ratio: 0.5529412031173706
+ Tree Height: 823
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: CameraScan
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 50
+ Reference Frame:
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic: /map
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Class: rviz/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ Marker Alpha: 1
+ Marker Scale: 5
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ {}
+ Update Interval: 0
+ Value: false
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ hokuyo_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_caster_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ payload_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_caster_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ velodyne_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /visualization_marker
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 255; 0; 0
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: Velodyne2D
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 7
+ Size (m): 0.10000000149011612
+ Style: Points
+ Topic: /scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 255; 255; 255
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: CameraScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.10000000149011612
+ Style: Flat Squares
+ Topic: /camera_scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 170; 255; 0
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: HokuyoScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /hokuyo_scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: GlobalCostmap
+ Topic: /move_base/global_costmap/costmap
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: LocalCostmap
+ Topic: /move_base/local_costmap/costmap
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: true
+ Enabled: true
+ Name: LocalCostmapArea
+ Topic: /move_base/local_costmap/costmap
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Class: rviz/Polygon
+ Color: 25; 255; 0
+ Enabled: true
+ Name: Footprint
+ Queue Size: 10
+ Topic: /move_base/global_costmap/footprint
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 255; 247; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: GlobalPlan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Queue Size: 10
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic: /move_base/DWAPlannerROS/global_plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Billboards
+ Line Width: 0.07999999821186066
+ Name: LocalPlan
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Queue Size: 10
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic: /move_base/DWAPlannerROS/local_plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/Pose
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: LocalGoal
+ Queue Size: 10
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /move_base_simple/goal
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/ThirdPersonFollower
+ Distance: 14.281959533691406
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: -0.9902782440185547
+ Y: 0.6635769605636597
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 1.5697963237762451
+ Target Frame: base_link
+ Yaw: 2.815399646759033
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 975
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000374fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000374000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e0000037a000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005b30000003efc0100000002fb0000000800540069006d00650000000000000005b30000046900fffffffb0000000800540069006d00650100000000000004500000000000000000000004570000037400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1459
+ X: -1
+ Y: -50
diff --git a/waypoint_saver/rviz_cfg/x.rviz b/waypoint_saver/rviz_cfg/x.rviz
deleted file mode 100644
index c337ca9..0000000
--- a/waypoint_saver/rviz_cfg/x.rviz
+++ /dev/null
@@ -1,405 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 0
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /TF1/Frames1
- Splitter Ratio: 0.5529412031173706
- Tree Height: 823
- - Class: rviz/Selection
- Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: CameraScan
-Preferences:
- PromptSaveOnExit: true
-Toolbars:
- toolButtonStyle: 2
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 50
- Reference Frame:
- Value: true
- - Alpha: 0.699999988079071
- Class: rviz/Map
- Color Scheme: map
- Draw Behind: false
- Enabled: true
- Name: Map
- Topic: /map
- Unreliable: false
- Use Timestamp: false
- Value: true
- - Class: rviz/TF
- Enabled: false
- Frame Timeout: 15
- Frames:
- All Enabled: false
- Marker Alpha: 1
- Marker Scale: 5
- Name: TF
- Show Arrows: true
- Show Axes: true
- Show Names: true
- Tree:
- {}
- Update Interval: 0
- Value: false
- - Alpha: 1
- Class: rviz/RobotModel
- Collision Enabled: false
- Enabled: true
- Links:
- All Links Enabled: true
- Expand Joint Details: false
- Expand Link Details: false
- Expand Tree: false
- Link Tree Style: Links in Alphabetic Order
- base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- camera_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- hokuyo_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- imu_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_caster_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_wheel:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- payload_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_caster_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_wheel:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- velodyne_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- Name: RobotModel
- Robot Description: robot_description
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- - Class: rviz/MarkerArray
- Enabled: true
- Marker Topic: /visualization_marker
- Name: MarkerArray
- Namespaces:
- {}
- Queue Size: 100
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/LaserScan
- Color: 255; 0; 0
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: Velodyne2D
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 7
- Size (m): 0.10000000149011612
- Style: Points
- Topic: /scan
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: true
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/LaserScan
- Color: 255; 255; 255
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: CameraScan
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 3
- Size (m): 0.10000000149011612
- Style: Flat Squares
- Topic: /camera_scan
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: true
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/LaserScan
- Color: 170; 255; 0
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: HokuyoScan
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 3
- Size (m): 0.009999999776482582
- Style: Points
- Topic: /hokuyo_scan
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: true
- Value: true
- - Alpha: 0.699999988079071
- Class: rviz/Map
- Color Scheme: costmap
- Draw Behind: false
- Enabled: true
- Name: GlobalCostmap
- Topic: /move_base/global_costmap/costmap
- Unreliable: false
- Use Timestamp: false
- Value: true
- - Alpha: 0.699999988079071
- Class: rviz/Map
- Color Scheme: costmap
- Draw Behind: false
- Enabled: true
- Name: LocalCostmap
- Topic: /move_base/local_costmap/costmap
- Unreliable: false
- Use Timestamp: false
- Value: true
- - Alpha: 0.699999988079071
- Class: rviz/Map
- Color Scheme: map
- Draw Behind: true
- Enabled: true
- Name: LocalCostmapArea
- Topic: /move_base/local_costmap/costmap
- Unreliable: false
- Use Timestamp: false
- Value: true
- - Alpha: 1
- Class: rviz/Polygon
- Color: 25; 255; 0
- Enabled: true
- Name: Footprint
- Queue Size: 10
- Topic: /move_base/global_costmap/footprint
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 255; 247; 0
- Enabled: true
- Head Diameter: 0.30000001192092896
- Head Length: 0.20000000298023224
- Length: 0.30000001192092896
- Line Style: Lines
- Line Width: 0.029999999329447746
- Name: GlobalPlan
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Queue Size: 10
- Radius: 0.029999999329447746
- Shaft Diameter: 0.10000000149011612
- Shaft Length: 0.10000000149011612
- Topic: /move_base/DWAPlannerROS/global_plan
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 25; 255; 0
- Enabled: true
- Head Diameter: 0.30000001192092896
- Head Length: 0.20000000298023224
- Length: 0.30000001192092896
- Line Style: Billboards
- Line Width: 0.07999999821186066
- Name: LocalPlan
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Queue Size: 10
- Radius: 0.029999999329447746
- Shaft Diameter: 0.10000000149011612
- Shaft Length: 0.10000000149011612
- Topic: /move_base/DWAPlannerROS/local_plan
- Unreliable: false
- Value: true
- - Alpha: 1
- Axes Length: 1
- Axes Radius: 0.10000000149011612
- Class: rviz/Pose
- Color: 255; 25; 0
- Enabled: true
- Head Length: 0.30000001192092896
- Head Radius: 0.10000000149011612
- Name: LocalGoal
- Queue Size: 10
- Shaft Length: 1
- Shaft Radius: 0.05000000074505806
- Shape: Arrow
- Topic: /move_base_simple/goal
- Unreliable: false
- Value: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Default Light: true
- Fixed Frame: map
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz/Interact
- Hide Inactive Objects: true
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/SetInitialPose
- Theta std deviation: 0.2617993950843811
- Topic: /initialpose
- X std deviation: 0.5
- Y std deviation: 0.5
- - Class: rviz/SetGoal
- Topic: /move_base_simple/goal
- - Class: rviz/PublishPoint
- Single click: true
- Topic: /clicked_point
- Value: true
- Views:
- Current:
- Class: rviz/ThirdPersonFollower
- Distance: 14.281959533691406
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Field of View: 0.7853981852531433
- Focal Point:
- X: -0.9902782440185547
- Y: 0.6635769605636597
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 1.5697963237762451
- Target Frame: base_link
- Yaw: 2.815399646759033
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 975
- Hide Left Dock: false
- Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd00000004000000000000015600000374fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000374000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e0000037a000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005b30000003efc0100000002fb0000000800540069006d00650000000000000005b30000046900fffffffb0000000800540069006d00650100000000000004500000000000000000000004570000037400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: true
- Width: 1459
- X: -1
- Y: -50
diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp
index 8505937..5181e03 100644
--- a/waypoint_saver/src/waypoint_saver.cpp
+++ b/waypoint_saver/src/waypoint_saver.cpp
@@ -32,10 +32,11 @@
}
+
void waypointsJoyCallback(const sensor_msgs::Joy &msg)
{
static ros::Time saved_time(0.0);
- if(msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0) {
+ if (msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0) {
tf::StampedTransform robot_gl;
try {
tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
@@ -54,6 +55,7 @@
}
+
void waypointsVizCallback(const geometry_msgs::PointStamped &msg)
{
Waypoint point;
@@ -66,51 +68,19 @@
}
+
void finishPoseCallback(const geometry_msgs::PoseStamped &msg)
{
finish_pose_ = msg;
- save2();
+ save();
waypoints_.clear();
}
-
+
+
void save()
{
std::ofstream ofs(filename_.c_str(), std::ios::out);
-
- ofs << "waypoints:" << std::endl;
- for(int i=0; i < waypoints_.size(); i++) {
- ofs << " " << "- point:" << std::endl;
- ofs << " x: " << waypoints_[i].point.x << std::endl;
- ofs << " y: " << waypoints_[i].point.y << std::endl;
- ofs << " z: " << waypoints_[i].point.z << std::endl;
- }
-
- ofs << "finish_pose:" << std::endl;
- ofs << " header:" << std::endl;
- ofs << " seq: " << finish_pose_.header.seq << std::endl;
- ofs << " stamp: " << finish_pose_.header.stamp << std::endl;
- ofs << " frame_id: " << finish_pose_.header.frame_id << std::endl;;
- ofs << " pose:" << std::endl;
- ofs << " position:" << std::endl;
- ofs << " x: " << finish_pose_.pose.position.x << std::endl;
- ofs << " y: " << finish_pose_.pose.position.y << std::endl;
- ofs << " z: " << finish_pose_.pose.position.z << std::endl;
- ofs << " orientation:" << std::endl;
- ofs << " x: " << finish_pose_.pose.orientation.x << std::endl;
- ofs << " y: " << finish_pose_.pose.orientation.y << std::endl;
- ofs << " z: " << finish_pose_.pose.orientation.z << std::endl;
- ofs << " w: " << finish_pose_.pose.orientation.w << std::endl;
-
- ofs.close();
-
- ROS_INFO_STREAM("write success");
- }
-
-
- void save2()
- {
- std::ofstream ofs(filename_.c_str(), std::ios::out);
/*
waypoints:
- point: {x: *, y: *, z: *, vel: *, rad: *}
@@ -118,11 +88,13 @@
ofs << "waypoints:" << std::endl;
for(int i=0; i < waypoints_.size(); i++) {
ofs << "- point: {" ;
- ofs << "x: " << waypoints_[i].get_x() << ", " ;
- ofs << "y: " << waypoints_[i].get_y() << ", " ;
- ofs << "z: " << waypoints_[i].get_z() << ", " ;
- ofs << "vel: " << waypoints_[i].get_vel() << ", " ;
- ofs << "rad: " << waypoints_[i].get_rad() << "}" << std::endl;
+ ofs << "x: " << waypoints_[i].get_x() << ", " ;
+ ofs << "y: " << waypoints_[i].get_y() << ", " ;
+ ofs << "z: " << waypoints_[i].get_z() << ", " ;
+ ofs << "vel: " << waypoints_[i].get_vel() << ", " ;
+ ofs << "rad: " << waypoints_[i].get_rad() << ", " ;
+ ofs << "stop: " << waypoints_[i].get_stop() << "}" ;
+ ofs << std::endl;
}
/*
finish_pose:
@@ -152,12 +124,14 @@
}
+
void run()
{
ros::spin();
}
+
private:
ros::Subscriber waypoints_viz_sub_;
ros::Subscriber waypoints_joy_sub_;
@@ -175,6 +149,8 @@
+
+
int main(int argc, char *argv[])
{
ros::init(argc, argv, "waypoints_saver");