Cwbollinger/mobile api wip

kits/base/omni_mobile_io_control.cpp

@@ -0,0 +1,103 @@
+
+#include "util/mobile_io.hpp"
+#include "util/omni_base.hpp"
+
+using namespace hebi;
+using namespace experimental;
+using namespace mobile;
+
+
+int main(int argc, char* argv[]) {
+
+  //////////////////////////
+  //// MobileIO Setup //////
+  //////////////////////////
+
+  std::cout << "Creating MobileIO" << std::endl;
+
+  std::string family = "Rosie";
+
+  // Create the MobileIO object
+  std::unique_ptr<MobileIO> mobile = MobileIO::create(family, "mobileIO");
+  while(!mobile) {
+    std::cout << "Couldn't find mobileIO, trying again..." << std::endl;
+    mobile = MobileIO::create(family, "mobileIO");
+  }
+
+  std::string instructions("A1/A2 - Move Base\n"
+                           "A7 - Turn Base\n"
+                           "B8 - Quit\n");
+
+  // Clear any garbage on screen
+  mobile -> clearText(); 
+
+  // Display instructions on screen
+  mobile -> sendText(instructions); 
+
+  //////////////////////////
+  //// OmniBase Setup //////
+  //////////////////////////
+
+  // Set module names for mobile base
+  OmniBase::Params p;
+  p.families_ = {family};
+  p.names_ = {"W1", "W2", "W3"};
+
+  std::cout << "Creating Omni Base" << std::endl;
+
+  auto base = OmniBase::create(p);
+
+  if (!base) {
+    std::cout << "Failed to create base, exiting!" << std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  auto last_state = mobile->getState();
+
+  //////////////////////////
+  //// Main Control Loop ///
+  //////////////////////////
+
+  while(base->update())
+  {
+    auto state = mobile->getState();
+    MobileIODiff diff(last_state, state);
+
+    /////////////////
+    // Input Handling
+    /////////////////
+
+    auto dy = -1 * state.getAxis(7) / 2.0;
+    auto dx = state.getAxis(8) / 2.0;
+    auto dtheta = -1 * state.getAxis(1);
+
+    // Button B8 - End Demo
+    if (diff.get(8) == MobileIODiff::ButtonState::ToOn)
+    {
+      // Clear MobileIO text
+      mobile->clearText();
+      return 1;
+    }
+
+    /////////////////
+    // Update & send
+    /////////////////
+
+    // create Vel Goal for 0.5 second, 0.25 second ramp down
+    auto goal = CartesianGoal::createFromVelocity(Vel{dx, dy, dtheta}, 0.5, 0.25);
+
+    // send goal to base
+    base->setGoal(goal);
+
+    // Update to the new last_state for mobile device
+    last_state = state;
+
+    // Send latest commands to the base
+    base->send();
+  }
+
+  // Clear MobileIO text
+  mobile -> clearText();
+
+  return 0;
+};

kits/base/omni_waypoints.cpp

@@ -0,0 +1,75 @@
+
+#include "util/omni_base.hpp"
+
+using namespace hebi;
+using namespace experimental;
+using namespace mobile;
+
+
+int main(int argc, char* argv[]) {
+
+  //////////////////////////
+  //// OmniBase Setup //////
+  //////////////////////////
+
+  // Set module names for mobile base
+  OmniBase::Params p;
+  p.families_ = {"Rosie"};
+  p.names_ = {"W1", "W2", "W3"};
+
+  std::cout << "Creating Omni Base" << std::endl;
+
+  auto base = OmniBase::create(p);
+
+  if (!base) {
+    std::cout << "Failed to create base, exiting!" << std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  auto currentPose = base->getOdometry();
+
+  Waypoint wp1;
+  Waypoint wp2;
+  Waypoint wp3;
+  Waypoint wp4;
+
+  wp1.t = 1.0;
+  wp1.pos = currentPose;
+
+  wp2 = wp1;
+  wp2.t += 1.0;
+  wp2.pos.x += 0.5;
+
+  wp3 = wp2;
+  wp3.t += 1.0;
+  wp3.pos.y += 0.5;
+
+  wp4 = wp3;
+  wp4.t += 1.0;
+  wp4.pos.x -= 0.5;
+
+  auto goal = CartesianGoal::createFromWaypoints({wp1, wp2, wp3, wp4}, false);
+
+  // send goal to base
+  base->setGoal(goal);
+
+  //////////////////////////
+  //// Main Control Loop ///
+  //////////////////////////
+
+  std::cout << "Executing Goal" << std::endl;
+  return -1;
+
+  while (base->update())
+  {
+    // Send updated command to the base
+    base->send();
+
+    // if the trajectory has been completed, start another square
+    if (base->goalComplete()) {
+      base->setGoal(goal);
+    }
+  }
+
+  return 0;
+};

projects/cmake/CMakeLists.txt

@@ -315,13 +315,44 @@ endforeach (EXAMPLE ${KIT_SOURCES})
 file(COPY ${ROOT_DIR}/kits/arm/hrdf DESTINATION ${ROOT_DIR}/build/kits/arm)
 file(COPY ${ROOT_DIR}/kits/arm/gains DESTINATION ${ROOT_DIR}/build/kits/arm)
 
+SET(BASE_SOURCES
 
+  ${ROOT_DIR}/kits/base/omni_waypoints.cpp
+  ${ROOT_DIR}/kits/base/omni_mobile_io_control.cpp)
 
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/base)
+foreach (EXAMPLE ${BASE_SOURCES})
 
+  # The target for the individual example is based on the filename
+  get_filename_component(EX_NAME ${EXAMPLE} NAME_WE)
+
+  if(WIN32)
+    add_executable(${EX_NAME} ${EXAMPLE} $<TARGET_OBJECTS:_hebic++-obj>)
+  else()
+    add_executable(${EX_NAME} ${EXAMPLE})
+  endif()
+  add_dependencies(examples ${EX_NAME})
+  target_include_directories(${EX_NAME} PRIVATE ${ROOT_DIR})
 
+  target_include_directories(${EX_NAME} PRIVATE ${PYTHON_INCLUDE_DIRS})
+  target_link_libraries(${EX_NAME} ${PYTHON_LIBRARIES})
+  if(WIN32)
+    target_link_libraries(${EX_NAME} hebi kernel32)
+    target_include_directories(${EX_NAME} PRIVATE ${HEBI_DIR}/src ${HEBI_DIR}/include ${HEBI_DIR}/Eigen)
+    # For Windows, we copy the .dll file into the binary directory so that we
+    # don't have to set the PATH variable.
 
+    set(LIBHEBI_LOCATION "lib/win_${LIBHEBI_TARGET_ARCHITECTURE}")
+    set(HEBI_CPP_LIB_DIRECTORY ${HEBI_DIR}/hebi/${LIBHEBI_LOCATION}/)
 
+    add_custom_command(TARGET ${EX_NAME} POST_BUILD
+      COMMAND ${CMAKE_COMMAND} -E copy_if_different
+      "${HEBI_CPP_LIB_DIRECTORY}/hebi.dll"
+      $<TARGET_FILE_DIR:${EX_NAME}>)
+  elseif(UNIX)
+    target_link_libraries(${EX_NAME} hebi hebic++ m pthread)
+  endif()
 
-
+endforeach (EXAMPLE ${BASE_SOURCES})