Skip to content

Commit 3af0387

Browse files
ethanckimethanckim
committed
move to verticies prototype
1 parent 733a8e8 commit 3af0387

File tree

6 files changed

+169
-3
lines changed

6 files changed

+169
-3
lines changed

‎maps/square.png

3.17 KB
Loading

‎maps/square.yaml

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
image: square.png
2+
resolution: 0.05
3+
origin: [-5, -5, 0.0]
4+
negate: 0
5+
occupied_thresh: 0.65
6+
free_thresh: 0.196

‎src/spiral_stc.cpp

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,79 @@ std::list<gridNode_t> SpiralSTC::spiral(std::vector<std::vector<bool> > const& g
102102
return pathNodes;
103103
}
104104

105+
106+
// /**
107+
// * Uses a wavefront algorithm to determine the nearest open verticies/corner on the grid.
108+
// * The mountain pattern (bourstrophedon) should idealy begin from the corner of the grid.
109+
// * @param grid internal map representation
110+
// * @param corner_list Output map in which all cells except the corners are marked as visited (true).
111+
// * @param start_x The x position of the corner to begin search
112+
// * @param start_y The y position of the corner to begin search
113+
// */
114+
// void findNearestOpenVerticies(std::vector<std::vector<bool>> const& grid,
115+
// std::vector<std::vector<bool>>& corner_list,
116+
// int start_x,
117+
// int start_y)
118+
// {
119+
// std::queue<std::tuple<int, int>> nodes;
120+
// int bound_x = -1, bound_y = -1;
121+
// bool processed[grid.size()][grid[0].size()] = {false}; // store node indicies which are already processed
122+
// nodes.push({start_x, start_y}); // add first node to queue
123+
124+
// while(!nodes.empty()) {
125+
126+
// // get current node
127+
// int x = std::get<0>(nodes.front());
128+
// int y = std::get<1>(nodes.front());
129+
// nodes.pop();
130+
131+
// // check node
132+
// // To accomodate box case such as:
133+
// // 0000000000000
134+
// // 0011111111100
135+
// // 0011000001100
136+
// // 0011000001100
137+
// // 0011111111100
138+
// // 0000000000000
139+
// if ((bound_x == -1 || bound_y == -1) && grid[x][y] == eNodeVisited) {
140+
// bound_x == x;
141+
// bound_y = y;
142+
// } else {
143+
// if((x < bound_x || y < bound_y) && grid[x][y] == eNodeVisited) {
144+
// bound_x = dmin(x, bound_x);
145+
// bound_y = dmin(y, bound_y);
146+
// }
147+
// if (grid[x][y] == eNodeOpen) {
148+
// if (bound_x == -1 || bound_y == -1) {
149+
// }
150+
// corner_list[x][y] = eNodeOpen;
151+
// return;
152+
// }
153+
// }
154+
155+
156+
// // If node is not open, add unprocessed adjacent nodes to queue
157+
// if (x+1 < grid.size() && processed[x+1][y] == false) {
158+
// nodes.push({x+1, y});
159+
// processed[x+1][y] == true;
160+
// } // East
161+
// if (x-1 > 0 && processed[x-1][y] == false) {
162+
// nodes.push({x-1, y});
163+
// processed[x-1][y] == true;
164+
// } // West
165+
// if (y+1 < grid[0].size() && processed[x][y+1] == false) {
166+
// nodes.push({x, y+1});
167+
// processed[x][y+1] == true;
168+
// } // North
169+
// if (y-1 > 0 && processed[x][y-1] == false) {
170+
// nodes.push({x, y-1});
171+
// processed[x][y-1] == true;
172+
// } // South
173+
174+
// }
175+
176+
// }
177+
105178
std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const& grid,
106179
Point_t& init,
107180
int &multiple_pass_counter,
@@ -135,6 +208,30 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
135208
#endif
136209

137210
pathNodes = SpiralSTC::spiral(grid, pathNodes, visited); // First spiral fill
211+
// std::list<Point_t> map_verticies;
212+
// map_verticies.push_back({0, 0});
213+
// map_verticies.push_back({nCols, 0});
214+
// map_verticies.push_back({nCols, nRows});
215+
// map_verticies.push_back({0, nRows});
216+
// std::vector<std::vector<bool>> corner_list(nCols, std::vector<bool>(nRows, eNodeVisited));
217+
// findNearestOpenVerticies(grid, corner_list, 0, 0);
218+
// findNearestOpenVerticies(grid, corner_list, 0, nRows-1);
219+
// findNearestOpenVerticies(grid, corner_list, nCols-1, 0);
220+
// findNearestOpenVerticies(grid, corner_list, nCols-1, nRows-1);
221+
222+
// for (int i = 0; i < nRows; ++i)
223+
// {
224+
// for (int j = 0; j < nCols; ++j)
225+
// {
226+
// std::cout << grid[i][j] << ' ';
227+
// }
228+
// std::cout << std::endl;
229+
// }
230+
231+
// if (a_star_to_open_space(grid, pathNodes.back(), 1, corner_list, map_verticies, pathNodes)) {
232+
// ROS_INFO("A_star_to_open_space is resigning");
233+
// }
234+
138235
std::list<Point_t> goals = map_2_goals(visited, eNodeOpen); // Retrieve remaining goalpoints
139236
// Add points to full path
140237
std::list<gridNode_t>::iterator it;
@@ -191,6 +288,7 @@ std::list<Point_t> SpiralSTC::spiral_stc(std::vector<std::vector<bool> > const&
191288
#endif
192289

193290
// Spiral fill from current position
291+
// TODO: Convert to U-turn pattern
194292
pathNodes = spiral(grid, pathNodes, visited);
195293

196294
#ifdef DEBUG_PLOT

‎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: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,14 @@
11
<?xml version="1.0"?>
22

33
<launch>
4-
<arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
4+
<arg name="map" default="$(find full_coverage_path_planner)/maps/square.yaml"/>
55
<arg name="coverage_area_offset" default="-2.5 -2.5 0 0 0 0"/>
66
<arg name="coverage_area_size_x" default="10"/>
77
<arg name="coverage_area_size_y" default="10"/>
88
<arg name="target_x_vel" default="0.5"/>
99
<arg name="target_yaw_vel" default="0.4"/>
1010
<arg name="robot_radius" default="0.3"/>
11-
<arg name="tool_radius" default="0.3"/>
11+
<arg name="tool_radius" default="0.2"/>
1212
<arg name="rviz" default="true"/>
1313

1414
<!--Move base flex, using the full_coverage_path_planner-->

‎test/full_coverage_path_planner/test_full_coverage_path_planner_plugin.launch

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22

33
<launch>
4-
<arg name="map" default="$(find full_coverage_path_planner)/maps/grid.yaml"/>
4+
<arg name="map" default="$(find full_coverage_path_planner)/maps/square.yaml"/>
55
<arg name="robot_radius" default="0.3"/>
66
<arg name="tool_radius" default="0.2"/>
77

0 commit comments

Comments
 (0)