Autonomous state machine

.catkin_workspace

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

.gitmodules

@@ -16,3 +16,6 @@
 [submodule "src/wrevolution"]
 	path = src/wrevolution
 	url = https://github.com/WisconsinRobotics/wrevolution
+[submodule "src/wr_hsi_sensing/src/sh2"]
+	path = src/wr_hsi_sensing/sh2
+	url = https://github.com/ceva-dsp/sh2

.vscode/settings.json

@@ -5,12 +5,13 @@
     ],
     "python.analysis.extraPaths": [
         "./src/wr_hsi_sensing/src",
-        "./src/wr_logic_ai/src"
+        "./src/wr_logic_ai/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"
-}
+}

project.json

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

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_hsi_sensing/CMakeLists.txt

@@ -10,6 +10,7 @@ project(wr_hsi_sensing)
 find_package(catkin REQUIRED COMPONENTS 
     message_generation
     std_msgs
+    roscpp
     rospy
     ublox_gps
 )
@@ -123,14 +124,25 @@ catkin_package(
 ## Your package locations should be listed before other locations
 include_directories(
 # include
+sh2
 ${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
 # )
 
+add_library(${PROJECT_NAME}
+  sh2/euler.c
+  sh2/sh2_SensorValue.c
+  sh2/sh2_util.c
+  sh2/sh2.c
+  sh2/shtp.c
+  src/BNO085_hal.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
 ## either from message generation or dynamic reconfigure
@@ -139,7 +151,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(BNO085_IMU src/BNO085_IMU.cpp)
+target_link_libraries(BNO085_IMU ${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/src/BNO085_IMU.cpp

@@ -0,0 +1,102 @@
+#include "BNO085_hal.hpp"
+
+#include "ros/console.h"
+#include "ros/init.h"
+#include "ros/node_handle.h"
+#include "ros/publisher.h"
+#include "ros/rate.h"
+#include "std_msgs/Float64.h"
+#include "std_msgs/Int16.h"
+#include <cstdlib>
+#include <iostream>
+
+static constexpr uint32_t QUEUE_SIZE = 1;
+
+const bool debug = true;
+BNO085 sensor(debug);
+
+void sensor_setup() {
+    // if (sensor.set_sensor_config(SH2_MAGNETIC_FIELD_UNCALIBRATED) != SH2_OK) {
+    //     ROS_FATAL("FAILED TO ENABLE MAGNETIC FIELD");
+    // }
+
+    // if (sensor.set_sensor_config(SH2_MAGNETIC_FIELD_CALIBRATED) != SH2_OK) {
+    // ROS_FATAL("FAILED TO ENABLE MAGNETIC FIELD CALIBRATED");
+    // }
+
+    if (sensor.set_sensor_config(SH2_GAME_ROTATION_VECTOR) != SH2_OK) {
+        ROS_FATAL("FAILED TO ENABLE IMU GAME ROTATION VECTOR");
+    }
+}
+
+auto main(int argc, char **argv) -> int {
+
+    // Initialize the node in ROS
+    ros::init(argc, argv, "BNO085");
+
+    // Create a node handle for the current node to subscribe to/advertise topics
+    ros::NodeHandle nHandle;
+
+    ros::Rate rate(10);
+
+    // Create publishers
+    auto pub_x{nHandle.advertise<std_msgs::Float64>("/mag_x", QUEUE_SIZE)};
+    auto pub_y{nHandle.advertise<std_msgs::Float64>("/mag_y", QUEUE_SIZE)};
+    auto pub_z{nHandle.advertise<std_msgs::Float64>("/mag_z", QUEUE_SIZE)};
+    auto pub_acc{nHandle.advertise<std_msgs::Int16>("/mag_acc", QUEUE_SIZE)};
+
+    auto pub_heading{nHandle.advertise<std_msgs::Float64>("/heading_data", QUEUE_SIZE)};
+
+    // Initalize IMU
+    // nHandle.getParam("imu/debug", debug);
+    if (!sensor.begin()) {
+        ROS_FATAL("FAILED TO INITIALIZE IMU");
+    }
+
+    sensor_setup();
+
+    if (!sensor.tare(true, SH2_TARE_BASIS_GAMING_ROTATION_VECTOR)) {
+        ROS_WARN("FAILED TO TARE IMU GAME ROTATION VECTOR");
+    } else {
+        if (!sensor.persist_tare()) {
+            ROS_WARN("IMU TARE WILL CHANGE ON NEXT RESET");
+        }
+    }
+    sensor.tare(false, SH2_TARE_BASIS_GAMING_ROTATION_VECTOR);
+
+
+    std_msgs::Float64 mag_x, mag_y, mag_z, heading;
+    std_msgs::Int16 mag_acc;
+    while (ros::ok()) {
+        if (sensor.get_sensor_event()) {
+
+            // if (sensor.sensor_value.sensorId == SH2_MAGNETIC_FIELD_CALIBRATED) {
+            //     mag_x.data = sensor.get_mag_x();
+            //     pub_x.publish(mag_x);
+
+            //     mag_y.data = sensor.get_mag_y();
+            //     pub_y.publish(mag_y);
+
+            //     mag_z.data = sensor.get_mag_z();
+            //     pub_z.publish(mag_z);
+
+            //     mag_acc.data = static_cast<int16_t>(sensor.get_accuracy());
+            //     pub_acc.publish(mag_acc);
+            // }
+
+            if (sensor.get_reset()) {
+                sensor_setup();
+            }
+
+            if (sensor.sensor_value.sensorId == SH2_GAME_ROTATION_VECTOR) {
+                // Convert Euler angle to 0 to 360
+                heading.data = (sensor.get_yaw() + M_PI) / M_PI * 180;
+                pub_heading.publish(heading);
+            }
+        }
+
+        rate.sleep();
+    }
+
+    return EXIT_SUCCESS;
+}