From 99dd1e1236d58a4ee9c93de7124afd83b2f2c141 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sat, 24 Oct 2020 14:31:19 +0200
Subject: [PATCH] Added full_coverage_path_planner launch file

---
 CMakeLists.txt                           | 13 ++++++++-----
 config/global_costmap_params.yaml        |  3 ++-
 launch/full_coverage_path_planner.launch | 23 +++++++++++++++++++++++
 launch/move_base.launch                  |  5 ++++-
 www/assets/javascripts/application.js    |  6 +++---
 www/index.html                           |  2 +-
 6 files changed, 41 insertions(+), 11 deletions(-)
 create mode 100644 launch/full_coverage_path_planner.launch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 965b4ee..999926a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(wild_thumper)
+EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE )
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -109,7 +110,6 @@ include_directories(
 
 ## Declare a cpp executable
 add_executable(path_following src/path_following.cpp)
-add_executable(wt_node src/wt_node.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -120,10 +120,13 @@ target_link_libraries(path_following
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
 )
-target_link_libraries(wt_node
-   ${catkin_LIBRARIES}
-   i2c
-)
+if(${ARCHITECTURE} EQUAL "armv7l")
+	add_executable(wt_node src/wt_node.cpp)
+	target_link_libraries(wt_node
+	   ${catkin_LIBRARIES}
+	   i2c
+	)
+endif()
 
 #############
 ## Install ##
diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml
index d44e361..332d533 100644
--- a/config/global_costmap_params.yaml
+++ b/config/global_costmap_params.yaml
@@ -4,7 +4,7 @@ global_costmap:
   publish_frequency: 1.0
   plugins:
   - {name: static_layer, type: 'costmap_2d::StaticLayer'}
-  #- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
+  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
 
   inflation_layer:
@@ -13,3 +13,4 @@ global_costmap:
   obstacle_layer:
     combination_method: 0 # overwrite
     track_unknown_space: true
+    enabled: false
diff --git a/launch/full_coverage_path_planner.launch b/launch/full_coverage_path_planner.launch
new file mode 100644
index 0000000..805ab3c
--- /dev/null
+++ b/launch/full_coverage_path_planner.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="coverage_area_offset" default="-5.0 -5.0 0 0 0 0"/>
+	<arg name="coverage_area_size_x" default="10"/>
+	<arg name="coverage_area_size_y" default="10"/>
+	<arg name="tool_radius" default="0.175"/>
+
+	<include file="$(find wild_thumper)/launch/move_base.launch">
+		<arg name="base_global_planner" value="full_coverage_path_planner/SpiralSTC"/>
+		<arg name="base_local_planner" value="pose_follower/PoseFollower"/>
+		<arg name="tool_radius" value="$(arg tool_radius)"/>
+	</include>
+
+	<!-- Launch coverage progress tracking -->
+	<node pkg="tf" type="static_transform_publisher" name="map_to_coveragemap" args="$(arg coverage_area_offset) map coverage_map 100" />
+	<node pkg="full_coverage_path_planner" type="coverage_progress" name="coverage_progress">
+		<param name="~target_area/x" value="$(arg coverage_area_size_x)" />
+		<param name="~target_area/y" value="$(arg coverage_area_size_y)" />
+		<param name="~coverage_radius" value="$(arg tool_radius)" />
+		<remap from="reset" to="coverage_progress/reset" />
+		<param name="~map_frame" value="/coverage_map"/>
+	</node>
+</launch>
diff --git a/launch/move_base.launch b/launch/move_base.launch
index 33c5636..70974dc 100644
--- a/launch/move_base.launch
+++ b/launch/move_base.launch
@@ -5,10 +5,11 @@
 	<arg name="global_odom" default="false" />
 	<arg name="close_to_obstacles" default="false" />
 	<arg name="nomap" default="false" />
-	<arg name="base_global_planner" default="navfn/NavfnROS"/> <!-- navfn/NavfnROS || carrot_planner/CarrotPlanner -->
+	<arg name="base_global_planner" default="navfn/NavfnROS"/> <!-- navfn/NavfnROS || carrot_planner/CarrotPlanner || full_coverage_path_planner/SpiralSTC -->
 	<arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/> <!-- base_local_planner/TrajectoryPlannerROS || dwa_local_planner/DWAPlannerROS || pose_follower/PoseFollower -->
 	<!-- default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]" -->
 	<arg name="recovery_behaviors" default="[{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]"/>
+	<arg name="tool_radius" default="0.175"/>
 
 	<!-- Run AMCL -->
 	<group unless="$(arg nomap)">
@@ -50,6 +51,8 @@
 		<param name="conservative_reset/affected_maps" value="local" />
 		<param name="make_plan_clear_costmap" value="false" />
 		<param name="make_plan_add_unreachable_goal" value="false" />
+		<param name="SpiralSTC/robot_radius" value="0.23" />
+		<param name="SpiralSTC/tool_radius" value="$(arg tool_radius)" />
 		<rosparam param="recovery_behaviors" subst_value="True">$(arg recovery_behaviors)</rosparam>
 		<rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
 		<rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
diff --git a/www/assets/javascripts/application.js b/www/assets/javascripts/application.js
index 1a61324..02dfe59 100644
--- a/www/assets/javascripts/application.js
+++ b/www/assets/javascripts/application.js
@@ -17,9 +17,9 @@ function init() {
 
 	ros.on('close', function() {
 		information.alerts.push({message: "Connection to websocket server closed.", info: true});
-		setTimeout(function() {
-			connect();
-		}, 2000);
+		//setTimeout(function() {
+		//	connect();
+		//}, 2000);
 	});
 
 	//tfClient.subscribe('base_link', function(tf) {
diff --git a/www/index.html b/www/index.html
index 15e2aed..8202c20 100644
--- a/www/index.html
+++ b/www/index.html
@@ -22,7 +22,7 @@
 		<script src="assets/javascripts/vue.js"></script>
 		<script src="assets/javascripts/application.js"></script>
 		<script type="text/javascript">
-			const robothostname="wildthumper"
+			const robothostname="localhost"
 		</script>
 
 		<title>Wild Thumper control</title>
-- 
2.39.5