Skip to content

Commit ae79feb

Browse files
ethanckimethanckim
committed
move base sim
1 parent 733a8e8 commit ae79feb

File tree

4 files changed

+113
-51
lines changed

4 files changed

+113
-51
lines changed

‎maps/square.png

3.17 KB
Loading

‎test/full_coverage_path_planner/fcpp.rviz

Lines changed: 46 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ Panels:
55
Property Tree Widget:
66
Expanded: ~
77
Splitter Ratio: 0.5
8-
Tree Height: 554
8+
Tree Height: 548
99
- Class: rviz/Selection
1010
Name: Selection
1111
- Class: rviz/Tool Properties
@@ -62,6 +62,7 @@ Visualization Manager:
6262
Value: true
6363
odom:
6464
Value: true
65+
Marker Alpha: 1
6566
Marker Scale: 1
6667
Name: TF
6768
Show Arrows: true
@@ -86,6 +87,14 @@ Visualization Manager:
8687
Unreliable: false
8788
Use Timestamp: false
8889
Value: true
90+
- Class: rviz/Marker
91+
Enabled: true
92+
Marker Topic: /path_coverage_marker
93+
Name: Marker
94+
Namespaces:
95+
{}
96+
Queue Size: 100
97+
Value: true
8998
- Alpha: 1
9099
Buffer Length: 1
91100
Class: rviz/Path
@@ -103,10 +112,35 @@ Visualization Manager:
103112
Z: 0
104113
Pose Color: 255; 85; 255
105114
Pose Style: Arrows
115+
Queue Size: 10
106116
Radius: 0.029999999329447746
107117
Shaft Diameter: 0.019999999552965164
108118
Shaft Length: 0.05000000074505806
109-
Topic: /move_base/SpiralSTC/plan
119+
Topic: /move_base/TrajectoryPlannerROS/global_plan
120+
Unreliable: false
121+
Value: true
122+
- Alpha: 1
123+
Buffer Length: 1
124+
Class: rviz/Path
125+
Color: 25; 255; 0
126+
Enabled: true
127+
Head Diameter: 0.30000001192092896
128+
Head Length: 0.20000000298023224
129+
Length: 0.30000001192092896
130+
Line Style: Lines
131+
Line Width: 0.029999999329447746
132+
Name: Path
133+
Offset:
134+
X: 0
135+
Y: 0
136+
Z: 0
137+
Pose Color: 255; 85; 255
138+
Pose Style: None
139+
Queue Size: 10
140+
Radius: 0.029999999329447746
141+
Shaft Diameter: 0.10000000149011612
142+
Shaft Length: 0.10000000149011612
143+
Topic: /move_base/TrajectoryPlannerROS/local_plan
110144
Unreliable: false
111145
Value: true
112146
- Angle Tolerance: 0.10000000149011612
@@ -130,6 +164,7 @@ Visualization Manager:
130164
Keep: 10000
131165
Name: Odometry
132166
Position Tolerance: 0.10000000149011612
167+
Queue Size: 10
133168
Shape:
134169
Alpha: 1
135170
Axes Length: 1
@@ -143,50 +178,16 @@ Visualization Manager:
143178
Topic: /odom
144179
Unreliable: false
145180
Value: true
146-
- Class: rviz/Marker
147-
Enabled: true
148-
Marker Topic: /visualization_marker
149-
Name: Tracking_pid local planner
150-
Namespaces:
151-
axle point: true
152-
control point: true
153-
goal point: true
154-
Queue Size: 100
155-
Value: true
156-
- Class: rviz/Marker
157-
Enabled: true
158-
Marker Topic: /move_base_flex/tracking_pid/interpolator
159-
Name: Interpolator (mbf)
160-
Namespaces:
161-
{}
162-
Queue Size: 100
163-
Value: true
164181
- Alpha: 0.30000001192092896
165182
Class: rviz/Map
166183
Color Scheme: map
167184
Draw Behind: false
168-
Enabled: true
185+
Enabled: false
169186
Name: Coverage progress map
170187
Topic: /coverage_grid
171188
Unreliable: false
172189
Use Timestamp: false
173-
Value: true
174-
- Class: rviz/Marker
175-
Enabled: true
176-
Marker Topic: /move_base/TrackingPidLocalPlanner/interpolator
177-
Name: Interpolator (mb)
178-
Namespaces:
179-
{}
180-
Queue Size: 100
181-
Value: true
182-
- Alpha: 1
183-
Class: rviz/Polygon
184-
Color: 25; 255; 0
185-
Enabled: true
186-
Name: Footprint
187-
Topic: /move_base_flex/global_costmap/footprint
188-
Unreliable: false
189-
Value: true
190+
Value: false
190191
Enabled: true
191192
Global Options:
192193
Background Color: 48; 48; 48
@@ -224,19 +225,18 @@ Visualization Manager:
224225
Invert Z Axis: false
225226
Name: Current View
226227
Near Clip Distance: 0.009999999776482582
227-
Scale: 97.19247436523438
228+
Scale: 107.97452545166016
228229
Target Frame: <Fixed Frame>
229-
Value: TopDownOrtho (rviz)
230-
X: -0.14492475986480713
231-
Y: 1.0135213136672974
230+
X: -0.3638369143009186
231+
Y: 0.05843987315893173
232232
Saved: ~
233233
Window Geometry:
234234
Displays:
235235
collapsed: false
236-
Height: 1023
236+
Height: 1016
237237
Hide Left Dock: false
238238
Hide Right Dock: true
239-
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000361fc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a60000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e9000002b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
239+
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035afc0200000008fb0000001200530065006c0065006300740069006f006e010000003d000000a50000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000e8000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003830000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
240240
Selection:
241241
collapsed: false
242242
Time:
@@ -245,6 +245,6 @@ Window Geometry:
245245
collapsed: true
246246
Views:
247247
collapsed: true
248-
Width: 927
249-
X: 67
248+
Width: 1267
249+
X: 72
250250
Y: 27

‎test/full_coverage_path_planner/move_base_sim.launch

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
<arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
5+
<arg name="coverage_area_offset" default="-2.5 -2.5 0 0 0 0"/>
6+
<arg name="coverage_area_size_x" default="10"/>
7+
<arg name="coverage_area_size_y" default="10"/>
8+
<arg name="target_x_vel" default="0.5"/>
9+
<arg name="target_yaw_vel" default="0.4"/>
10+
<arg name="robot_radius" default="0.3"/>
11+
<arg name="tool_radius" default="0.3"/>
12+
<arg name="rviz" default="true"/>
13+
14+
<!-- Mobile robot simulator -->
15+
<node pkg="mobile_robot_simulator" type="mobile_robot_simulator_node" name="mobile_robot_simulator" output="screen">
16+
<param name="publish_map_transform" value="true"/>
17+
<param name="publish_rate" value="10.0"/>
18+
<param name="velocity_topic" value="/cmd_vel"/>
19+
<param name="odometry_topic" value="/odom"/>
20+
</node>
21+
22+
<!--Run the map server-->
23+
<node name="grid_server" pkg="map_server" type="map_server" args="$(arg map)">
24+
<param name="frame_id" value="map"/>
25+
</node>
26+
27+
<!--Tracking_pid cannot just accept a nav_msgs/Path, it can only go to a PoseStamped,
28+
so the path_interpolator drags a PoseStamped over a Path at a given speed-->
29+
<node name="interpolator" pkg="tracking_pid" type="path_interpolator">
30+
<param name="target_x_vel" value="$(arg target_x_vel)"/>
31+
<param name="target_yaw_vel" value="$(arg target_yaw_vel)"/>
32+
<!-- <remap from="path" to="/move_base/SpiralSTC/plan"/> -->
33+
</node>
34+
35+
<!--Tracking_pid tries to get the robot as close to it's goal point as possible-->
36+
<node name="controller" pkg="tracking_pid" type="controller" output="screen">
37+
<remap from="move_base/cmd_vel" to="/cmd_vel"/>
38+
<remap from="local_trajectory" to="trajectory"/>
39+
<param name="controller_debug_enabled" value="True"/>
40+
<param name="track_base_link" value="true"/>
41+
<param name="l" value="0.5"/>
42+
<param name="Ki_long" value="0.0"/>
43+
<param name="Kp_long" value="2.0"/>
44+
<param name="Kd_long" value="0.5"/>
45+
<param name="Ki_lat" value="0.0"/>
46+
<param name="Kp_lat" value="4.0"/>
47+
<param name="Kd_lat" value="0.3"/>
48+
</node>
49+
50+
<!-- Launch coverage progress tracking -->
51+
<node pkg="tf" type="static_transform_publisher" name="map_to_coveragemap" args="$(arg coverage_area_offset) map coverage_map 100" />
52+
<node pkg="full_coverage_path_planner" type="coverage_progress" name="coverage_progress">
53+
<param name="~target_area/x" value="$(arg coverage_area_size_x)" />
54+
<param name="~target_area/y" value="$(arg coverage_area_size_y)" />
55+
<param name="~coverage_radius" value="$(arg tool_radius)" />
56+
<remap from="reset" to="coverage_progress/reset" />
57+
<param name="~map_frame" value="/coverage_map"/>
58+
</node>
59+
60+
<!-- rviz -->
61+
<node if="$(eval rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find full_coverage_path_planner)/test/full_coverage_path_planner/fcpp.rviz" />
62+
</launch>

‎test/full_coverage_path_planner/test_full_coverage_path_planner.launch

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,15 @@
1414
<!--Move base flex, using the full_coverage_path_planner-->
1515
<node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen" required="true">
1616
<param name="tf_timeout" value="1.5"/>
17-
<rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/planners.yaml" command="load" />
18-
<rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/local_costmap_params.yaml" command="load" />
17+
<!-- <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/planners.yaml" command="load" />
18+
<rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/local_costmap_params.yaml" command="load" /> -->
1919
<param name="SpiralSTC/robot_radius" value="$(arg robot_radius)"/>
2020
<param name="SpiralSTC/tool_radius" value="$(arg tool_radius)"/>
2121
<param name="global_costmap/robot_radius" value="$(arg robot_radius)"/>
22-
<remap from="odom" to="/odom"/>
23-
<remap from="scan" to="/scan"/>
22+
<!-- <remap from="odom" to="/odom"/>
23+
<remap from="scan" to="/scan"/> -->
2424

25-
<remap from="/move_base_flex/SpiralSTC/plan" to="/move_base/SpiralSTC/plan"/>
25+
<!-- <remap from="/move_base_flex/SpiralSTC/plan" to="/move_base/SpiralSTC/plan"/> -->
2626
<remap from="/move_base_flex/tracking_pid/interpolator" to="/move_base/TrackingPidLocalPlanner/interpolator"/>
2727
</node>
2828

0 commit comments

Comments
 (0)