Skip to content

Commit 44447e2

Browse files
SteveMacenskiSteveMacenski
committed
final changes for the day after call
1 parent 58160b8 commit 44447e2

File tree

4 files changed

+48
-44
lines changed

4 files changed

+48
-44
lines changed

‎nav2_coverage/README.md

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,6 @@ If you use this work, please make sure to cite both Nav2 and Fields2Cover:
4646

4747
## Notes of Wisdom
4848

49-
TODO visualize dynamic param
50-
5149
Walk through
5250
- Lifecycle-Component-Action Task Server like you expect in Nav2
5351
- Fully parameterized with dynamic parameters to easily test / tune
@@ -77,5 +75,12 @@ Future
7775

7876
Q
7977
- Demo just have it follow the nav path on a coverage pattern, or something else more complex (or leave to you to determine?)
78+
--> just normal path it up
8079
- Other F2C add on features you care about? --> designed to expand or contribute back to F2C
80+
- headland pass!
81+
- inner boundary not to go through (...) can wait, firefly might do.
82+
83+
84+
Leader follower 2-3 months out to kick off
85+
a bunch of other stuff --> projects interest me. open nav hire to fill!
8186

‎nav2_coverage/include/nav2_coverage/visualizer.hpp

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -44,10 +44,6 @@ class Visualizer
4444
template<typename NodeT>
4545
void activate(NodeT node)
4646
{
47-
nav2_util::declare_parameter_if_not_declared(
48-
node, "visualize", rclcpp::ParameterValue(true));
49-
visualize_ = node->get_parameter("visualize").as_bool();
50-
5147
nav_plan_pub_ = rclcpp::create_publisher<nav_msgs::msg::Path>(
5248
node->get_node_topics_interface(),
5349
"coverage_server/coverage_plan", rclcpp::QoS(1));
@@ -69,7 +65,6 @@ class Visualizer
6965
const std::shared_ptr<ComputeCoveragePath::Result> & result,
7066
const std_msgs::msg::Header & header);
7167

72-
bool visualize_;
7368
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr nav_plan_pub_;
7469
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr headlands_pub_;
7570
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr planning_field_pub_;

‎nav2_coverage/src/visualizer.cpp

Lines changed: 37 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -34,50 +34,54 @@ void Visualizer::visualize(
3434
const std::shared_ptr<ComputeCoveragePath::Result> & result,
3535
const std_msgs::msg::Header & header)
3636
{
37-
if (!visualize_) {
38-
return;
39-
}
40-
4137
// Visualize coverage path
42-
nav_plan_pub_->publish(std::move(std::make_unique<nav_msgs::msg::Path>(result->nav_path)));
38+
if (nav_plan_pub_->get_subscription_count() > 0) {
39+
nav_plan_pub_->publish(std::move(std::make_unique<nav_msgs::msg::Path>(result->nav_path)));
40+
}
4341

4442
// Visualize field boundary
45-
auto field_polygon = std::make_unique<geometry_msgs::msg::PolygonStamped>();
46-
field_polygon->header = header;
47-
Polygon boundary = total_field.getGeometry(0); // Only outer-most polygon boundary
48-
for (unsigned int i = 0; i != boundary.size(); i++) {
49-
field_polygon->polygon.points.push_back(util::toMsg(boundary.getGeometry(i)));
43+
if (headlands_pub_->get_subscription_count() > 0) {
44+
auto field_polygon = std::make_unique<geometry_msgs::msg::PolygonStamped>();
45+
field_polygon->header = header;
46+
Polygon boundary = total_field.getGeometry(0); // Only outer-most polygon boundary
47+
for (unsigned int i = 0; i != boundary.size(); i++) {
48+
field_polygon->polygon.points.push_back(util::toMsg(boundary.getGeometry(i)));
49+
}
50+
headlands_pub_->publish(std::move(field_polygon));
5051
}
51-
headlands_pub_->publish(std::move(field_polygon));
5252

5353
// Visualize field for planning (after headland removed)
54-
auto headlandless_polygon = std::make_unique<geometry_msgs::msg::PolygonStamped>();
55-
headlandless_polygon->header = header;
56-
Polygon planning_field = no_headland_field.getGeometry(0); // Only outer-most polygon boundary
57-
for (unsigned int i = 0; i != planning_field.size(); i++) {
58-
headlandless_polygon->polygon.points.push_back(util::toMsg(planning_field.getGeometry(i)));
54+
if (planning_field_pub_->get_subscription_count() > 0) {
55+
auto headlandless_polygon = std::make_unique<geometry_msgs::msg::PolygonStamped>();
56+
headlandless_polygon->header = header;
57+
Polygon planning_field = no_headland_field.getGeometry(0); // Only outer-most polygon boundary
58+
for (unsigned int i = 0; i != planning_field.size(); i++) {
59+
headlandless_polygon->polygon.points.push_back(util::toMsg(planning_field.getGeometry(i)));
60+
}
61+
planning_field_pub_->publish(std::move(headlandless_polygon));
5962
}
60-
planning_field_pub_->publish(std::move(headlandless_polygon));
6163

6264
// Visualize swaths alone
63-
auto output_swaths = std::make_unique<visualization_msgs::msg::Marker>();
64-
output_swaths->header = header;
65-
output_swaths->action = visualization_msgs::msg::Marker::ADD;
66-
output_swaths->type = visualization_msgs::msg::Marker::LINE_LIST;
67-
output_swaths->pose.orientation.w = 1.0;
68-
output_swaths->scale.x = 0.3;
69-
output_swaths->scale.y = 0.3;
70-
output_swaths->scale.z = 0.3;
71-
output_swaths->color.b = 1.0;
72-
output_swaths->color.a = 1.0;
65+
if (swaths_pub_->get_subscription_count() > 0) {
66+
auto output_swaths = std::make_unique<visualization_msgs::msg::Marker>();
67+
output_swaths->header = header;
68+
output_swaths->action = visualization_msgs::msg::Marker::ADD;
69+
output_swaths->type = visualization_msgs::msg::Marker::LINE_LIST;
70+
output_swaths->pose.orientation.w = 1.0;
71+
output_swaths->scale.x = 0.3;
72+
output_swaths->scale.y = 0.3;
73+
output_swaths->scale.z = 0.3;
74+
output_swaths->color.b = 1.0;
75+
output_swaths->color.a = 1.0;
7376

74-
for (unsigned int i = 0; i != result->coverage_path.swaths.size(); i++) {
75-
auto & swath = result->coverage_path.swaths[i];
76-
output_swaths->points.push_back(util::pointToPoint32(swath.start));
77-
output_swaths->points.push_back(util::pointToPoint32(swath.end));
78-
}
77+
for (unsigned int i = 0; i != result->coverage_path.swaths.size(); i++) {
78+
auto & swath = result->coverage_path.swaths[i];
79+
output_swaths->points.push_back(util::pointToPoint32(swath.start));
80+
output_swaths->points.push_back(util::pointToPoint32(swath.end));
81+
}
7982

80-
swaths_pub_->publish(std::move(output_swaths));
83+
swaths_pub_->publish(std::move(output_swaths));
84+
}
8185
}
8286

8387
} // namespace nav2_coverage

‎nav2_coverage/test/tester.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -139,18 +139,18 @@ def main():
139139
# goal.headland_mode.mode = "CONSTANT"
140140
# goal.headland_mode.width = 5.0
141141

142-
# goal.swath_mode.objective = "LENGTH" # LENGTH, NUMBER, or COVERAGE
142+
# goal.swath_mode.objective = "NUMBER" # LENGTH, NUMBER, or COVERAGE
143143
# goal.swath_mode.mode = "BRUTE_FORCE" # BRUTE_FORCE, SET_ANGLE
144144
# goal.swath_mode.best_angle = 1.57
145145
# goal.swath_mode.step_angle = 1.72e-2
146146

147147
# goal.route_mode.mode = "SPIRAL" # BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM
148-
# goal.route_mode.spiral_n = 8
149-
# goal.route_mode.custom_order =
148+
# goal.route_mode.spiral_n = 3
149+
# # goal.route_mode.custom_order =
150150

151151
# goal.path_mode.mode = "DUBIN" # DUBIN, REEDS_SHEPP
152152
# goal.path_mode.continuity_mode = "CONTINUOUS" # CONTINUOUS, DISCONTINUOUS
153-
# goal.path_mode.turn_point_distance = 0.3
153+
# goal.path_mode.turn_point_distance = 0.1
154154

155155
coverage_info = tester.getCoveragePath(goal)
156156
result = tester.getResult()

0 commit comments

Comments
 (0)