Autonomous state machine

.catkin_workspace

@@ -1 +1,2 @@
 # This file currently only serves to mark the location of a catkin workspace for tool integration
+

.devcontainer/devcontainer.json

@@ -41,7 +41,7 @@
 				"ms-python.vscode-pylance",
 				"ms-iot.vscode-ros",
 				"twxs.cmake",
-				"ms-vscode.cmake-tools",				
+				"ms-vscode.cmake-tools",
 				"josetr.cmake-language-support-vscode",
 				"ms-vscode-remote.remote-containers",
 				"ms-vscode-remote.remote-ssh-edit",

.gitignore

@@ -3,7 +3,7 @@ build/
 devel/
 
 # python env
-/venv/
+venv/
 
 # ide
 /.vscode/*
@@ -45,4 +45,4 @@ docs/generated-images
 
 # non-generated documentation images
 !src/wr_logic_teleop_ds/control_scheme_power_levels.png
-!src/wr_logic_teleop_ds/control_scheme_tank_drive.png
+!src/wr_logic_teleop_ds/control_scheme_tank_drive.png

.gitmodules

@@ -1,18 +1,18 @@
 [submodule "src/wroboclaw"]
 	path = src/wroboclaw
-	url = https://github.com/WisconsinRobotics/wroboclaw
+	url = git@github.com:WisconsinRobotics/wroboclaw.git
 [submodule "src/wreadinput"]
 	path = src/wreadinput
-	url = https://github.com/WisconsinRobotics/wreadinput
+	url = git@github.com:WisconsinRobotics/wreadinput.git
 [submodule "src/wready"]
 	path = src/wready
-	url = https://github.com/WisconsinRobotics/wready
+	url = git@github.com:WisconsinRobotics/wready.git
 [submodule "src/wresponsecontrol"]
 	path = src/wresponsecontrol
-	url = https://github.com/WisconsinRobotics/wresponsecontrol
+	url = git@github.com:WisconsinRobotics/wresponsecontrol.git
 [submodule "src/wrplidar_ros"]
 	path = src/wrplidar_ros
-	url = https://github.com/WisconsinRobotics/wrplidar_ros.git
+	url = git@github.com:WisconsinRobotics/wrplidar_ros.git
 [submodule "src/wrevolution"]
 	path = src/wrevolution
-	url = https://github.com/WisconsinRobotics/wrevolution
+	url = git@github.com:WisconsinRobotics/wrevolution.git

.vscode/settings.json

@@ -5,12 +5,14 @@
     ],
     "python.analysis.extraPaths": [
         "./src/wr_hsi_sensing/src",
-        "./src/wr_logic_ai/src"
+        "./src/wr_logic_ai/src",
+        "./src/wr_logic_search/src",
+        "./src/wr_logic_shortrange/src"
     ],
 	"C_Cpp.intelliSenseEngine": "disabled",
     "cmake.sourceDirectory": "${workspaceFolder}/src",
     "[python]": {
         "editor.defaultFormatter": "ms-python.black-formatter"
     },
     "python.formatting.provider": "none"
-}
+}

env_remote.sh

@@ -9,8 +9,10 @@ CATKIN_SHELL=sh
 _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
 . "$_CATKIN_SETUP_DIR/setup.sh"
 
-export ROS_MASTER/_URI='http://wrover.local:11311'
+# export ROS_MASTER_URI='http://wrover.local:11311'
+export ROS_MASTER_URI='http://192.168.2.100:11311'
 export ROS_IP=`ip addr show eth0 | grep -Po 'inet [\d.]+' | awk '{print $2}'`
-export ROS_HOSTNAME='wrover.local'
+# export ROS_HOSTNAME='wrover.local'
+export ROS_HOSTNAME='192.168.2.100'
 
 exec "$@"

mapproxy.yaml

@@ -0,0 +1,63 @@
+# -------------------------------
+# MapProxy example configuration.
+# -------------------------------
+#
+# This is a minimal MapProxy configuration.
+# See full_example.yaml and the documentation for more options.
+#
+
+# Starts the following services:
+# Demo:
+#     http://localhost:8080/demo
+# WMS:
+#     capabilities: http://localhost:8080/service?REQUEST=GetCapabilities
+# WMTS:
+#     capabilities: http://localhost:8080/wmts/1.0.0/WMTSCapabilities.xml
+#     first tile: http://localhost:8080/wmts/osm/webmercator/0/0/0.png
+# Tile service (compatible with OSM/etc.)
+#     first tile: http://localhost:8080/tiles/osm/webmercator/0/0/0.png
+# TMS:
+#     note: TMS is not compatible with OSM/Google Maps/etc.
+#     first tile: http://localhost:8080/tms/1.0.0/osm/webmercator/0/0/0.png
+# KML:
+#     initial doc: http://localhost:8080/kml/osm/webmercator
+
+services:
+  demo:
+  tms:
+    use_grid_names: true
+    # origin for /tiles service
+    origin: 'nw'
+  kml:
+      use_grid_names: true
+  wmts:
+  wms:
+    md:
+      title: MapProxy WMS Proxy
+      abstract: This is a minimal MapProxy example.
+
+layers:
+  - name: gm_layer
+    title: Google Maps tiles
+    sources: [gm_cache]
+
+caches:
+  gm_cache:
+    grids: [gm_grid]
+    sources: [gm]
+    cache:
+      type: file
+      directory_layout: tms
+
+sources:
+  gm:
+    type: tile
+    url: https://mt0.google.com/vt/lyrs=s&hl=en&x=%(x)s&y=%(y)s&z=%(z)s
+    grid: gm_grid
+
+grids:
+    gm_grid:
+        base: GLOBAL_MERCATOR
+        origin: ul
+
+globals:

project.json

@@ -58,6 +58,13 @@
         "package": "ros-noetic-mapviz",
         "provider": "apt"
       }
+    },
+    {
+      "name": "smach",
+      "build": {
+        "package": "ros-noetic-smach-ros",
+        "provider": "apt"
+      }
     }
   ]
-}
+}

requirements.txt

@@ -5,5 +5,5 @@ python-statemachine==2.0.0
 scipy
 rpi.gpio
 smbus
-numpy==1.24.1
-opencv-contrib-python==4.7.0.68
+numpy==1.23.5
+opencv-contrib-python==4.9.0.80

src/wr_control_drive_ds/CMakeLists.txt

@@ -11,7 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   wr_drive_msgs
-  wroboclaw
+  #wroboclaw
 )
 
 ## System dependencies are found with CMake's conventions

src/wr_control_drive_ds/launch/std.launch

@@ -4,8 +4,8 @@
     <group ns="drive_system">
       <node name="control" pkg="wr_control_drive_ds" type="DriveTrainControlSystem" machine="wrover"/>
     </group>
-    <group ns="camera">
+    <!-- <group ns="camera">
       <node name="cam_mast_control" pkg="wr_control_drive_ds" type="cam_mast_control.py" machine="wrover"/>
-    </group>
+    </group> -->
   </group>
 </launch>

src/wr_entry_point/launch/comp/comp_init.launch

@@ -7,7 +7,8 @@
   <machine name="base_station" address="localhost" env-loader="/opt/ros/noetic/env.sh"
     default="false" />
   <!-- If running in local mode, define "wiscrobo" to also be the local machine (i.e. base station) -->
-  <machine name="wrover" address="wrover.local"
+  <!-- <machine name="wrover" address="wrover.local" -->
+  <machine name="wrover" address="192.168.2.100"
     env-loader="/home/wiscrobo/workspace/WRover_Software/env_remote.sh" user="wiscrobo"
     default="true" unless="$(env WROVER_LOCAL)">
   </machine>

src/wr_entry_point/launch/erdm.launch

@@ -17,4 +17,6 @@
 
   <include file="$(find wr_entry_point)/launch/comp/comp_arm.launch" />
   <include file="$(find wr_entry_point)/launch/comp/comp_falcon.launch" />
+
+  <include file="$(find wr_hsi_sensing)/launch/gps_data_mapper.launch" />
 </launch>

src/wr_hsi_roboclaw/config/roboclaw_enc.yaml

@@ -1,5 +1,6 @@
-com_port: '/dev/ttyAMA0'
-baud: 115200
+com_port: '/dev/roboclaw'
+# baud: 115200
+baud: 9600
 claws:
   aux0: # arm turntable/shoulder, science ?
     topic: '/hsi/roboclaw/aux0'

src/wr_hsi_roboclaw/config/roboclaw_noenc.yaml

@@ -1,4 +1,4 @@
-com_port: '/dev/ttyTHS1'
+com_port: '/dev/ttyTHS0'
 baud: 38400
 claws:
   aux0: # arm turntable/shoulder, science ?

src/wr_hsi_sensing/CMakeLists.txt

@@ -10,6 +10,10 @@ project(wr_hsi_sensing)
 find_package(catkin REQUIRED COMPONENTS 
     message_generation
     std_msgs
+    sensor_msgs
+    geometry_msgs
+    visualization_msgs
+    roscpp
     rospy
     ublox_gps
 )
@@ -52,7 +56,7 @@ catkin_python_setup()
 
 ## Generate messages in the 'msg' folder
 add_message_files(
-  FILES
+    FILES
 #   Message1.msg
 #   Message2.msg
     CoordinateMsg.msg
@@ -121,15 +125,16 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-include_directories(
+#include_directories(
 # include
-${catkin_INCLUDE_DIRS}
-)
+#${catkin_INCLUDE_DIRS}
+#)
 
 ## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/wr_hsi_sensing.cpp
+# add_library(sh2
+#   # src/${PROJECT_NAME}/wr_hsi_sensing.cpp
 # )
+# target_link_libraries(${PROJECT_NAME} sh2)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -139,7 +144,8 @@ ${catkin_INCLUDE_DIRS}
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/wr_hsi_sensing_node.cpp)
+#add_executable()
+#target_link_libraries(${catkin_LIBRARIES} ${PROJECT_NAME})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the

src/wr_hsi_sensing/config/ardusimple_config.yaml

@@ -1,6 +1,6 @@
 debug: 1 # Range 0-4 (0 means no debug statements will print)
 
-device: /dev/ttyACM0
+device: /dev/gps
 frame_id: gps # TODO: change it in the future?
 
 config_on_startup: false

src/wr_hsi_sensing/launch/mapviz.launch

@@ -2,6 +2,7 @@
   <!-- Launches mapviz visualization. -->
   <!-- Requires ardusimple_provided node to be running -->
   <!-- <include file="$(find wr_hsi_sensing)/launch/ardusimple_provided.launch" /> -->
+  <node pkg="wr_hsi_sensing" type="publish_points.py" name="gps_data_points_pub" />
 
   <node pkg="mapviz" type="mapviz" name="mapviz">
     <!-- <param name="config" value=""/> -->
@@ -11,10 +12,11 @@
     <param name="local_xy_frame" value="/map"/> <!-- Could be "/map"-->
     <param name="local_xy_origin" value="auto"/>
     <param name="local_xy_navsatfix_topic" value="/gps/fix"/>
-    <remap from="fix" to="/gps/fix"/>
+    <!-- <remap from="fix" to="/gps/fix"/> -->
+    <remap from="fix" to="/navsat/fix"/>
 
   </node>
 
   <node pkg="tf" type="static_transform_publisher" name="swri_transform" args="0 0 0 0 0 0 /map /origin 100"  />
 
-</launch>
+</launch>

src/wr_hsi_sensing/nodes/coordinates.json

@@ -0,0 +1,42 @@
+[
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Sample Cash"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Rock locations?"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Rock Box"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Astronaout"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Work Area"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Wrekage Site"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "6A: Far Wrekage Site"
+    },
+    {
+        "lat": 38.3717950,
+        "long": -110.7045599,
+        "note": "Drone"
+    }
+]