Arm Firmware Master PR

.gitignore

@@ -8,7 +8,7 @@
 CLion*
 clion*
 *~
-.swp
+*.swp
 filter_init.txt
 cmake-build-debug
 cmake-build-release

.rosinstall

@@ -10,6 +10,12 @@
 - git:
     local-name: src/external_pkgs/ros_arduino_bridge
     uri: https://github.com/UBC-Snowbots/ros_arduino_bridge.git
+- git:
+    local-name: src/external_pkgs/swiftnav_piksi
+    uri: https://github.com/PaulBouchier/swiftnav_piksi.git
+- git:
+    local-name: external_libs/libsbp
+    uri: https://github.com/PaulBouchier/libsbp
 - git:
     local-name: src/external_pkgs/sicktoolbox
     uri: https://github.com/UBC-Snowbots/sicktoolbox.git
@@ -23,3 +29,10 @@
     local-name: src/mapping_urc/multi_resolution_graph
     uri: 'https://github.com/garethellis0/multi_resolution_graph.git'
     version: 0.0.3
+- git:
+    local-name: external_libs/dkms-hid-nintendo
+    uri: 'https://github.com/nicman23/dkms-hid-nintendo'
+- git:
+    local-name: external_libs/joycond
+    uri: https://github.com/DanielOgorchock/joycond
+

setup_scripts/install_dependencies.sh

@@ -51,16 +51,19 @@ cd $CURR_DIR
 rosdep install --from-paths \
     $CURR_DIR/../src \
     $CURR_DIR/../src/external_pkgs \
-    --ignore-src --rosdistro melodic --skip-keys=librealsense2 -y 
+    --ignore-src --rosdistro melodic --skip-keys=librealsense2 --skip-keys=libswiftnav -y 
 
 echo "================================================================"
 echo "Installing other dependencies specified by our packages"
 echo "================================================================"
 cd $CURR_DIR
-sudo ./setup_realsense_manual.sh
+# sudo ./setup_realsense_manual.sh
 
 cd $CURR_DIR
 sudo ./install_phidgets.sh
+cd $CURR_DIR
+sudo ./install_libsbp.sh
+cd $CURR_DIR
 
 echo "================================================================"
 echo "Installing Misc. Utilities"
@@ -74,32 +77,32 @@ echo "================================================================"
 echo "Installing Robotic Arm Dependencies"
 echo "================================================================"
 
-echo "Upgrading CMake"
-apt-get install wget
-apt-get install apt-transport-https ca-certificates gnupg software-properties-common wget -y
-wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | sudo apt-key add
-apt-add-repository 'deb https://apt.kitware.com/ubuntu/ bionic main'
-apt-get update
-apt-get install cmake
+# echo "Upgrading CMake"
+# apt-get install wget
+# apt-get install apt-transport-https ca-certificates gnupg software-properties-common wget -y
+# wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | sudo apt-key add
+# apt-add-repository 'deb https://apt.kitware.com/ubuntu/ bionic main'
+# apt-get update
+# apt-get install cmake
 
 echo "Installing Nintendo HID kernel module"
 
-apt-get update && apt-get install -y git dkms
-git clone https://github.com/nicman23/dkms-hid-nintendo && \
-cd ./dkms-hid-nintendo && \
-add . && sudo dkms build nintendo -v 3.2 && sudo dkms install nintendo -v 3.2
+sudo apt-get update && sudo apt-get install -y git dkms
+cd $CURR_DIR/../external_libs/dkms-hid-nintendo && \
+sudo dkms add . && sudo dkms build nintendo -v 3.2 && sudo dkms install nintendo -v 3.2
 
 echo "the Joycond Linux Daemon"
 
-apt-get install libudev-dev -y
-git clone https://github.com/DanielOgorchock/joycond && \
-cd ../joycond && \
-cmake . && make install && systemctl enable --now joycond
+sudo apt-get install libudev-dev -y
+cd $CURR_DIR/../external_libs/joycond && \
+cmake . && sudo make install && sudo systemctl enable --now joycond
 
-sudo cp moveitjoy_module.py /opt/ros/melodic/lib/python2.7/dist-packages/moveit_ros_visualization
+sudo cp $CURR_DIR/moveitjoy_module.py /opt/ros/melodic/lib/python2.7/dist-packages/moveit_ros_visualization
 
 echo "ProController is now enabled"
 
+wget https://www.pjrc.com/teensy/00-teensy.rules
+sudo mv 00-teensy.rules /etc/udev/rules.d/
 
 
 echo "================================================================"

setup_scripts/install_libsbp.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+echo "================================================================"
+echo "Installing Swift Navigation's Binary Protocol library || (GPS)"
+echo "================================================================"
+
+cd ../external_libs/libsbp/c # get to build space
+mkdir build
+cd build
+cmake ../
+make
+sudo make install   # install headers and libraries into /usr/local
+

setup_scripts/moveitjoy_module.py

@@ -49,12 +49,12 @@
 import numpy
 import time
 import tf
-from std_msgs.msg import Empty, String
+from std_msgs.msg import Empty, String, Bool
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import PoseStamped
 from visualization_msgs.msg import InteractiveMarkerInit
 
-proControllerEnabled = True
+proControllerEnabled = False
 proController = False
 proControllerFirstDetect = True
 
@@ -627,6 +627,17 @@ def __init__(self):
             queue_size=1,
         )
         self.sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)
+        self.sub = rospy.Subscriber("/moveit_toggle", Bool, self.controllerCB, queue_size=1)
+
+        # rospy.Timer(rospy.Duration(0.1), planAndExecute)
+
+
+    '''
+    def planAndExecute(self, timerEvent):
+        if(proControllerEnabled):
+            self.plan_pub.publish(Empty())
+            self.execute_pub.publish(Empty())
+            '''
 
     def updatePlanningGroup(self, next_index):
         if next_index >= len(self.planning_groups_keys):
@@ -746,10 +757,6 @@ def joyCB(self, msg):
             if proControllerFirstDetect:
                 print("procontroller detected")
                 proControllerFirstDetect = False
-
-            if msg.buttons[9] == 1:
-                proControllerEnabled = not proControllerEnabled
-		#print("Controller:{proControllerEnabled} in first".format(proControllerEnabled=proControllerEnabled))
 		time.sleep(1)
 
         else:
@@ -773,6 +780,10 @@ def joyCB(self, msg):
             self.run(status)
             self.history.add(status)
 
+    def controllerCB(self, msg):
+        proControllerEnabled = msg.data
+        print("Pro Controller enabled" if proControllerEnabled else "Pro Controller disabled")
+
     def computePoseFromJoy(self, pre_pose, status):
         new_pose = PoseStamped()
         new_pose.header.frame_id = self.frame_id

src/arm_hardware_driver/CMakeLists.txt

@@ -0,0 +1,215 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arm_hardware_driver)
+
+# Build in "Release" (with lots of compiler optimizations) by default
+# (If built in "Debug", some functions can take orders of magnitude longer)
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+    set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release")
+endif()
+
+add_definitions(-std=c++14)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sb_utils
+  sb_msgs
+)
+find_library(libserial_LIBRARIES libserial.so.0)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+    INCLUDE_DIRS include
+#  LIBRARIES sample_package
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+    ${catkin_INCLUDE_DIRS}
+    ${sb_utils_INCLUDE_DIRS}
+    ./include
+)
+
+## Declare a C++ library
+# add_library(sample_package
+#   src/${PROJECT_NAME}/sample_package.cpp
+# )
+
+## 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
+# add_dependencies(sample_package ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(sample_package_node src/sample_package_node.cpp)
+add_executable(arm_hardware_driver src/arm_hardware_driver.cpp src/armHardwareDriver.cpp include/armHardwareDriver.h)
+
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(sample_package_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(sample_package_node
+#   ${catkin_LIBRARIES}
+# )
+target_link_libraries(arm_hardware_driver ${catkin_LIBRARIES} ${libserial_LIBRARIES} ${sb_utils_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS sample_package sample_package_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+# if (CATKIN_ENABLE_TESTING)
+# 
+#     # Adding gtests to the package
+#     catkin_add_gtest(my-node-test test/my-node-test.cpp src/MyNode.cpp)
+#     target_link_libraries(my-node-test ${catkin_LIBRARIES})
+# 
+#     # Adding rostest to the package
+#     find_package(rostest REQUIRED)
+#     # name the test and link it to the .test file and the .cpp file itself, this will allow
+#     # "catkin_make run_tests" to be able to find and run this rostest
+#     add_rostest_gtest(my_node_rostest test/sample_package_test.test test/my_node_rostest.cpp)
+#     target_link_libraries(my_node_rostest ${catkin_LIBRARIES})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)