diff --git a/waypoint_nav/rviz_cfg/record_waypoints.rviz b/waypoint_nav/rviz_cfg/record_waypoints.rviz deleted file mode 100644 index bfed1a6..0000000 --- a/waypoint_nav/rviz_cfg/record_waypoints.rviz +++ /dev/null @@ -1,139 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 435 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.7 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /map - Value: true - - Alpha: 1 - Class: rviz/PointStamped - Color: 204; 41; 204 - Enabled: true - History Length: 1 - Name: PointStamped - Radius: 0.2 - Topic: /clicked_point - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.1 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.3 - Head Radius: 0.1 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05 - Shape: Arrow - Topic: /move_base_simple/goal - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - 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 - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 38.8507 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.403598 - Y: 0.599761 - Z: 0.253224 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.775398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.675416 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 716 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000013c00000242fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000242000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000036e0000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1200 - X: 196 - Y: 79 diff --git a/waypoint_nav/waypoints_cfg/waypoints_test.yaml b/waypoint_nav/waypoints_cfg/waypoints_test.yaml new file mode 100644 index 0000000..713cc07 --- /dev/null +++ b/waypoint_nav/waypoints_cfg/waypoints_test.yaml @@ -0,0 +1,13 @@ +waypoints: +- point: {x: -1.01321, y: -0.528956, z: -0.000870705, vel: 1, rad: 0.8, stop: 0} +- point: {x: -0.0284393, y: -0.471859, z: 0.00520468, vel: 1, rad: 0.8, stop: 0} +- point: {x: 0.981428, y: -0.5452, z: 0.00261879, vel: 1, rad: 0.8, stop: 0} +- point: {x: 1.90456, y: -0.536331, z: 0.00246334, vel: 1, rad: 0.8, stop: 0} +- point: {x: 1.95224, y: 0.538929, z: 0.00476408, vel: 1, rad: 0.8, stop: 0} +- point: {x: 0.603558, y: 0.551442, z: 0.00079298, vel: 1, rad: 0.8, stop: 0} +- point: {x: 0.539775, y: 1.787, z: 0.00275993, vel: 1, rad: 0.8, stop: 0} +finish_pose: + header: {seq: 0, stamp: 244.465000000, frame_id: map} + pose: + position: {x: -0.488029, y: 1.82672, z: 0} + orientation: {x: 0, y: 0, z: 0.999992, w: 0.00399584} diff --git a/waypoint_saver/rviz_cfg/record_waypoints.rviz b/waypoint_saver/rviz_cfg/record_waypoints.rviz new file mode 100644 index 0000000..71a0db5 --- /dev/null +++ b/waypoint_saver/rviz_cfg/record_waypoints.rviz @@ -0,0 +1,240 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 418 + - 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: LaserScan +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: 10 + 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 + - 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_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_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 + - 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: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /waypoints + Name: Waypoints + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /clicked_point + 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: Pose + 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/Orbit + Distance: 9.116127014160156 + 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.43540072441101074 + Y: -0.04363036900758743 + Z: 0.47935307025909424 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.179796576499939 + Target Frame: + Yaw: 3.1354191303253174 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 716 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000022dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e0000022d000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026c00fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000022d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 195 + Y: 59 diff --git a/waypoint_saver/rviz_cfg/saver.rviz b/waypoint_saver/rviz_cfg/saver.rviz deleted file mode 100644 index c337ca9..0000000 --- a/waypoint_saver/rviz_cfg/saver.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 b7e2de7..6d6f7c4 100644 --- a/waypoint_saver/src/waypoint_saver.cpp +++ b/waypoint_saver/src/waypoint_saver.cpp @@ -40,23 +40,10 @@ { static ros::Time saved_time(0.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); - Waypoint point; - point.set_x(robot_gl.getOrigin().x()); - point.set_y(robot_gl.getOrigin().y()); - point.set_z(robot_gl.getOrigin().z()); - point.set_rad(default_rad_); - waypoints_.push_back(point); - addWaypointMarker(point); - saved_time = ros::Time::now(); - } - catch(tf::TransformException &e) { - ROS_WARN_STREAM("tf::TransformException: " << e.what()); - } + pushbackWaypoint(); + saved_time = ros::Time::now(); } - markers_pub_.publish(marker_array_); + publishMarkerArray(); } @@ -70,6 +57,8 @@ point.set_rad(default_rad_); ROS_INFO_STREAM("point = " << point); waypoints_.push_back(point); + addWaypointMarker(point); + publishMarkerArray(); } @@ -80,6 +69,26 @@ save(); waypoints_.clear(); } + + + + void pushbackWaypoint() + { + tf::StampedTransform robot_gl; + try { + tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl); + Waypoint point; + point.set_x(robot_gl.getOrigin().x()); + point.set_y(robot_gl.getOrigin().y()); + point.set_z(robot_gl.getOrigin().z()); + point.set_rad(default_rad_); + waypoints_.push_back(point); + addWaypointMarker(point); + } + catch(tf::TransformException &e) { + ROS_WARN_STREAM("tf::TransformException: " << e.what()); + } + } @@ -108,7 +117,16 @@ marker.color.b=0.2f; marker.color.a = 1.0f; - marker_array_.markers.push_back(marker); + markers_.push_back(marker); + } + + + + void publishMarkerArray() + { + visualization_msgs::MarkerArray marker_array; + marker_array.markers = markers_; + markers_pub_.publish(marker_array); } @@ -173,7 +191,8 @@ ros::Subscriber finish_pose_sub_; ros::Publisher markers_pub_; std::vector waypoints_; - visualization_msgs::MarkerArray marker_array_; + // visualization_msgs::MarkerArray marker_array_; + std::vector markers_; geometry_msgs::PoseStamped finish_pose_; tf::TransformListener tf_listener_; int save_joy_button_;