diff --git a/.gitignore b/.gitignore index 3dcb6d58a..82cbd03ba 100644 --- a/.gitignore +++ b/.gitignore @@ -8,7 +8,7 @@ CLion* clion* *~ -.swp +*.swp filter_init.txt cmake-build-debug cmake-build-release diff --git a/.rosinstall b/.rosinstall index 379eb10ce..87b7413b5 100644 --- a/.rosinstall +++ b/.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 + diff --git a/docs/CIRC_Images/cam1--images/0.png b/docs/CIRC_Images/cam1--images/0.png new file mode 100644 index 000000000..970808890 Binary files /dev/null and b/docs/CIRC_Images/cam1--images/0.png differ diff --git a/docs/CIRC_Images/cam2--images/0.png b/docs/CIRC_Images/cam2--images/0.png new file mode 100644 index 000000000..48abaf47e Binary files /dev/null and b/docs/CIRC_Images/cam2--images/0.png differ diff --git a/setup_scripts/install_dependencies.sh b/setup_scripts/install_dependencies.sh index 7ed6a3288..60bf942d7 100755 --- a/setup_scripts/install_dependencies.sh +++ b/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 "================================================================" diff --git a/setup_scripts/install_libsbp.sh b/setup_scripts/install_libsbp.sh new file mode 100755 index 000000000..bc33992bf --- /dev/null +++ b/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 + diff --git a/setup_scripts/moveitjoy_module.py b/setup_scripts/moveitjoy_module.py index 681b7d175..05fd02578 100755 --- a/setup_scripts/moveitjoy_module.py +++ b/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 diff --git a/src/arm_hardware_driver/CMakeLists.txt b/src/arm_hardware_driver/CMakeLists.txt new file mode 100644 index 000000000..5a0a09267 --- /dev/null +++ b/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) diff --git a/src/arm_hardware_driver/include/armHardwareDriver.h b/src/arm_hardware_driver/include/armHardwareDriver.h new file mode 100644 index 000000000..1cff3300d --- /dev/null +++ b/src/arm_hardware_driver/include/armHardwareDriver.h @@ -0,0 +1,154 @@ +/* + * Created By: Tate Kolton and Ihsan Olawale + * Created On: July 4, 2022 + * Description: Header file for recieving messages from pro controller and + * relaying them to arm hardware driver module + */ + +#ifndef ARM_HARDWARE_DRIVER_MYNODE_H +#define ARM_HARDWARE_DRIVER_MYNODE_H + +// STD Includes +#include +#include + +// ROS Includes +#include +#include +#include +#include + +// Snowbots Includes +#include +#include + +// Other +#include + + +class ArmHardwareDriver { + public: + ArmHardwareDriver(ros::NodeHandle& nh); + void teensySerialCallback(const std_msgs::String::ConstPtr& inMsg); + void parseInput(std::string inMsg); + void joint_space_motion(std::string inMsg); + void drill_motion(std::string inMsg); + void jointSpaceMove(const char joystick, const char dir); + void changeSpeed(const char dir); + void changeAxis(const char joystick); + void releaseAxis(const char joystick, const char dir); + void endEffector(const char dir); + void endEffectorRel(); + void prepareDrilling(); + void collectSample(); + void depositSample(); + void manualDrill(const char dir); + void releaseDrill(); + void homeArm(); + void cartesian_motion(std::string inMsg); + void cartesian_moveit_move(std::vector& pos_commands, + std::vector& joint_positions); + void updateEncoderSteps(std::string msg); + void encStepsToJointPos(std::vector& enc_steps, + std::vector& joint_positions); + void jointPosToEncSteps(std::vector& joint_positions, + std::vector& enc_steps); + void sendMsg(std::string outMsg); + void recieveMsg(); + void requestArmPosition(); + void updateHWInterface(); + void requestEEFeedback(); + void requestJPFeedback(); + void homeEE(); + void axisRelease(const char axis); + void axisMove(const char axis, const char dir); + + // character representations of buttons for arm communication + const char leftJSU = 'A'; + const char leftJSD = 'B'; + const char rightJSU = 'C'; + const char rightJSD = 'D'; + const char buttonA = 'E'; + const char buttonB = 'F'; + const char buttonX = 'G'; + const char buttonY = 'H'; + const char triggerL = 'I'; + const char triggerR = 'J'; + const char bumperL = 'K'; + const char bumperR = 'L'; + const char buttonARel = 'M'; + const char buttonBRel = 'N'; + const char buttonXRel = 'O'; + const char buttonYRel = 'P'; + const char triggerLRel = 'Q'; + const char triggerRRel = 'R'; + const char bumperLRel = 'S'; + const char bumperRRel = 'T'; + const char arrowL = 'U'; + const char arrowR = 'V'; + const char arrowU = 'W'; + const char arrowD = 'X'; + const char arrowRLRel = '0'; + const char leftJSRel = 'Y'; + const char rightJSRel = 'Z'; + const char rightJSPress = '7'; + const char rightJSPressRel = '8'; + const char homeVal = '4'; + const char homeValEE = '6'; + + const char J1 = '1'; + const char J2 = '2'; + const char J3 = '3'; + const char J4 = '4'; + const char J5 = '5'; + const char J6 = '6'; + // arm modes + const char jointMode = '1'; + const char IKMode = '2'; + const char drillMode = '3'; + + // joystick direction characters + const char left = 'L'; + const char right = 'R'; + const char up = 'U'; + const char down = 'D'; + const char wrist = 'W'; + const char garbage = 'G'; + const char open = 'O'; + const char close = 'C'; + + int num_joints_ = 6; + double ppr = 400.0; + double encppr = 512.0; + + bool serialOpen = true; + bool dataInTransit = false; + bool homeFlag = false; + char mode = jointMode; + + // hardware interface communication variables + std::vector encPos, encCmd; + std::vector armCmd, armPos, encStepsPerDeg; + std::vector reductions{50, 161, 93.07, 44.8, 57.34, 57.34}; + + // timer variables + double refresh_rate_hz = 10.0; + ros::Timer arm_pos_timer; + + private: + ros::NodeHandle nh; + void armPositionCallBack(const sb_msgs::ArmPosition::ConstPtr& cmd_msg); + void teensyFeedback(const ros::TimerEvent& e); + + ros::Subscriber subPro; + ros::Subscriber sub_command_pos; + ros::Publisher pub_observed_pos; + ros::Timer feedbackLoop; + + // The SerialStream to/from the teensy + LibSerial::SerialStream teensy; + + // The Port the teensy is connected to + std::string port; +}; +#endif // ARM_HARDWARE_DRIVER_MYNODE_H diff --git a/src/arm_hardware_driver/package.xml b/src/arm_hardware_driver/package.xml new file mode 100644 index 000000000..79f713ecf --- /dev/null +++ b/src/arm_hardware_driver/package.xml @@ -0,0 +1,60 @@ + + + arm_hardware_driver + 0.0.0 + The arm_hardware_driver package + + + + + tate + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + sb_utils + sb_msgs + libserial-dev + roscpp + std_msgs + sb_utils + sb_msgs + libserial-dev + + + + + + + + diff --git a/src/arm_hardware_driver/src/armHardwareDriver.cpp b/src/arm_hardware_driver/src/armHardwareDriver.cpp new file mode 100644 index 000000000..777fe1b49 --- /dev/null +++ b/src/arm_hardware_driver/src/armHardwareDriver.cpp @@ -0,0 +1,406 @@ +/* + * Created By: Tate Kolton + * Created On: July 4, 2022 + * Description: This package receives info from /cmd_arm topic and publishes + * serial data via callback to be recieved by the arm MCU (Teensy 4.1) + */ + +#include "../include/armHardwareDriver.h" + +ArmHardwareDriver::ArmHardwareDriver(ros::NodeHandle& nh) : nh(nh) { + // Setup NodeHandles + + ros::NodeHandle private_nh("~"); + + // Setup Subscribers + int queue_size = 10; + + subPro = nh.subscribe( + "/cmd_arm", queue_size, &ArmHardwareDriver::teensySerialCallback, this); + sub_command_pos = nh.subscribe( + "/cmd_pos_arm", queue_size, &ArmHardwareDriver::armPositionCallBack, this); + pub_observed_pos = + private_nh.advertise("/observed_pos_arm", 1); + + // Get Params + SB_getParam( + private_nh, "/hardware_driver/port", port, (std::string) "/dev/ttyACM0"); + // Open the given serial port + teensy.Open(port); + teensy.SetBaudRate(LibSerial::SerialStreamBuf::BAUD_9600); + teensy.SetCharSize(LibSerial::SerialStreamBuf::CHAR_SIZE_8); + + encCmd.resize(num_joints_); + armCmd.resize(num_joints_); + encStepsPerDeg.resize(num_joints_); + armPos.resize(num_joints_); + encPos.resize(num_joints_); + armCmd.resize(num_joints_); + + for (int i = 0; i < num_joints_; i++) { + encStepsPerDeg[i] = reductions[i] * ppr * 5.12 / 360.0; + } + + float feed_freq = 10.131; // not exactly 5 to ensure that this doesn't regularly interfere with HW interface callback + ros::Duration feedbackFreq = ros::Duration(1.0/feed_freq); + feedbackLoop = nh.createTimer(feedbackFreq, &ArmHardwareDriver::teensyFeedback, this); + +} + +//Timer initiated event to request joint feedback +void ArmHardwareDriver::teensyFeedback(const ros::TimerEvent& e) +{ + + //ROS_INFO("timer working"); + /* + if(homeFlag) + { + */ + //requestEEFeedback(); + //if(mode == jointMode) + //{ + //requestJPFeedback(); + //} + // } +} + +void ArmHardwareDriver::requestEEFeedback() +{ + std::string outMsg = "FBE\n"; + sendMsg(outMsg); + recieveMsg(); +} + +void ArmHardwareDriver::requestJPFeedback() +{ + std::string outMsg = "FBJ\n"; + sendMsg(outMsg); + recieveMsg(); +} + +// Callback function to relay pro controller messages to teensy MCU on arm via +// rosserial +void ArmHardwareDriver::teensySerialCallback( +const std_msgs::String::ConstPtr& inMsg) { + parseInput(inMsg->data); +} + +void ArmHardwareDriver::parseInput(std::string inMsg) { + mode = inMsg[0]; + + if (mode == jointMode) { + joint_space_motion(inMsg); + } else if (mode == IKMode) { + cartesian_motion(inMsg); + } else if (mode == drillMode) { + drill_motion(inMsg); + } +} + +// Sends joint space motion related commands to teensy +void ArmHardwareDriver::joint_space_motion(std::string inMsg) { + char action = inMsg[1]; + + if(action == homeVal) { + homeArm(); + } else if(action == leftJSU) { + axisMove(J3,up); + } else if (action == leftJSD) { + axisMove(J3, down); + } else if (action == rightJSU) { + axisMove(J2, up); + } else if (action == rightJSD) { + axisMove(J2, down); + } else if (action == leftJSRel) { + axisRelease(J3); + } else if (action == rightJSRel) { + axisRelease(J2); + } else if (action == buttonA) { + jointSpaceMove(wrist, up); + } else if (action == buttonB) { + jointSpaceMove(wrist, left); + } else if (action == buttonX) { + jointSpaceMove(wrist, right); + } else if (action == buttonY) { + jointSpaceMove(wrist, down); + } else if (action == triggerL) { + axisMove(J1, left); + } else if (action == triggerR) { + axisMove(J1, right); + } else if ((action == triggerLRel) || (action == triggerRRel)) { + axisRelease(J1); + } else if (action == buttonARel) { + releaseAxis(wrist, up); + } else if (action == buttonBRel) { + releaseAxis(wrist, left); + } else if (action == buttonXRel) { + releaseAxis(wrist, right); + } else if (action == buttonYRel) { + releaseAxis(wrist, down); + } else if(action == bumperL) { + axisMove(J4, left); + } else if(action == bumperR) { + axisMove(J4, right); + } else if((action == bumperLRel) || (action == bumperRRel)) { + axisRelease(J4); + } else if (action == arrowL) { + endEffector(open); + } else if (action == arrowR) { + endEffector(close); + } else if (action == arrowRLRel) { + endEffectorRel(); + } else if(action == homeValEE) { + homeEE(); + } +} + +void ArmHardwareDriver::cartesian_motion(std::string inMsg) { + char action = inMsg[1]; + + if (action == arrowL) { + endEffector(open); + } else if (action == arrowR) { + endEffector(close); + } else if (action == arrowRLRel) { + endEffectorRel(); + } else if(action == homeValEE) { + homeEE(); + } +} + +// Sends drilling mode related commands to teensy +void ArmHardwareDriver::drill_motion(std::string inMsg) { + char action = inMsg.at(1); + + if (action == buttonARel) { + prepareDrilling(); + } else if (action == buttonBRel) { + collectSample(); + } else if (action == buttonX) { + depositSample(); + } else if (action == triggerL) { + manualDrill(left); + } else if (action == triggerR) { + manualDrill(right); + } else if ((action == triggerLRel) || (action == triggerRRel)) { + releaseDrill(); + } + // below two lines to be implemented once cartesian mode is sorted + // case rightJSU: moveDrillUp(); break; + // case rightJSD: moveDrillDown(); break; +} + +void ArmHardwareDriver::jointSpaceMove(const char joystick, const char dir) { + std::string outMsg = "JM"; + outMsg += "M"; + outMsg += joystick; + outMsg += dir; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::axisMove(const char axis, const char dir) +{ + std::string outMsg = "JMT"; + outMsg += axis; + outMsg += dir; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::axisRelease(const char axis) +{ + std::string outMsg = "JMW"; + outMsg += axis; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::changeSpeed(const char dir) { + //std::string outMsg = "JM"; + //outMsg = "S"; + //outMsg += dir; + //outMsg += "\n"; + //sendMsg(outMsg); +} + +void ArmHardwareDriver::changeAxis(const char joystick) { + std::string outMsg = "JM"; + outMsg += "A"; + outMsg += joystick; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::releaseAxis(const char joystick, const char dir) { + std::string outMsg = "JM"; + outMsg += "R"; + outMsg += joystick; + outMsg += dir; + outMsg += "\n"; + sendMsg(outMsg); +} + +// End Effector Hardware Driver Functions +void ArmHardwareDriver::endEffector(const char dir) { + std::string outMsg = "EE"; + outMsg += dir; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::endEffectorRel() { + std::string outMsg = "EER\n"; + sendMsg(outMsg); +} + +// Drilling Mode Hardware Driver Functions +void ArmHardwareDriver::prepareDrilling() { + std::string outMsg = "DMP\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::collectSample() { + std::string outMsg = "DMC\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::depositSample() { + std::string outMsg = "DMD\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::manualDrill(const char dir) { + std::string outMsg = "DMM"; + outMsg += dir; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::releaseDrill() { + std::string outMsg = "DMMX"; + outMsg += "\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::homeArm() { + std::string outMsg = "HM\n"; + homeFlag = false; + sendMsg(outMsg); + recieveMsg(); + homeFlag = true; +} + +void ArmHardwareDriver::homeEE() { + std::string outMsg = "EEH\n"; + sendMsg(outMsg); +} + +void ArmHardwareDriver::armPositionCallBack( +const sb_msgs::ArmPosition::ConstPtr& commanded_msg) { + // TODO: ihsan fill std::vector type with sb_msgs values + armCmd.assign(commanded_msg->positions.begin(), + commanded_msg->positions.end()); + jointPosToEncSteps(armCmd, encCmd); + + std::string outMsg = "MT"; + for (int i = 0; i < num_joints_; ++i) { + outMsg += 'A' + i; + outMsg += std::to_string(encCmd[i]); + } + outMsg += "\n"; + sendMsg(outMsg); + recieveMsg(); +} + +void ArmHardwareDriver::updateEncoderSteps(std::string msg) { + size_t idx1 = msg.find("A", 2) + 1; + size_t idx2 = msg.find("B", 2) + 1; + size_t idx3 = msg.find("C", 2) + 1; + size_t idx4 = msg.find("D", 2) + 1; + size_t idx5 = msg.find("E", 2) + 1; + size_t idx6 = msg.find("F", 2) + 1; + size_t idx7 = msg.find("Z", 2) + 1; + encPos[0] = std::stoi(msg.substr(idx1, idx2 - idx1)); + encPos[1] = std::stoi(msg.substr(idx2, idx3 - idx2)); + encPos[2] = std::stoi(msg.substr(idx3, idx4 - idx3)); + encPos[3] = std::stoi(msg.substr(idx4, idx5 - idx4)); + encPos[4] = std::stoi(msg.substr(idx5, idx6 - idx5)); + encPos[5] = std::stoi(msg.substr(idx6, idx7 - idx6)); +} + +void ArmHardwareDriver::encStepsToJointPos( +std::vector& enc_steps, std::vector& joint_positions) { + for (int i = 0; i < enc_steps.size(); ++i) { + // convert enc steps to joint deg + joint_positions[i] = + static_cast(enc_steps[i]) / encStepsPerDeg[i]; + } +} + +void ArmHardwareDriver::jointPosToEncSteps(std::vector& joint_positions, + std::vector& enc_steps) { + for (int i = 0; i < joint_positions.size(); ++i) { + // convert joint deg to enc steps + enc_steps[i] = static_cast(joint_positions[i] * encStepsPerDeg[i]); + } +} + +// Libserial Implementation + +void ArmHardwareDriver::sendMsg(std::string outMsg) { + // Send everything in outMsg through serial port + /* + if(serialOpen) + { + */ + // close serial port to other processes + serialOpen = false; + dataInTransit = true; + teensy << outMsg; + // } + ROS_INFO("Sent via serial: %s", outMsg.c_str()); +} + +void ArmHardwareDriver::recieveMsg() { + // fill inMsg string with whatever comes through serial port until \n + /* + if(dataInTransit) + { + */ + std::stringstream buffer; + char next_char; + do { + teensy >> next_char; + // ROS_INFO("next_char: %c", next_char); + buffer << next_char; + } while (next_char != 'Z'); + std::string inMsg = buffer.str(); + + // check if joint state is available + if (inMsg.substr(0, 2) == "JP") { + updateEncoderSteps(inMsg); + encStepsToJointPos(encPos, armPos); + // updateHWInterface(); + // check if end effector force feedback is available + } else if (inMsg.substr(0, 2) == "EE") + ROS_INFO("%s", inMsg.c_str()); + // check if homing is completed + else if(inMsg.substr(0, 2) == "HC") + { + ROS_INFO("ARM CALIBRATION COMPLETE, NOW ACCEPTING CONTROLLER COMMANDS!"); + } + // open serial port to other processes + serialOpen = true; + dataInTransit = false; + /* + } + */ +} + +void ArmHardwareDriver::updateHWInterface() { + // TODO: Ihsan fill in correct message implementation + sb_msgs::ArmPosition outMsg; + outMsg.positions.assign(armPos.begin(), armPos.end()); + pub_observed_pos.publish(outMsg); +} diff --git a/src/arm_hardware_driver/src/arm_hardware_driver.cpp b/src/arm_hardware_driver/src/arm_hardware_driver.cpp new file mode 100644 index 000000000..5b36f73f6 --- /dev/null +++ b/src/arm_hardware_driver/src/arm_hardware_driver.cpp @@ -0,0 +1,29 @@ +/* + * Created By: Tate Kolton + * Created On: July 4, 2022 + * Description: Node for recieving messages from pro controller and relaying them to arm hardware driver module + */ + +#include "../include/armHardwareDriver.h" +#include + +int main(int argc, char** argv) { + + + // Setup your ROS node + std::string node_name = "arm_hardware_driver"; + ros::CallbackQueue ros_queue; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + nh.setCallbackQueue(&ros_queue); + + // Create an instance of your class + ArmHardwareDriver teensyComm(nh); + + // Start up ros. This will continue to run until the node is killed + ros::MultiThreadedSpinner spinner(0); + spinner.spin(&ros_queue); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/arm_hardware_driver/src/arm_hardware_driver.cpp~tate_teensy-driver-tate b/src/arm_hardware_driver/src/arm_hardware_driver.cpp~tate_teensy-driver-tate new file mode 100644 index 000000000..5b36f73f6 --- /dev/null +++ b/src/arm_hardware_driver/src/arm_hardware_driver.cpp~tate_teensy-driver-tate @@ -0,0 +1,29 @@ +/* + * Created By: Tate Kolton + * Created On: July 4, 2022 + * Description: Node for recieving messages from pro controller and relaying them to arm hardware driver module + */ + +#include "../include/armHardwareDriver.h" +#include + +int main(int argc, char** argv) { + + + // Setup your ROS node + std::string node_name = "arm_hardware_driver"; + ros::CallbackQueue ros_queue; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + nh.setCallbackQueue(&ros_queue); + + // Create an instance of your class + ArmHardwareDriver teensyComm(nh); + + // Start up ros. This will continue to run until the node is killed + ros::MultiThreadedSpinner spinner(0); + spinner.spin(&ros_queue); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/arm_hardware_interface/CMakeLists.txt b/src/arm_hardware_interface/CMakeLists.txt new file mode 100644 index 000000000..40b9145b9 --- /dev/null +++ b/src/arm_hardware_interface/CMakeLists.txt @@ -0,0 +1,215 @@ +cmake_minimum_required(VERSION 2.8.3) +project(arm_hardware_interface) + +# 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_msgs + controller_manager +) +find_package(sb_utils REQUIRED) + +## 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 arm_hardware_interface +# 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(arm_hardware_interface +# src/${PROJECT_NAME}/arm_hardware_interface.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(arm_hardware_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(arm_hardware_interface_node src/arm_hardware_interface_node.cpp) +add_executable(arm_hardware_interface src/arm_hardware_interface.cpp src/armHardwareInterface.cpp include/armHardwareInterface.h) + + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(arm_hardware_interface_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(arm_hardware_interface_node +# ${catkin_LIBRARIES} +# ) +target_link_libraries(arm_hardware_interface ${catkin_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 arm_hardware_interface arm_hardware_interface_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/armHardwareInterface.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(arm_hardware_interface_rostest test/arm_hardware_interface_test.test test/arm_hardware_interface_rostest.cpp) +# target_link_libraries(arm_hardware_interface_rostest ${catkin_LIBRARIES}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/arm_hardware_interface/config/controllers.yaml b/src/arm_hardware_interface/config/controllers.yaml new file mode 100644 index 000000000..71b5b06d5 --- /dev/null +++ b/src/arm_hardware_interface/config/controllers.yaml @@ -0,0 +1,33 @@ +controllers: + state: + type: joint_state_controller/JointStateController + publish_rate: 50 + position: + type: position_controllers/JointTrajectoryController + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 + constraints: + goal_time: 0.5 + j1: + trajectory: 0.5 + goal: 0.02 + j2: + trajectory: 0.5 + goal: 0.02 + j3: + trajectory: 0.5 + goal: 0.02 + j4: + trajectory: 0.5 + goal: 0.02 + j5: + trajectory: 0.5 + goal: 0.02 + j6: + trajectory: 0.5 + goal: 0.02 diff --git a/src/arm_hardware_interface/config/hardware_driver.yaml b/src/arm_hardware_interface/config/hardware_driver.yaml new file mode 100644 index 000000000..af2fbb5ee --- /dev/null +++ b/src/arm_hardware_interface/config/hardware_driver.yaml @@ -0,0 +1,2 @@ +hardware_driver: + port: "/dev/ttyACM0" diff --git a/src/arm_hardware_interface/config/hardware_interface.yaml b/src/arm_hardware_interface/config/hardware_interface.yaml new file mode 100644 index 000000000..538f704af --- /dev/null +++ b/src/arm_hardware_interface/config/hardware_interface.yaml @@ -0,0 +1,9 @@ +hardware_interface: + loop_hz: 10 # hz + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 diff --git a/src/arm_hardware_interface/config/joint_offsets.yaml b/src/arm_hardware_interface/config/joint_offsets.yaml new file mode 100644 index 000000000..6eea01715 --- /dev/null +++ b/src/arm_hardware_interface/config/joint_offsets.yaml @@ -0,0 +1,8 @@ +hardware_interface: + joint_offsets: # degrees + j1: 0.0 + j2: 0.0 + j3: 0.0 + j4: 0.0 + j5: 0.0 + j6: 0.0 diff --git a/src/arm_hardware_interface/include/armHardwareInterface.h b/src/arm_hardware_interface/include/armHardwareInterface.h new file mode 100644 index 000000000..cdef2dd8d --- /dev/null +++ b/src/arm_hardware_interface/include/armHardwareInterface.h @@ -0,0 +1,73 @@ +#ifndef ARM_HARDWARE_INTERFACE_H +#define ARM_HARDWARE_INTERFACE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace hardware_interface; + + class ArmHardwareInterface: public hardware_interface::RobotHW + { + public: + ArmHardwareInterface(ros::NodeHandle& nh); + ~ArmHardwareInterface(); + + void init(); + void read(); + void write(ros::Duration elapsed_time); + void cmdArmPosition(const ros::TimerEvent& e); + + private: + ros::NodeHandle nh; + ros::Timer non_realtime_loop_; + ros::Duration control_period_; + ros::Duration elapsed_time_; + ros::Time previous_time_; + double loop_hz_; + boost::shared_ptr controller_manager_; + + // Motor driver + std::vector actuator_commands_; + std::vector actuator_positions_; + + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; + + // Shared memory + int num_joints_; + std::vector joint_names_; + std::vector joint_offsets_; + std::vector joint_positions_; + std::vector joint_velocities_; + std::vector joint_efforts_; + std::vector joint_position_commands_; + + bool cartesian_mode = false; + + // Misc + double degToRad(double deg); + double radToDeg(double rad); + + void controllerModeCallBack(const std_msgs::Bool::ConstPtr& inMsg); + void armPositionCallBack(const sb_msgs::ArmPosition::ConstPtr& observed_Msg); + + ros::Subscriber subMode; + ros::Subscriber sub_arm_pos; + ros::Publisher pub_arm_pos; + }; + +#endif // ARM_HARDWARE_INTERFACE_H diff --git a/src/arm_hardware_interface/launch/arm_hardware_bringup.launch b/src/arm_hardware_interface/launch/arm_hardware_bringup.launch new file mode 100644 index 000000000..b46f2d3b6 --- /dev/null +++ b/src/arm_hardware_interface/launch/arm_hardware_bringup.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_hardware_interface/package.xml b/src/arm_hardware_interface/package.xml new file mode 100644 index 000000000..df2324a11 --- /dev/null +++ b/src/arm_hardware_interface/package.xml @@ -0,0 +1,64 @@ + + + arm_hardware_interface + 0.0.0 + The arm_hardware_interface package + + + + + tate + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + sb_utils + sb_msgs + hardware_interface + joint_limits_interface + controller_manager + roscpp + std_msgs + sb_utils + sb_msgs + hardware_interface + joint_limits_interface + controller_manager + + + + + + + + diff --git a/src/arm_hardware_interface/src/armHardwareInterface.cpp b/src/arm_hardware_interface/src/armHardwareInterface.cpp new file mode 100644 index 000000000..1d4372a68 --- /dev/null +++ b/src/arm_hardware_interface/src/armHardwareInterface.cpp @@ -0,0 +1,171 @@ +#include +#include +#include +#include +#include +#include + +ArmHardwareInterface::ArmHardwareInterface(ros::NodeHandle& nh) : nh(nh) { + // ros::NodeHandle private_nh("~"); + std::string modeSubscriber = "/moveit_toggle"; + subMode = nh.subscribe( + modeSubscriber, 10, &ArmHardwareInterface::controllerModeCallBack, this); + std::string arm_pos_subscriber = "/observed_pos_arm"; + sub_arm_pos = nh.subscribe( + arm_pos_subscriber, 10, &ArmHardwareInterface::armPositionCallBack, this); + std::string arm_pos_publisher = "/cmd_pos_arm"; + pub_arm_pos = nh.advertise(arm_pos_publisher, 1); + + ROS_INFO("1"); + + nh.getParam("/hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) { + ROS_FATAL_STREAM_NAMED( + "init", + "No joints found on parameter server for controller. Did you load the " + "proper yaml file?"); + } + init(); + + // init ros controller manager + controller_manager_.reset( + new controller_manager::ControllerManager(this, nh)); + + nh.param("/hardware_interface/loop_hz", loop_hz_, 0.1); + ROS_DEBUG_STREAM_NAMED("constructor", + "Using loop freqency of " << loop_hz_ << " hz"); + ros::Duration update_freq = ros::Duration(1.0 / loop_hz_); + non_realtime_loop_ = + nh.createTimer(update_freq, &ArmHardwareInterface::cmdArmPosition, this); + + // initialize controller + for (int i = 0; i < num_joints_; ++i) { + ROS_DEBUG_STREAM_NAMED("constructor", + "Loading joint name: " << joint_names_[i]); + + // Create joint state interface + JointStateHandle jointStateHandle(joint_names_[i], + &joint_positions_[i], + &joint_velocities_[i], + &joint_efforts_[i]); + joint_state_interface_.registerHandle(jointStateHandle); + + // Create position joint interface + JointHandle jointPositionHandle(jointStateHandle, + &joint_position_commands_[i]); + position_joint_interface_.registerHandle(jointPositionHandle); + } + + for (int i = 0; i < num_joints_; ++i) { + if (!nh.getParam("/hardware_interface/joint_offsets/" + joint_names_[i], + joint_offsets_[i])) { + ROS_WARN( + "Failed to get params for /hardware_interface/joint_offsets/%s", + joint_names_[i].c_str()); + joint_offsets_[i] = 0; + } + } + + for (int i = 0; i < num_joints_; ++i) { + // assign zero angles initially + joint_positions_[i] = 0; + joint_position_commands_[i] = 0; + } + registerInterface(&joint_state_interface_); + registerInterface(&position_joint_interface_); +} + +ArmHardwareInterface::~ArmHardwareInterface() {} + +void ArmHardwareInterface::controllerModeCallBack( +const std_msgs::Bool::ConstPtr& inMsg) { + cartesian_mode = inMsg->data; + if (cartesian_mode) + ROS_INFO("Enabling Cartesian Mode"); + else { + ROS_INFO("Disabling Cartesian Mode"); + previous_time_ = ros::Time::now(); + } +} + +void ArmHardwareInterface::armPositionCallBack( +const sb_msgs::ArmPosition::ConstPtr& observed_msg) { + // TODO: ihsan implement snowbots message type + // actuator_positions_ = ___________ + ROS_INFO("Received new message"); + actuator_positions_.assign(observed_msg->positions.begin(), + observed_msg->positions.end()); + + if (!cartesian_mode) { + elapsed_time_ = ros::Duration(observed_msg->header.stamp - previous_time_); + previous_time_ = observed_msg->header.stamp; + for (int i = 0; i < num_joints_; ++i) { + // fake controller, set position state to equal command state + joint_positions_[i] = joint_position_commands_[i]; + } + controller_manager_->update(ros::Time::now(), elapsed_time_); + } else { + read(); + controller_manager_->update(ros::Time::now(), elapsed_time_); + } +} + +void ArmHardwareInterface::cmdArmPosition(const ros::TimerEvent& e) { + if (cartesian_mode) { + ROS_INFO("-I- Timer Initiated Position Exchange"); + + for(int i=0; i(joint_names_.size()); + + // resize vectors + actuator_commands_.resize(num_joints_); + actuator_positions_.resize(num_joints_); + joint_positions_.resize(num_joints_); + joint_velocities_.resize(num_joints_); + joint_efforts_.resize(num_joints_); + joint_position_commands_.resize(num_joints_); + joint_offsets_.resize(num_joints_); +} + +void ArmHardwareInterface::read() { + // TODO: assign observed_msg components to actuator_positions_ + + for (int i = 0; i < num_joints_; ++i) { + // apply offsets, convert from deg to rad for moveit + joint_positions_[i] = + degToRad(actuator_positions_[i] + joint_offsets_[i]); + } +} + +void ArmHardwareInterface::write(ros::Duration elapsed_time) { + for (int i = 0; i < num_joints_; ++i) { + // convert from rad to deg, apply offsets + actuator_commands_[i] = + radToDeg(joint_position_commands_[i]) - joint_offsets_[i]; + } +} + +double ArmHardwareInterface::degToRad(double deg) { + return deg / 180.0 * M_PI; +} + +double ArmHardwareInterface::radToDeg(double rad) { + return rad / M_PI * 180.0; +} diff --git a/src/arm_hardware_interface/src/arm_hardware_interface.cpp b/src/arm_hardware_interface/src/arm_hardware_interface.cpp new file mode 100644 index 000000000..33924499e --- /dev/null +++ b/src/arm_hardware_interface/src/arm_hardware_interface.cpp @@ -0,0 +1,15 @@ +#include +#include + +int main(int argc, char** argv) { + std::string node_name = "arm_hardware_interface"; + ros::init(argc, argv, node_name); + ros::CallbackQueue ros_queue; + ros::NodeHandle nh; + nh.setCallbackQueue(&ros_queue); + ArmHardwareInterface arm(nh); + + ros::MultiThreadedSpinner spinner(0); + spinner.spin(&ros_queue); + return 0; +} diff --git a/src/cameras/CMakeLists.txt b/src/cameras/CMakeLists.txt index 38390e9df..b49b4b6c1 100644 --- a/src/cameras/CMakeLists.txt +++ b/src/cameras/CMakeLists.txt @@ -7,12 +7,27 @@ endif() add_definitions(-std=c++14) + find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs + std_msgs + cv_bridge + sensor_msgs + image_transport ) +find_package(OpenCV REQUIRED) +find_package(sb_utils REQUIRED) - +include_directories( + ${catkin_INCLUDE_DIRS} + ${sb_utils_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ./include +) catkin_package( CATKIN_DEPENDS roscpp geometry_msgs ) + +add_executable(ImageSaver src/ImageSaver.cpp src/image_saver.cpp include/ImageSaver.h) +target_link_libraries(ImageSaver ${catkin_LIBRARIES} ${OpenCV_LIBS} ${sb_utils_LIBRARIES}) diff --git a/src/cameras/include/ImageSaver.h b/src/cameras/include/ImageSaver.h new file mode 100644 index 000000000..0f689be57 --- /dev/null +++ b/src/cameras/include/ImageSaver.h @@ -0,0 +1,56 @@ +/* + * Created By: Ihsan Olawale, Rowan Zawadski + * Created On: July 17th, 2022 + * Description: An example node that subscribes to a topic publishing strings, + * and re-publishes everything it receives to another topic with + * a "!" at the end + */ + +#ifndef CAMERAS_IMAGE_SAVER_H +#define CAMERAS_IMAGE_SAVER_H + +// OpenCV +#include +#include +#include + +// Image Conversion +#include +#include +#include +#include + +// STD Includes +#include +#include + +// ROS Includes +#include +#include +#include + +// Snowbots Includes +#include + +class ImageSaver { + public: + ImageSaver(int argc, char** argv, std::string node_name); + int PicsToTake1 = 0; + int PicsToTake2 = 0; + int picCounter1 = 0; + int picCounter2 = 0; + + private: + void subscriberCallBack1(const sensor_msgs::Image::ConstPtr& img1); + void subscriberCallBack2(const sensor_msgs::Image::ConstPtr& img2); + void subscriberCallBackShutter1(const std_msgs::Int32::ConstPtr& msg); + void subscriberCallBackShutter2(const std_msgs::Int32::ConstPtr& msg); + + image_transport::Subscriber camera1_subscribe; + image_transport::Subscriber camera2_subscribe; + ros::Subscriber shutter1; // subscriber to initiate photo saving + ros::Subscriber shutter2; // subscriber to initiate photo saving + + int camera; +}; +#endif // CAMERAS_IMAGE_SAVER_H diff --git a/src/cameras/launch/image_saving.launch b/src/cameras/launch/image_saving.launch new file mode 100644 index 000000000..025277853 --- /dev/null +++ b/src/cameras/launch/image_saving.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/cameras/package.xml b/src/cameras/package.xml index accfb7e22..1daa88bcc 100644 --- a/src/cameras/package.xml +++ b/src/cameras/package.xml @@ -9,14 +9,21 @@ TODO catkin - geometry_msgs roscpp + geometry_msgs + std_msgs + sb_utils + cv_bridge + geometry_msgs + cv_bridge roscpp std_msgs + sb_utils + - \ No newline at end of file + diff --git a/src/cameras/src/ImageSaver.cpp b/src/cameras/src/ImageSaver.cpp new file mode 100644 index 000000000..2e8ac469b --- /dev/null +++ b/src/cameras/src/ImageSaver.cpp @@ -0,0 +1,94 @@ +/* + * Created By: Rowan Zawadzki + * Created On: Aug 8 2022 + * Description: A node that saves images from the realsense cameras CIRC2022 + */ + +#include + +ImageSaver::ImageSaver(int argc, char** argv, std::string node_name) { + // Setup NodeHandles + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + image_transport::ImageTransport it(nh); + // decide which camera to save from + // std::string parameter1 = "camera"; + // SB_getParam(private_nh, parameter1, camera, 2); + // Setup Subscriber(s) + std::string cam1_topic; + std::string cam2_topic; + cam1_topic = "cam_1/color/image_raw"; + cam2_topic = "cam_2/color/image_raw"; + + std::string shutter1_topic = "shutter1_button"; + std::string shutter2_topic = "shutter2_button"; + + int queue_size = 10; + camera1_subscribe = it.subscribe( + cam1_topic, queue_size, &ImageSaver::subscriberCallBack1, this); + camera2_subscribe = it.subscribe( + cam2_topic, queue_size, &ImageSaver::subscriberCallBack2, this); + + int queue_size2 = 4; + shutter1 = nh.subscribe( + shutter1_topic, queue_size2, &ImageSaver::subscriberCallBackShutter1, this); + shutter2 = nh.subscribe( + shutter2_topic, queue_size2, &ImageSaver::subscriberCallBackShutter2, this); +} + +void ImageSaver::subscriberCallBack1(const sensor_msgs::Image::ConstPtr& img1) { + if (PicsToTake1 > 0) { + camera = 1; + + cv::Mat image; + image = cv_bridge::toCvShare(img1, "bgr8")->image; + + ROS_INFO( + "Camera %d shutter has been pushed ---%d--- time(s) so far, taking " + "photo(s) and saving to your Snowflake/docs/CIRC_Images/cam%d--images " + "file.", + camera, + picCounter1 + 1, + camera); + + cv::imwrite( + cv::format("../Snowflake/docs/CIRC_Images/cam1--images/%d.png", + picCounter1), + image); + picCounter1++; + PicsToTake1 -= 1; + void subscriberCallBack(const sensor_msgs::Image::ConstPtr& image); + } +} +void ImageSaver::subscriberCallBack2(const sensor_msgs::Image::ConstPtr& img2) { + if (PicsToTake2 > 0) { + camera = 2; + + cv::Mat image; + image = cv_bridge::toCvShare(img2, "bgr8")->image; + + ROS_INFO( + "Camera %d shutter has been pushed ---%d--- time(s) so far, taking " + "photo(s) and saving to your Snowflake/docs/CIRC_Images/cam%d--images " + "file.", + camera, + picCounter2 + 1, + camera); + + cv::imwrite( + cv::format("../Snowflake/docs/CIRC_Images/cam2--images/%d.png", + picCounter2), + image); + picCounter2++; + PicsToTake2 -= 1; + } +} +void ImageSaver::subscriberCallBackShutter1( +const std_msgs::Int32::ConstPtr& msg) { + PicsToTake1 += msg->data; +} +void ImageSaver::subscriberCallBackShutter2( +const std_msgs::Int32::ConstPtr& msg) { + PicsToTake2 += msg->data; +} diff --git a/src/cameras/src/image_saver.cpp b/src/cameras/src/image_saver.cpp new file mode 100644 index 000000000..7380c3faa --- /dev/null +++ b/src/cameras/src/image_saver.cpp @@ -0,0 +1,23 @@ +/* + * Created By: Ihsan Olawale, Rowan Zawadski + * Created On: July 17th, 2022 + * Description: An example node that subscribes to a topic publishing strings, + * and re-publishes everything it receives to another topic with + * a "!" at the end + */ + +#include + +int main(int argc, char** argv) { + // Setup your ROS node + std::string node_name = "image_saver"; + + // Create an instance of your class + ImageSaver image_saver(argc, argv, node_name); + + // Start up ros. This will continue to run until the node is killed + ros::spin(); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/cameras/src/simple_web_pub.cpp b/src/cameras/src/simple_web_pub.cpp new file mode 100644 index 000000000..78a6291af --- /dev/null +++ b/src/cameras/src/simple_web_pub.cpp @@ -0,0 +1,51 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#include + +#include +#include +#include +//#include +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "image_saver"); + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + + image_transport::CameraPublisher simp; + + ros::spin(); +} \ No newline at end of file diff --git a/src/gps_waypoint_detection/CMakeLists.txt b/src/gps_waypoint_detection/CMakeLists.txt new file mode 100644 index 000000000..d4301f5ba --- /dev/null +++ b/src/gps_waypoint_detection/CMakeLists.txt @@ -0,0 +1,213 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gps_waypoint_detection) + +# 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 +) +find_package(sb_utils REQUIRED) + +## 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 gps_waypoint_detection +# 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(detect_waypoint +# src/${PROJECT_NAME}/detect_waypoint.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(detect_waypoint src/detect_waypoint.cpp src/DetectWaypoint.cpp include/DetectWaypoint.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(detect_waypoint ${catkin_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) diff --git a/src/gps_waypoint_detection/README.md b/src/gps_waypoint_detection/README.md new file mode 100644 index 000000000..0d768b0d7 --- /dev/null +++ b/src/gps_waypoint_detection/README.md @@ -0,0 +1,10 @@ +# Sample Package +- This package is a sample package containing a lot of examples we can point to for basic ROS concepts. +- This package contains a node which acts as a subscriber and a publisher, a common pattern used in most (if not all) ROS nodes. + +# Nodes +## my_node +- This is a sample node which appends an exclamation mark to messages it subscribes to + +### Params +- character: The character appended to the message, defaults to "!" diff --git a/src/gps_waypoint_detection/include/DetectWaypoint.h b/src/gps_waypoint_detection/include/DetectWaypoint.h new file mode 100644 index 000000000..56a1b51fa --- /dev/null +++ b/src/gps_waypoint_detection/include/DetectWaypoint.h @@ -0,0 +1,75 @@ +/* + * Created By: Gareth Ellis + * Created On: July 16th, 2016 + * Description: An example node that subscribes to a topic publishing strings, + * and re-publishes everything it receives to another topic with + * a "!" at the end + */ + +#ifndef GPSWAYPOINTDETECTION_DETECTWAYPOINT.H +#define GPSWAYPOINTDETECTION_DETECTWAYPOINT.H + +// STD Includes +#include + +// ROS Includes +#include +#include +#include +#include +#include + + +// Snowbots Includes +#include + +class DetectWaypoint { +public: + DetectWaypoint(int argc, char **argv, std::string node_name); + /** + * Adds an exclamation point to a given string + * + * Some Longer explanation should go here + * + * @param input_string the string to add an exclamation point to + * + * @return input_string with an exclamation point added to it + */ + +private: + /** + * Callback function for when a new string is received + * + * @param msg the string received in the callback + */ + // void subscriberCallBack(const std_msgs::String::ConstPtr& msg); + /** + * Publishes a given string + * + * @param msg_to_publish the string to publish + */ + // void republishMsg(std::string msg_to_publish); + +//waypoint # -+- -+- one two three four five six seven eight + double WaypointLatitude[8] = { -75.88 , 38.23 , 18.33 , 0 , 0 , 0 , 0 , 0 }; + double WaypointLongitude[8] = { 133.88 , -78.23 , 88.33 , 0 , 0 , 0 , 0 , 0 }; + + double current_target_lat = WaypointLatitude[0]; + double current_target_long = WaypointLongitude[0]; + + + double RoverLong = 0.00; + double RoverLat = 0.00; + ros::Subscriber coord_sub; + ros::Publisher proximity_pub; + bool pathfind = false; + ros::Publisher pathfinder; + void CoordsCallBack(const sensor_msgs::NavSatFix::ConstPtr& coords); + + double CalcProximity(double currLatitude, double currLongitude); //returns proximity in (fake) meters + + + + +}; +#endif //GPSWAYPOINTDETECTION_DETECTWAYPOINT.H diff --git a/src/gps_waypoint_detection/launch/detect_waypoint.launch b/src/gps_waypoint_detection/launch/detect_waypoint.launch new file mode 100644 index 000000000..2c27e8c75 --- /dev/null +++ b/src/gps_waypoint_detection/launch/detect_waypoint.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/src/gps_waypoint_detection/package.xml b/src/gps_waypoint_detection/package.xml new file mode 100644 index 000000000..348515c5d --- /dev/null +++ b/src/gps_waypoint_detection/package.xml @@ -0,0 +1,56 @@ + + + gps_waypoint_detection + 0.0.0 + The weewaa package + + + + + gareth + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + sb_utils + roscpp + std_msgs + sb_utils + + + + + + + + diff --git a/src/gps_waypoint_detection/src/DetectWaypoint.cpp b/src/gps_waypoint_detection/src/DetectWaypoint.cpp new file mode 100644 index 000000000..7eadd34df --- /dev/null +++ b/src/gps_waypoint_detection/src/DetectWaypoint.cpp @@ -0,0 +1,50 @@ +/* + * Created By: Gareth Ellis + * Created On: July 16th, 2016 + * Description: An example node that subscribes to a topic publishing strings, + * and re-publishes everything it receives to another topic with + * a "!" at the end + */ + +#include + +DetectWaypoint::DetectWaypoint(int argc, char **argv, std::string node_name) { + // Setup NodeHandles + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + // Obtains character from the parameter server (or launch file), sets '!' as default + std::string parameter_name = "pathfind"; + std::string default_character = "!"; + SB_getParam(private_nh, parameter_name, pathfind, false); + + // Setup Subscriber(s) + std::string topic_to_subscribe_to = "gps/fix"; + int queue_size = 10; + coord_sub = nh.subscribe( + topic_to_subscribe_to, queue_size, &DetectWaypoint::CoordsCallBack, this); + + // Setup Publisher(s) + std::string topic = private_nh.resolveName("wp_proximity"); + queue_size = 1; + proximity_pub = private_nh.advertise(topic, queue_size); +} + +void DetectWaypoint::CoordsCallBack(const sensor_msgs::NavSatFix::ConstPtr& coords) { + ROS_INFO("Received Coords"); + RoverLat = coords->latitude; + RoverLong = coords->longitude; + ROS_INFO("The coords be lookin like LAT: -- %lf --, LONG: -- %lf -- ", RoverLat, RoverLong); + DetectWaypoint::CalcProximity(RoverLat, RoverLong); + + +} + +double DetectWaypoint::CalcProximity(double currLatitude, double currLongitude){ +double distX = current_target_lat - currLatitude; +double distY = current_target_long - currLongitude; +double proximity = sqrt((distX*distX) + (distY*distY)); +ROS_INFO("proximity: %lf", proximity); + +} diff --git a/src/gps_waypoint_detection/src/detect_waypoint.cpp b/src/gps_waypoint_detection/src/detect_waypoint.cpp new file mode 100644 index 000000000..d50b81767 --- /dev/null +++ b/src/gps_waypoint_detection/src/detect_waypoint.cpp @@ -0,0 +1,24 @@ +/* + * Created By: Gareth Ellis + * Created On: July 16th, 2016 + * Description: An example node that subscribes to a topic publishing strings, + * and re-publishes everything it receives to another topic with + * a "!" at the end + */ + +#include + + +int main(int argc, char **argv){ + // Setup your ROS node + std::string node_name = "detect_waypoint"; + + // Create an instance of your class + DetectWaypoint detect_waypoint(argc, argv, node_name); + + // Start up ros. This will continue to run until the node is killed + ros::spin(); + + // Once the node stops, return 0 + return 0; +} \ No newline at end of file diff --git a/src/gps_waypoint_detection/test/my-node-test.cpp b/src/gps_waypoint_detection/test/my-node-test.cpp new file mode 100644 index 000000000..48e042472 --- /dev/null +++ b/src/gps_waypoint_detection/test/my-node-test.cpp @@ -0,0 +1,18 @@ +/* + * Created By: Gareth Ellis + * Created On: July 16th, 2016 + * Description: Tests for MyNode + */ + +#include +#include + +TEST(MyNode, addExclamationPoint){ + EXPECT_EQ("!", MyClass::addCharacterToString("", "!")); + EXPECT_EQ("Hello!", MyClass::addCharacterToString("Hello", "!")); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/src/gps_waypoint_detection/test/my_node_rostest.cpp b/src/gps_waypoint_detection/test/my_node_rostest.cpp new file mode 100644 index 000000000..f527f038e --- /dev/null +++ b/src/gps_waypoint_detection/test/my_node_rostest.cpp @@ -0,0 +1,68 @@ +/* + * Created By: Valerian Ratu + * Created On: January 29, 2017 + * Description: Integration testing for MyNode + */ + + +#include +#include +#include + + +/** + * This is the helper class which will publish and subscribe messages which will test the node being instantiated + * It contains at the minimum: + * publisher - publishes the input to the node + * subscriber - publishes the output of the node + * callback function - the callback function which corresponds to the subscriber + * getter function - to provide a way for gtest to check for equality of the message recieved + */ +class MyNodeTest : public testing::Test{ +protected: + virtual void SetUp(){ + test_publisher = nh_.advertise("subscribe_topic", 1); + test_subscriber = nh_.subscribe("/my_node/publish_topic", 1, &MyNodeTest::callback, this); + + // Let the publishers and subscribers set itself up timely + ros::Rate loop_rate(1); + loop_rate.sleep(); + } + + ros::NodeHandle nh_; + std::string message_output; + ros::Publisher test_publisher; + ros::Subscriber test_subscriber; + +public: + + void callback(const std_msgs::String::ConstPtr msg){ + message_output = msg->data.c_str(); + } +}; + +TEST_F(MyNodeTest, exclamationMarkAppend){ + + // publishes "Hello" to the test node + std_msgs::String msg; + msg.data = "Hello"; + test_publisher.publish(msg); + + // Wait for the message to get passed around + ros::Rate loop_rate(1); + loop_rate.sleep(); + + // spinOnce allows ros to actually process your callbacks + // for the curious: http://answers.ros.org/question/11887/significance-of-rosspinonce/ + ros::spinOnce(); + + EXPECT_EQ("Hello!", message_output); +} + + +int main(int argc, char **argv) { + // !! Don't forget to initialize ROS, since this is a test within the ros framework !! + ros::init(argc, argv, "my_node_rostest"); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/src/gps_waypoint_detection/test/sample_package_test.test b/src/gps_waypoint_detection/test/sample_package_test.test new file mode 100644 index 000000000..7ac091b33 --- /dev/null +++ b/src/gps_waypoint_detection/test/sample_package_test.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/marker_qr_detection/CMakeLists.txt b/src/marker_qr_detection/CMakeLists.txt new file mode 100644 index 000000000..360978ea2 --- /dev/null +++ b/src/marker_qr_detection/CMakeLists.txt @@ -0,0 +1,226 @@ +cmake_minimum_required(VERSION 2.8.3) +project(marker_qr_detection) + +# 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 + cv_bridge + sensor_msgs + image_transport +) +find_package(OpenCV REQUIRED) +find_package(sb_utils REQUIRED) + +## 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 marker_qr_detection +# 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} + ${OpenCV_INCLUDE_DIRS} + ./include +) + +## Declare a C++ library +# add_library(marker_qr_detection +# src/${PROJECT_NAME}/marker_qr_detection.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(marker_qr_detection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(marker_qr_detection_node src/sample_package_node.cpp) +add_executable(detect_marker src/detect_marker.cpp src/DetectMarker.cpp include/DetectMarker.h) + + + + + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(marker_qr_detection_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(marker_qr_detection_node +# ${catkin_LIBRARIES} +# ) +target_link_libraries(detect_marker ${catkin_LIBRARIES} ${OpenCV_LIBS} ${sb_utils_LIBRARIES}) + + + +# add_executable(detect_qr_code src/detect_qr_code.cpp src/DetectQRCode.cpp include/DetectQRCode.h) +# target_link_libraries(detect_qr_code ${catkin_LIBRARIES} ${OpenCV_LIBS} ${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 marker_qr_detection 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/DetectMarker.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(detect_marker_rostest test/marker_qr_detection_test.test test/detect_marker_rostest.cpp) +# target_link_libraries(detect_marker_rostest ${catkin_LIBRARIES}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/marker_qr_detection/include/DetectMarker.h b/src/marker_qr_detection/include/DetectMarker.h new file mode 100644 index 000000000..2ac28dd53 --- /dev/null +++ b/src/marker_qr_detection/include/DetectMarker.h @@ -0,0 +1,58 @@ +/* + * Created By: Ihsan Olawale, Rowan Zawadski + * Created On: July 17th, 2022 + * Description: An example node that subscribes to a topic publishing strings, + * and re-publishes everything it receives to another topic with + * a "!" at the end + */ + +#ifndef MARKER_QR_DETECTION_DETECT_MARKER_H +#define MARKER_QR_DETECTION_DETECT_MARKER_H + +// OpenCV +#include +#include + +// Image Conversion +#include +#include +#include +#include + +// STD Includes +#include +#include + +// ROS Includes +#include +#include + +// Snowbots Includes +#include + +class DetectMarker { + public: + DetectMarker(int argc, char** argv, std::string node_name); + + private: + /** + * Callback function for when a new string is received + * + * @param msg the string received in the callback + */ + void subscriberCallBack(const sensor_msgs::Image::ConstPtr& msg); + + std::vector fetchMarkerIds(const cv::Mat& image); + + cv::Mat rosToMat(const sensor_msgs::Image::ConstPtr& image); + + image_transport::Subscriber my_subscriber; + ros::Publisher my_publisher; + image_transport::Publisher bounder; + + cv::Ptr dictionary; + cv::Ptr parameters; + bool draw_markers = false; + int camera = 1; +}; +#endif // MARKER_QR_DETECTION_DETECT_MARKER_H diff --git a/src/marker_qr_detection/launch/arcu_detect.launch b/src/marker_qr_detection/launch/arcu_detect.launch new file mode 100644 index 000000000..2d002ed65 --- /dev/null +++ b/src/marker_qr_detection/launch/arcu_detect.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/marker_qr_detection/package.xml b/src/marker_qr_detection/package.xml new file mode 100644 index 000000000..5a3f6a618 --- /dev/null +++ b/src/marker_qr_detection/package.xml @@ -0,0 +1,59 @@ + + + marker_qr_detection + 0.0.0 + The marker_qr_detection package + + + + + ihsan + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + sb_utils + cv_bridge + + roscpp + std_msgs + cv_bridge + sb_utils + + + + + + + + diff --git a/src/marker_qr_detection/src/DetectMarker.cpp b/src/marker_qr_detection/src/DetectMarker.cpp new file mode 100644 index 000000000..12309c2af --- /dev/null +++ b/src/marker_qr_detection/src/DetectMarker.cpp @@ -0,0 +1,87 @@ +/* + * Created By: Ihsan Olawale, Rowan Zawadski + * Created On: July 17th, 2022 + * Description: A node that subscribes and scans arcu codes on with the + * realsense cameras + */ + +#include + +DetectMarker::DetectMarker(int argc, char** argv, std::string node_name) { + // Setup NodeHandles + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + // Setup image transport + image_transport::ImageTransport it(nh); + + // Initialize ArUco parameters + dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + parameters = cv::aruco::DetectorParameters::create(); + + std::string topic_to_subscribe_to; + + // Obtains draw_markers parameter from the parameter server (or launch file) + std::string parameter_name = "draw_markers"; + SB_getParam(private_nh, parameter_name, draw_markers, false); + std::string parameter_name2 = "camera"; + SB_getParam(private_nh, parameter_name2, camera, 1); + // Setup Subscriber(s) + if (camera == 1) { + topic_to_subscribe_to = "cam_1/color/image_raw"; + } else { + topic_to_subscribe_to = "cam_2/color/image_raw"; + } + int queue_size = 5; + my_subscriber = it.subscribe( + topic_to_subscribe_to, queue_size, &DetectMarker::subscriberCallBack, this); + + // Setup Publisher(s) + // marker id data (string) + std::string topic = private_nh.resolveName("identified"); + queue_size = 10; + my_publisher = private_nh.advertise(topic, queue_size); + + // image detection publisher + bounder = it.advertise("bounding_boxes", 1); + + ROS_INFO("initiation appears successfull."); + if (draw_markers) { + ROS_INFO("I WILL ATTEMPT TO DRAW DETECTED MARKERS"); + } else { + ROS_INFO( + "I WILL ***********NOT************* ATTEMPT TO DRAW DETECTED MARKERS"); + } +} + +void DetectMarker::subscriberCallBack(const sensor_msgs::Image::ConstPtr& msg) { + ROS_INFO("Received message"); + std::vector markerIds = fetchMarkerIds(rosToMat(msg)); + std::cout << "Markers detected:"; + for (int markerId : markerIds) { std::cout << " " << markerId; } + std::cout << std::endl; +} + +std::vector DetectMarker::fetchMarkerIds(const cv::Mat& image) { + std::vector markerIds; + std::vector> markerCorners; + cv::aruco::detectMarkers( + image, dictionary, markerCorners, markerIds, parameters); + if (draw_markers) { + cv::Mat outputImage; + image.copyTo(outputImage); + cv::aruco::drawDetectedMarkers(outputImage, markerCorners, markerIds); + bounder.publish( + cv_bridge::CvImage(std_msgs::Header(), "bgr8", outputImage) + .toImageMsg()); + ROS_INFO("attempted draw"); + } + return markerIds; +} + +cv::Mat DetectMarker::rosToMat(const sensor_msgs::Image::ConstPtr& image) { + cv_bridge::CvImagePtr image_ptr; + image_ptr = cv_bridge::toCvCopy(image, image->encoding); + return image_ptr->image; +} diff --git a/src/marker_qr_detection/src/detect_marker.cpp b/src/marker_qr_detection/src/detect_marker.cpp new file mode 100644 index 000000000..8a8881d34 --- /dev/null +++ b/src/marker_qr_detection/src/detect_marker.cpp @@ -0,0 +1,22 @@ +/* + * Created By: Ihsan Olawale, Rowan Zawadski + * Created On: July 17th, 2022 + * Description: A node that subscribes and scans arcu codes on with the + * realsense cameras + */ + +#include + +int main(int argc, char** argv) { + // Setup your ROS node + std::string node_name = "detect_marker"; + + // Create an instance of your class + DetectMarker detect_marker(argc, argv, node_name); + + // Start up ros. This will continue to run until the node is killed + ros::spin(); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/move_group_arm/include/moveGroup.h b/src/move_group_arm/include/moveGroup.h new file mode 100644 index 000000000..6646647ec --- /dev/null +++ b/src/move_group_arm/include/moveGroup.h @@ -0,0 +1,29 @@ +#include +#include + +#include +#include + +#include +#include + +#include + +class MoveGroupArm { + public: + MoveGroupArm(int argc, char** argv, string node_name); + + private: + void updatePose(const sb_msgs::ArmPosition::ConstPtr& observed_msg); + void executePose(const std_msgs::Bool::ConstPtr& inMsg); + double degToRad(double deg); + void init(); + + ros::Subscriber curPos; + ros::Subscriber subExecute; + + std::vector actuator_positions_; + std::vector joint_positions_; + std::vector joint_group_positions; + int num_joints_ = 6; +} \ No newline at end of file diff --git a/src/move_group_arm/src/moveGroup.cpp b/src/move_group_arm/src/moveGroup.cpp new file mode 100644 index 000000000..38e6a861e --- /dev/null +++ b/src/move_group_arm/src/moveGroup.cpp @@ -0,0 +1,79 @@ +/* + * Created By: Tate Kolton + * Created On: August 9, 2022 + * Description: Uses move group interface to update current joint pose in moveit when switching + * from joint space mode to cartesian mode (IK) + */ + +#include "../include/moveGroup.h" + +MoveGroupArm::MoveGroupArm(int argc, char** argv, string node_name) +{ + + ros::init(argc, argv, node_name); + ros::NodeHandle node_handle; + subPos = + node_handle.subscribe("/observed_pos_arm", 1, &updatePose, this); + subExecute = + node_handle.subscribe("/move_group_trigger", 1, &executePose, this); + + init(); +} + +void MoveGroupArm::executePose(const std_msgs::Bool::ConstPtr& inMsg) { + + // fetches current state of rover + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); + + // writes new position command + for(int i=0; ipositions.begin(), observed_msg->positions.end()); +} + +void MoveGroupArm::init() +{ + actuator_positions_.resize(num_joints_); + joint_positions_.resize(num_joints_); + joint_group_positions.resize(num_joints_); + static const std::string PLANNING_GROUP = "arm"; + + // The :move_group_interface:`MoveGroupInterface` class can be easily + // setup using just the name of the planning group you would like to control and plan for. + moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + + // Raw pointers are frequently used to refer to the planning group for improved performance. + const robot_state::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); +} + +double ArmHardwareInterface::degToRad(double deg) +{ + return deg / 180.0 * 3.14159; +} \ No newline at end of file diff --git a/src/move_group_arm/src/move_group.cpp b/src/move_group_arm/src/move_group.cpp new file mode 100644 index 000000000..4b416fb14 --- /dev/null +++ b/src/move_group_arm/src/move_group.cpp @@ -0,0 +1,21 @@ +/* + * Created By: Tate Kolton + * Created On: August 9, 2022 + * Description: Move groups interface for arm + */ + +#include "../include/moveGroup.h" + +int main(int argc, char** argv) { + // Setup your ROS node + std::string node_name = "move_group"; + + // Create an instance of your class + MoveGroupArm arm(argc, argv, node_name); + + // Start up ros. This will continue to run until the node is killed + ros::spin(); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/procontroller_snowbots/include/ProController.h b/src/procontroller_snowbots/include/ProController.h index 579f46a64..cd6180d52 100644 --- a/src/procontroller_snowbots/include/ProController.h +++ b/src/procontroller_snowbots/include/ProController.h @@ -11,11 +11,15 @@ #include #include #include +#include +#include #include #include #include #include #include +#include + using namespace std; class ProController { @@ -25,6 +29,7 @@ class ProController { private: void setup(); void readInputs(); + bool inDeadzone(int value); void leftJoystickX(int value); // ABS_X void leftJoystickY(int value); // ABS_Y void rightJoystickX(int value); // ABS_RX @@ -44,9 +49,8 @@ class ProController { void arrowsUorD(int value); // ABS_HAT0Y void leftJoystickPress(int value); // BTN_THUMBL void rightJoystickPress(int value); // BTN_THUMBR - tuple - publishMoveXZ(double x_new, double z_new, double x_old, double z_old); - void publishArmXZ(double x_new, double z_new, double x_old, double z_old); + tuple publishMoveXZ(double x_new, double z_new, double x_old, double z_old); + void publishArmMessage(std::string outMsg); void printState(); void printControllerDebug(int type, int code, int value); // see documentation to changes sensitivities at runtime @@ -54,12 +58,63 @@ class ProController { double Z_SENSITIVITY = 1.0; double x; double z; + std::string armOutMsg, armOutVal; +// character representations of buttons for arm communication + const char leftJSU = 'A'; + const char leftJSD = 'B'; + const char rightJSU = 'C'; + const char rightJSD = 'D'; + const char buttonA = 'E'; + const char buttonB = 'F'; + const char buttonX = 'G'; + const char buttonY = 'H'; + const char triggerL = 'I'; + const char triggerR = 'J'; + const char bumperL = 'K'; + const char bumperR = 'L'; + const char buttonARel = 'M'; + const char buttonBRel = 'N'; + const char buttonXRel = 'O'; + const char buttonYRel = 'P'; + const char triggerLRel = 'Q'; + const char triggerRRel = 'R'; + const char bumperLRel = 'S'; + const char bumperRRel = 'T'; + const char arrowL = 'U'; + const char arrowR = 'V'; + const char arrowU = 'W'; + const char arrowD = 'X'; + const char arrowRLRel = '0'; + const char arrowUDRel = '5'; + const char leftJSRel = 'Y'; + const char rightJSRel = 'Z'; + const char rightJSPress = '7'; + const char rightJSPressRel = '8'; + const char homeVal = '4'; + const char homeValEE = '6'; + const char J1 = '1'; + const char J2 = '2'; + const char J3 = '3'; + const char J4 = '4'; + // arm modes + const char jointMode = '1'; + const char IKMode = '2'; + const char drillMode = '3'; + struct libevdev* dev = NULL; - enum Mode { wheels = 0, arm = 1 }; + enum Mode { wheels = 0, arm_joint_space = 1, arm_cartesian = 2, drilling = 3, num_modes = 2 }; Mode state; bool debug = false; ros::Publisher pubmove; ros::Publisher pubarm; + ros::Publisher pubmode; + ros::Publisher pubmovegrp; + + std_msgs::Bool true_message; + std_msgs::Bool false_message; + int speed = 50; + int max_speed = 100; + int increment = 10; }; #endif // PROCONTROLLER_SNOWBOTS_CONTROLLER_H diff --git a/src/procontroller_snowbots/src/ProController.cpp b/src/procontroller_snowbots/src/ProController.cpp index 467d67fbd..f6cb159a4 100755 --- a/src/procontroller_snowbots/src/ProController.cpp +++ b/src/procontroller_snowbots/src/ProController.cpp @@ -10,7 +10,9 @@ // Read the master documentation if there's any issues with this package ProController::ProController(int argc, char** argv, string node_name) { string publisher = "/cmd_vel"; - setup(); + string armPublisher = "/cmd_arm"; + string modePublisher = "/moveit_toggle"; + // string moveGrpPublisher = "/move_group_trigger"; ros::init(argc, argv, node_name); ros::NodeHandle private_nh("~"); if (private_nh.param("X", X_SENSITIVITY, 1.0)) { @@ -22,8 +24,15 @@ ProController::ProController(int argc, char** argv, string node_name) { if (private_nh.param("debug", debug, false)) { ROS_INFO("Debug mode %s", (debug) ? "on" : "off"); } + setup(); pubmove = private_nh.advertise(publisher, 1); - pubarm = private_nh.advertise("/cmd_arm", 1); + pubarm = private_nh.advertise(armPublisher, 1); + pubmode = private_nh.advertise(modePublisher, 1); + // pubmovegrp = private_nh.advertise(moveGrpPublisher,1); + + true_message.data = true; + false_message.data = false; + ROS_INFO("Preparing to read inputs...\n"); state = Mode::wheels; printState(); @@ -57,10 +66,10 @@ void ProController::setup() { libevdev_get_name(dev), path); if (debug) { - ROS_INFO("Input device ID: bus %#x vendor %#x product %#x\n", - libevdev_get_id_bustype(dev), - libevdev_get_id_vendor(dev), - libevdev_get_id_product(dev)); + ROS_DEBUG("Input device ID: bus %#x vendor %#x product %#x\n", + libevdev_get_id_bustype(dev), + libevdev_get_id_vendor(dev), + libevdev_get_id_product(dev)); } break; } @@ -87,6 +96,10 @@ void ProController::readInputs() { if (debug) { printControllerDebug(ev.type, ev.code, ev.value); } else { + + // vehicle for arm output message + armOutMsg = ""; + armOutVal = ""; // handle all controller inputs using API functions switch (ev.code) { case ABS_X: leftJoystickX(ev.value); break; @@ -113,8 +126,23 @@ void ProController::readInputs() { if (state == Mode::wheels) { // Publish motion, update x and z old using tuple tie(x_old, z_old) = publishMoveXZ(x, z, x_old, z_old); - } else if (state == Mode::arm) { - publishArmXZ(x, z, x_old, z_old); + } + else if (state == Mode::arm_joint_space) { // Joint space control of the arm + armOutMsg += jointMode; + armOutMsg += armOutVal; + publishArmMessage(armOutMsg); + } + + else if (state == Mode::arm_cartesian) { // Inverse Kinematics mode i.e. cartesian motion + armOutMsg += IKMode; + armOutMsg += armOutVal; + publishArmMessage(armOutMsg); + } + + else { // Drilling mode for arm + armOutMsg += drillMode; + armOutMsg += armOutVal; + publishArmMessage(armOutMsg); } } } @@ -126,15 +154,19 @@ void ProController::readInputs() { void ProController::printControllerDebug(int type, int code, int value) { auto codeout = libevdev_event_code_get_name(type, code); auto typeout = libevdev_event_type_get_name(type); - ROS_INFO("Event: Type: %s Code: %s Value: %d\n", typeout, codeout, value); + ROS_DEBUG("Event: Type: %s Code: %s Value: %d\n", typeout, codeout, value); } // Prints a status message detailing the current control mode void ProController::printState() { if (state == Mode::wheels) { ROS_INFO("Current mode: controlling wheels"); - } else if (state == Mode::arm) { - ROS_INFO("Current mode: controlling arm"); + } else if (state == Mode::arm_joint_space) { + ROS_INFO("Current mode: controlling arm in the joint space"); + } else if (state == Mode::arm_cartesian) { + ROS_INFO("Current mode: controlling arm in the cartesian space"); + } else if (state == Mode::drilling) { + ROS_INFO("Current mode: drilling with the arm"); } else { ROS_INFO("There is no current mode, which is a problem"); } @@ -148,8 +180,8 @@ tuple ProController::publishMoveXZ(double x_new, double z_old) { if (abs(x_old - x_new) > 0.0001 || abs(z_old - z_new) > 0.0001) { geometry_msgs::Twist msg; - msg.linear.x = x_new; - msg.angular.z = z_new; + msg.linear.x = x_new * speed / 100; + msg.angular.z = z_new * speed / 100; pubmove.publish(msg); // return tuple return make_tuple(x_new, z_new); @@ -157,125 +189,173 @@ tuple ProController::publishMoveXZ(double x_new, return make_tuple(x_old, z_old); } -// The place to implement arm commands if they use left joystick x and y -void ProController::publishArmXZ(double x_new, - double z_new, - double x_old, - double z_old) { - // use pubarm to do something with x and z inputs +// If controller recieves new commands and is in an arm mode, send message to arm +void ProController::publishArmMessage(std::string outMsg) { + std_msgs::String outMsgWrapper; + outMsg += '\n'; + outMsgWrapper.data = outMsg; + pubarm.publish(outMsgWrapper); +} + +bool ProController::inDeadzone(int value) { + return state == Mode::wheels ? value > 108 && value < 148 : value > 88 && value < 168 ; } // Updates z, which is then published by publish___XZ in readInputs() void ProController::leftJoystickX(int value) { - if (value > 115 && value < 135) { - z = 0; - } else { + + if (inDeadzone(value)) { + if(state == Mode::wheels) { + x = 0; + } + } + + else { // 128 is the center, so this normalizes the result to // [-1,1]*Z_SENSITIVITY ROS_INFO("Left Joystick X event with value: %d\n", value); - z = -(value - 128) / 128.0 * Z_SENSITIVITY; + x = (value - 128) / 128.0 * X_SENSITIVITY; } } // Updates x, which is then published by publish___XZ in readInputs() void ProController::leftJoystickY(int value) { - if (value > 115 && value < 135) { - x = 0; + if (inDeadzone(value)) { + if(state == Mode::wheels) { + z = 0; + } + + else if(state == Mode::arm_joint_space){ + armOutVal = leftJSRel; + } } else { + // 128 is the center, so this normalizes the result to // [-1,1]*X_SENSITIVITY ROS_INFO("Left Joystick Y event with value: %d\n", value); - x = (value - 128) / 128.0 * X_SENSITIVITY; + z = -(value - 128) / 128.0 * Z_SENSITIVITY; + if(z > 0) { + armOutVal = leftJSU; + } + + else { + armOutVal = leftJSD; + } } } // Currently doing nothing void ProController::rightJoystickX(int value) { - if (value > 118 && value < 137) { + if (inDeadzone(value)) { // do nothing } else { ROS_INFO("Right Joystick X event with value: %d\n", value); } } -// Currently doing nothing void ProController::rightJoystickY(int value) { - if (value > 118 && value < 137) { - // do nothing - } else { + + if (inDeadzone(value)) { + if(state != Mode::wheels) + armOutVal = rightJSRel; + } + + else { + + // 128 is the center, so this normalizes the result to + // [-1,1]*Z_SENSITIVITY ROS_INFO("Right Joystick Y event with value: %d\n", value); + z = -(value - 128) / 128.0 * Z_SENSITIVITY; + + // Right joystick is only used in Y direction in all arm modes + if(state != Mode::wheels) { + + if(z > 0) { + armOutVal = rightJSU; + } + + else { + armOutVal = rightJSD; + } + } } } -// Currently doing nothing void ProController::A(int value) { if (value == 1) { ROS_INFO("A button pressed"); + armOutVal = buttonA; } else if (value == 0) { ROS_INFO("A button released"); + armOutVal = buttonARel; } } -// Currently doing nothing void ProController::B(int value) { if (value == 1) { ROS_INFO("B button pressed"); + armOutVal = buttonB; } else if (value == 0) { ROS_INFO("B button released"); + armOutVal = buttonBRel; } } -// Currently doing nothing void ProController::X(int value) { if (value == 1) { ROS_INFO("X button pressed"); + armOutVal = buttonX; } else if (value == 0) { ROS_INFO("X button released"); + armOutVal = buttonXRel; } } -// Currently doing nothing void ProController::Y(int value) { if (value == 1) { ROS_INFO("Y button pressed"); + armOutVal = buttonY; } else if (value == 0) { ROS_INFO("Y button released"); + armOutVal = buttonYRel; } } -// Currently doing nothing void ProController::leftBumper(int value) { if (value == 1) { ROS_INFO("Left bumper pressed"); + armOutVal = bumperL; } else if (value == 0) { ROS_INFO("Left bumper released"); + armOutVal = bumperLRel; } } -// Currently doing nothing void ProController::rightBumper(int value) { if (value == 1) { ROS_INFO("Right bumper pressed"); + armOutVal = bumperR; } else if (value == 0) { ROS_INFO("Right bumper released"); + armOutVal = bumperRRel; } } -// Currently doing nothing void ProController::select(int value) { if (value == 1) { ROS_INFO("Select button pressed"); } else if (value == 0) { ROS_INFO("Select button released"); + armOutVal = homeValEE; } } -// Currently doing nothing void ProController::start(int value) { if (value == 1) { ROS_INFO("Start button pressed"); } else if (value == 0) { ROS_INFO("Start button released"); + armOutVal = homeVal; } } @@ -285,54 +365,70 @@ void ProController::home(int value) { if (value == 1) { ROS_INFO("Home button pressed"); } else if (value == 0) { - if (state == Mode::wheels) { - state = Mode::arm; - printState(); + state = static_cast((state + 1) % (Mode::num_modes)); + if (state == Mode::wheels || state == Mode::arm_joint_space) { + pubmode.publish(false_message); } else { - state = Mode::wheels; - printState(); + // pubmovegrp.publish(true_message); + // sleep(8); + pubmode.publish(true_message); } + printState(); } } } -// Currently doing nothing void ProController::leftTrigger(int value) { if (value == 255) { ROS_INFO("Left trigger pressed"); + armOutVal = triggerL; } else if (value == 0) { ROS_INFO("Left trigger released"); + armOutVal = triggerLRel; } } -// Currently doing nothing void ProController::rightTrigger(int value) { if (value == 255) { ROS_INFO("Right trigger pressed"); + armOutVal = triggerR; } else if (value == 0) { ROS_INFO("Right trigger released"); + armOutVal = triggerRRel; } } -// Currently doing nothing void ProController::arrowsRorL(int value) { if (value == 1) { ROS_INFO("Right button pressed"); + armOutVal = arrowR; } else if (value == 0) { ROS_INFO("Arrow button released"); + armOutVal = arrowRLRel; } else { ROS_INFO("Left button pressed"); + armOutVal = arrowL; } } -// Currently doing nothing void ProController::arrowsUorD(int value) { if (value == 1) { ROS_INFO("Up button pressed"); + armOutVal = arrowU; + if (state == Mode::wheels) { + speed = speed < max_speed ? speed + increment : speed; + ROS_INFO("Speed increased to %d%% of max output", speed); + } } else if (value == 0) { ROS_INFO("Arrow button released"); + armOutVal = arrowUDRel; } else { ROS_INFO("Down button pressed"); + armOutVal = arrowD; + if (state == Mode::wheels) { + speed = speed > increment ? speed - increment : speed; + ROS_INFO("Speed decreased to %d%% of max output", speed); + } } } @@ -349,7 +445,9 @@ void ProController::leftJoystickPress(int value) { void ProController::rightJoystickPress(int value) { if (value == 1) { ROS_INFO("Right Joystick pressed"); + armOutVal = rightJSPress; } else if (value == 0) { ROS_INFO("Right Joystick released"); + armOutVal = rightJSPressRel; } } diff --git a/src/sb_msgs/CMakeLists.txt b/src/sb_msgs/CMakeLists.txt new file mode 100644 index 000000000..fdd4b0e41 --- /dev/null +++ b/src/sb_msgs/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sb_msgs) + +## 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 + roscpp + std_msgs + message_generation + ) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## Generate messages in the 'msg' folder +add_message_files( + DIRECTORY msg + FILES ArmPosition.msg +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES std_msgs +) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) diff --git a/src/sb_msgs/msg/ArmPosition.msg b/src/sb_msgs/msg/ArmPosition.msg new file mode 100644 index 000000000..546579355 --- /dev/null +++ b/src/sb_msgs/msg/ArmPosition.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +float64[] positions diff --git a/src/sb_msgs/package.xml b/src/sb_msgs/package.xml new file mode 100644 index 000000000..c727678f2 --- /dev/null +++ b/src/sb_msgs/package.xml @@ -0,0 +1,26 @@ + + + sb_msgs + 0.0.0 + + This package contains miscellaneous messages developed by UBC Snowbots + + + ihsan + + TODO + + catkin + + message_generation + std_msgs + + message_runtime + std_msgs + + + + + + + diff --git a/src/snowbots_arm_simplified_configs/config/joint_limits.yaml b/src/snowbots_arm_simplified_configs/config/joint_limits.yaml index 5b13a54ee..ca66e7f8f 100755 --- a/src/snowbots_arm_simplified_configs/config/joint_limits.yaml +++ b/src/snowbots_arm_simplified_configs/config/joint_limits.yaml @@ -3,32 +3,50 @@ # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: j1: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 + has_position_limits: true + max_position: 3.1415 + min_position: -3.1415 + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: true + max_acceleration: 0.3 j2: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 + has_position_limits: true + max_position: 2.04 + min_position: -0.75049 + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: true + max_acceleration: 0.3 j3: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 + has_position_limits: true + max_position: 1.48 + min_position: -0.6108 + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: true + max_acceleration: 0.3 j4: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 + has_position_limits: true + max_position: 1.57 + min_position: -1.57 + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: true + max_acceleration: 0.3 j5: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 + has_position_limits: true + max_position: 1.74 + min_position: -1.396 + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: true + max_acceleration: 0.3 j6: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + has_position_limits: true + max_position: 1.484 + min_position: -1.134 + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: true + max_acceleration: 0.3 diff --git a/src/snowbots_arm_simplified_configs/config/kinematics.yaml b/src/snowbots_arm_simplified_configs/config/kinematics.yaml index 5e11fce29..a65c1af0e 100755 --- a/src/snowbots_arm_simplified_configs/config/kinematics.yaml +++ b/src/snowbots_arm_simplified_configs/config/kinematics.yaml @@ -1,8 +1,4 @@ arm: - kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 -end_effector: kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/src/snowbots_arm_simplified_configs/config/ros_controllers.yaml b/src/snowbots_arm_simplified_configs/config/ros_controllers.yaml index c2a311dbc..c43878853 100755 --- a/src/snowbots_arm_simplified_configs/config/ros_controllers.yaml +++ b/src/snowbots_arm_simplified_configs/config/ros_controllers.yaml @@ -1,28 +1,25 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: arm - joint_model_group_pose: default -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - j1 - - j2 - - j3 - - j4 - - j5 - - j6 - sim_control_mode: 1 # 0: position, 1: velocity -# Publish all joint states -# Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 +## Simulation settings for using moveit_sim_controllers +#moveit_sim_hw_interface: +# joint_model_group: arm +# joint_model_group_pose: default +## Settings for ros_control_boilerplate control loop +#generic_hw_control_loop: +# loop_hz: 300 +# cycle_time_error_threshold: 0.01 +## Settings for ros_control hardware interface +#hardware_interface: +# joints: +# - j1 +# - j2 +# - j3 +# - j4 +# - j5 +# - j6 +# sim_control_mode: 1 # 0: position, 1: velocity +## Publish all joint states +## Creates the /joint_states topic necessary in ROS controller_list: - - name: arm_controller + - name: /controllers/position action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory @@ -33,14 +30,49 @@ controller_list: - j4 - j5 - j6 - - name: joint_controller - action_ns: follow_joint_trajectory - default: True - type: FollowJointTrajectory - joints: - - j1 - - j2 - - j3 - - j4 - - j5 - - j6 \ No newline at end of file +#arm_controller: +# type: position_controllers/JointPositionController +# joints: +# - j1 +# - j2 +# - j3 +# - j4 +# - j5 +# - j6 +# gains: +# j1: +# p: 100 +# d: 1 +# i: 1 +# i_clamp: 1 +# j2: +# p: 100 +# d: 1 +# i: 1 +# i_clamp: 1 +# j3: +# p: 100 +# d: 1 +# i: 1 +# i_clamp: 1 +# j4: +# p: 100 +# d: 1 +# i: 1 +# i_clamp: 1 +# j5: +# p: 100 +# d: 1 +# i: 1 +# i_clamp: 1 +# j6: +# p: 100 +# d: 1 +# i: 1 +# i_clamp: 1 +#joint_controller: +# type: position_controllers/JointTrajectoryController +# joints: +# [] +# gains: +# {} diff --git a/src/snowbots_arm_simplified_configs/launch/experiment.launch b/src/snowbots_arm_simplified_configs/launch/experiment.launch new file mode 100644 index 000000000..e5a90c582 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/launch/experiment.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/snowbots_arm_simplified_configs/launch/move_group.launch b/src/snowbots_arm_simplified_configs/launch/move_group.launch index 778ac33ee..b67e40d89 100755 --- a/src/snowbots_arm_simplified_configs/launch/move_group.launch +++ b/src/snowbots_arm_simplified_configs/launch/move_group.launch @@ -55,8 +55,7 @@ - - + diff --git a/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch b/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch old mode 100755 new mode 100644 index a4605c037..1f030f098 --- a/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch +++ b/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch @@ -4,12 +4,13 @@ - - - - + + + + + args="$(arg command_args)" output="screen"> + diff --git a/src/snowbots_arm_simplified_configs/launch/ros_controllers.launch b/src/snowbots_arm_simplified_configs/launch/ros_controllers.launch index 473f300ba..7d56ed430 100755 --- a/src/snowbots_arm_simplified_configs/launch/ros_controllers.launch +++ b/src/snowbots_arm_simplified_configs/launch/ros_controllers.launch @@ -6,6 +6,6 @@ + output="screen" args="arm_controller joint_controller "/> diff --git a/src/snowbots_arm_simplified_configs/launch/snowbots_arm_moveit_bringup.launch b/src/snowbots_arm_simplified_configs/launch/snowbots_arm_moveit_bringup.launch new file mode 100644 index 000000000..372a1dd84 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/launch/snowbots_arm_moveit_bringup.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/snowbots_arm_simplified_configs/launch/trajectory_execution.launch.xml b/src/snowbots_arm_simplified_configs/launch/trajectory_execution.launch.xml index cdc56b8ca..6516f9cfa 100755 --- a/src/snowbots_arm_simplified_configs/launch/trajectory_execution.launch.xml +++ b/src/snowbots_arm_simplified_configs/launch/trajectory_execution.launch.xml @@ -1,7 +1,6 @@ - @@ -16,8 +15,6 @@ - diff --git a/src/snowbots_arm_simplified_configs/package.xml b/src/snowbots_arm_simplified_configs/package.xml index 5c3403c5b..d8538b1e8 100755 --- a/src/snowbots_arm_simplified_configs/package.xml +++ b/src/snowbots_arm_simplified_configs/package.xml @@ -16,13 +16,7 @@ catkin - moveit moveit_ros_move_group - rqt - joy - joystick_drivers - ros_controllers - ros_control moveit_fake_controller_manager moveit_kinematics moveit_planners_ompl diff --git a/src/teensy-firmware/src/armFirmware.cpp b/src/teensy-firmware/src/armFirmware.cpp new file mode 100644 index 000000000..ecaddd288 --- /dev/null +++ b/src/teensy-firmware/src/armFirmware.cpp @@ -0,0 +1,636 @@ +/* +Created By: Tate Kolton, Rowan Zawadzki +Created On: December 21, 2021 +Updated: April 27, 2022 +Description: Main firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU +*/ + +#include + +// general parameters +#define NUM_AXES 6 +#define NUM_AXES_EX_WRIST 4 +#define NUM_AXES_EFF 8 +#define NUM_PARAMS 7 +#define ON 0 +#define OFF 1 +#define SW_ON 0 +#define SW_OFF 1 +#define FWD 1 +#define REV 0 + +int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; + +static const char release = 'R'; +static const char move = 'M'; +static const char change = 'A'; +static const char speed = 'S'; +static const char right = 'R'; +static const char left = 'L'; +static const char up = 'U'; +static const char down = 'D'; +static const char wrist = 'W'; +static const char garbage = 'G'; +static const char faster = 'U'; +static const char slower = 'D'; + +int stepPins[8] = {11, 9, 5, 7, 1, 3, 1, 3}; +int dirPins[8] = {10, 8, 4, 6, 0, 2, 0, 2}; + +// limit switch pins +int limPins[6] = {23, 22, 20, 21, 18, 19}; + +// pulses per revolution for motors +long ppr[6] = {400, 400, 400, 400, 400, 400}; + +// Gear Reduction Ratios +float red[6] = {30.0, 161.0, 44.8, 100, 57.34, 57.34}; + +// Encoder pulses per revolution +long pprEnc = 512; + +// Motor speeds and accelerations +int maxSpeed[8] = {1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200}; +int maxAccel[8] = {900, 3000, 4000, 3000, 5000, 5000, 5000, 5000}; +int homeSpeed[8] = {500, 1200, 600, 400, 2000, 2000, 2000, 2000}; // {500, 1500, 700, 1200, 1200, 1200, 1200, 1200}; +int homeAccel[8] = {500, 2000, 1500, 1000, 1500, 1500, 1500, 1500}; //{500, 2000, 1000, 1500, 1500, 1500, 1500, 1500}; + +// Range of motion (degrees) for each axis +int maxAngles[6] = {180, 70, 180, 120, 140, 100}; + +const unsigned long readInterval = 10; +unsigned long currentTime; +unsigned long previousTime = 0; + +// stepper motor objects for AccelStepper library +AccelStepper steppers[8]; + +// variable declarations +long max_steps[] = {red[0]*maxAngles[0]/360.0*ppr[0], red[1]*maxAngles[1]/360.0*ppr[1], red[2]*maxAngles[2]/360.0*ppr[2], red[3]*maxAngles[3]/360.0*ppr[3], red[4]*maxAngles[4]/360.0*ppr[4], red[5]*maxAngles[5]/360.0*ppr[5]}; +int axisDir[8] = {1, -1, 1, -1, 1, 1, -1, 1}; +int currentAxis = 1; +int runFlags[] = {0, 0, 0, 0, 0, 0}; +int i; +bool initFlag = false; + +// variables for homing / arm calibration +long homePosConst = -99000; +long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst, axisDir[6]*homePosConst, axisDir[7]*homePosConst}; +long homeCompAngles[] = {axisDir[0]*90, axisDir[1]*5, axisDir[2]*93, axisDir[3]*12, axisDir[4]*102, axisDir[5]*102, axisDir[6]*80, axisDir[7]*80}; +long homeCompConst[] = {2000, 2000, 1000, 500, 500, 500, 500, 500}; +long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5], axisDir[6]*homeCompConst[6], axisDir[7]*homeCompConst[7]}; +long homeCompSteps[] = {homeCompAngles[0]*red[0]*ppr[0]/360.0, homeCompAngles[1]*red[1]*ppr[1]/360.0, homeCompAngles[2]*red[2]*ppr[2]/360.0, homeCompAngles[3]*red[3]*ppr[3]/360.0, homeCompAngles[4]*red[4]*ppr[4]/360.0, homeCompAngles[5]*red[5]*ppr[5]/360.0, homeCompAngles[6]*red[4]*ppr[4]/360.0, homeCompAngles[7]*red[5]*ppr[5]/360.0}; +char value; + +// values for changing speed +const int maxSpeedIndex = 2; +int speedVals[maxSpeedIndex+1][NUM_AXES_EFF] = {{600, 900, 1500, 1250, 1050, 1050, 1050, 1050}, {900, 1200, 2000, 1665, 1460, 1460, 1460, 1460}, {1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200}}; +int speedIndex = maxSpeedIndex; + + + +void setup() { // setup function to initialize pins and provide initial homing to the arm + + Serial.begin(9600); + + // initializes step pins, direction pins, limit switch pins, and stepper motor objects for accelStepper library + for(i = 0; i readInterval) { + recieveCommand(); + previousTime = currentTime; + } + +runSteppers(); +} + +void recieveCommand() +{ + String inData = ""; + char recieved = garbage; + if(Serial.available()>0) + { + do { + recieved = Serial.read(); + inData += String(recieved); + } while(recieved != '\n'); + } + + if(recieved == '\n') + { + parseMessage(inData); + } +} + +void parseMessage(String inMsg) +{ + String function = inMsg.substring(0, 2); + + if(function == "MT") + { + cartesianCommands(inMsg); + } + + else if(function == "JM") + { + jointCommands(inMsg); + } + + else if(function == "EE") + { + endEffectorCommands(inMsg); + } + + else if(function == "DM") + { + drillCommands(inMsg); + } + + else if(function == "HM") + { + homeArm(); + } +} + +//****//CARTESIAN MODE FUNCTIONS//****// + +void cartesianCommands(String inMsg) +{ + // read current joint positions + readEncPos(curEncSteps); + + // update host with joint positions + sendCurrentPosition(); + + // get new position commands + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt(); + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt(); + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt(); + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt(); + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt(); + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt(); + + // update target joint positions + readEncPos(curEncSteps); + for (int i = 0; i < NUM_AXES; i++) + { + int diffEncSteps = cmdEncSteps[i] - curEncSteps[i]; + if (abs(diffEncSteps) > 2) + { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < MOTOR_STEPS_PER_REV[i]) + { + // for the last rev of motor, introduce artificial decceleration + // to help prevent overshoot + diffMotSteps = diffMotSteps / 2; + } + steppers[i].move(diffMotSteps); + } + } +} + +// parses which commands to execute when in joint space mode +void jointCommands(String inMsg) +{ + char function = inMsg[2]; + char detail1 = inMsg[3]; + + switch(function) + { + case release: releaseEvent(detail1, inMsg[4]); break; + case speed: changeSpeed(detail1); break; + case axis: changeAxis(detail1); break; + case move: jointMovement(detail1, inMsg[4]); break; + } +} + +void endEffectorCommands(String inMsg) +{ + // fill with end effector commands depending on substring +} + +void drillMotion(String inMsg) +{ + // fill with calls to drill functions depending on substring +} + +void sendCurrentPosition() +{ + String outMsg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2]) + + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n"); + Serial.print(outMsg); +} + +// Sets movement target positions when in joint space mode +void jointMovement(char joystick, char dir) +{ + if(joystick == wrist) + { + switch(dir) + { + case up: runWrist(FWD, 5); break; + case down: runWrist(REV, 5); break; + case left: runWrist(FWD, 6); break; + case right: runWrist(REV, 6); break; + } + } + + else if(joystick == left) + { + switch(dir) + { + case left: runAxes(FWD, currentAxis); break; + case right: runAxes(REV, currentAxis); break; + } + } + + else + { + switch(dir) + { + case up: runAxes(FWD, currentAxis+1); break; + case down: runAxes(REV, currentAxis+1); break; + } + } +} + +//****//ENCODER RELATED FUNCTIONS//****// + +void readEncPos(int* encPos) +{ + encPos[0] = J1encPos.read() * ENC_DIR[0]; + encPos[1] = J2encPos.read() * ENC_DIR[1]; + encPos[2] = J3encPos.read() * ENC_DIR[2]; + encPos[3] = J4encPos.read() * ENC_DIR[3]; + encPos[4] = J5encPos.read() * ENC_DIR[4]; + encPos[5] = J6encPos.read() * ENC_DIR[5]; +} + +//****//JOINT SPACE MODE FUNCTIONS//****// + +// sets target position for axes in joint space mode +void runAxes(int dir, int axis) { // assigns run flags to indicate forward / reverse motion and sets target position + + if(axis == 3) { + dir = !dir; + } + + if(runFlags[axis-1] == 1 && dir == FWD) { + } + + else if(runFlags[axis-1] == -1 && dir == REV) { + } + + else if(dir == FWD) { + steppers[axis-1].moveTo(max_steps[axis-1]*axisDir[axis-1]); + runFlags[axis-1] = 1; + } + + else { + steppers[axis-1].moveTo(0); + runFlags[axis-1] = -1; + } +} + +void runWrist(int dir, int axis) { // assigns target position for selected axis based on user input. + + if(axis == 5) { // axis 5 motion -> both wrist motors spin in opposite directions + if(runFlags[5] == 1 && dir == FWD) { + } + + else if(runFlags[5] == -1 && dir == REV) { + } + + else if(dir == FWD) { + steppers[6].moveTo(axisDir[6]*max_steps[5]); + steppers[7].moveTo(axisDir[7]*max_steps[5]); + runFlags[5] = 1; + } + + else { + steppers[6].moveTo(0); + steppers[7].moveTo(0); + runFlags[5] = -1; + } + } + + else if(axis == 6) { // axis 6 motion -> both wrist motors spin in same direction + dir = !dir; + if(runFlags[4] == 1 && dir == FWD) { + } + + else if(runFlags[4] == -1 && dir == REV) { + } + + else if(dir == FWD) { + steppers[4].moveTo(axisDir[4]*max_steps[4]); + steppers[5].moveTo(axisDir[5]*max_steps[4]); + runFlags[4] = 1; + } + + else { + steppers[4].moveTo(0); + steppers[5].moveTo(0); + runFlags[4] = -1; + } + } +} + +void changeAxis(int dir) { // when user hits specified button, axis targets change + + if((dir == up) && (currentAxis == 1)) + { + currentAxis = 3; + zeroRunFlags(); + } + + else if((dir == down) && (currentAxis == 3)) + { + currentAxis = 1; + zeroRunFlags(); + } +} + +void releaseEvent(char joystick, char dir) { // when user releases a joystick serial sends a character + + if(joystick == wrist) + { + if ((dir == up) || (dir == down)) + { + steppers[6].stop(); + steppers[7].stop(); + runFlags[5] = 0; + } + + else + { + steppers[4].stop(); + steppers[5].stop(); + runFlags[4] = 0; + } + } + + else if(joystick == left) + { + steppers[currentAxis-1].stop(); + runFlags[currentAxis-1].stop(); + } + + else + { + steppers[currentAxis].stop(); + runFlags[currentAxis].stop(); + } +} + +void changeSpeed(char speedVal) { // changes speed of all axes based on user input + + if(speedVal == faster){ + if(speedIndex < maxSpeedIndex) { + speedIndex++; + for(i=0;i 0) { + speedIndex--; + for(i=0;i 0) + { + recieved = Serial.read(); + inData += String(recieved) + if(recieved == '\n') + { + serialFlag = true; + } + } + + if(serialFlag) + { + if(inData = "HM") + { + homeArm(); + initFlag = true; + } + + else + { + inData = ""; + serialFlag = false; + } + } + } +} \ No newline at end of file diff --git a/src/teensy-firmware/src/armFirmware/armFirmware.ino b/src/teensy-firmware/src/armFirmware/armFirmware.ino new file mode 100644 index 000000000..2002cde82 --- /dev/null +++ b/src/teensy-firmware/src/armFirmware/armFirmware.ino @@ -0,0 +1,1097 @@ +/* +Created By: Tate Kolton, Rowan Zawadzki +Created On: December 21, 2021 +Updated: April 27, 2022 +Description: Main firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU +*/ + +#include +#include +#include + +// general parameters +#define NUM_AXES 6 +#define NUM_AXES_EX_WRIST 4 +#define NUM_AXES_EFF 8 +#define NUM_PARAMS 7 +#define ON 0 +#define OFF 1 +#define SW_ON 0 +#define SW_OFF 1 +#define FWD 1 +#define REV 0 + +static const char release = 'R'; +static const char move = 'M'; +static const char change = 'A'; +static const char speed = 'S'; +static const char right = 'R'; +static const char left = 'L'; +static const char up = 'U'; +static const char down = 'D'; +static const char wrist = 'W'; +static const char garbage = 'G'; +static const char faster = 'U'; +static const char slower = 'D'; +static const char prepare = 'P'; +static const char collect = 'C'; +static const char deposit = 'D'; +static const char manual = 'M'; +static const char drillRelease = 'X'; +static const char open = 'O'; +static const char close = 'C'; +static const char joint = 'J'; +static const char EEval = 'E'; +static const char homeValEE = 'H'; +static const char moveBase = 'T'; +static const char moveWrist = 'M'; +static const char relWrist = 'R'; +static const char relBase = 'W'; + +// Motor variables +int stepPins[8] = {6, 8, 10, 2, 12, 25, 12, 25}; +int dirPins[8] = {5, 7, 9, 1, 11, 24, 11, 24}; +int stepPinsIK[6] = {6, 8, 2, 10, 25, 12}; +int dirPinsIK[6] = {5, 7, 1, 9, 24, 11}; +int encPinA[6] = {17, 38, 40, 36, 15, 13}; +int encPinB[6] = {16, 37, 39, 35, 14, 41}; + +// end effector variables +const float calibrationFactor = -111.25; +float force; +HX711 scale; +const int dataPin = 34; +const int clkPin = 33; +int calPos = 0; +int closePos = 0; +int openPos = 500; // needs to be calibrated +int EEstepPin = 4; +int EEdirPin = 3; +int speedEE = 100; +int accEE = 500; +int speedDrill = 3000; +int accDrill = 1000; +const int MOTOR_DIR_EE = 1; +const int openButton = 5; +const int closeButton = 4; +const float calForce = 0.3; +const float maxForce = 10.0; +float EEforce; +int forcePct = 0; + +// limit switch pins +int limPins[6] = {18, 19, 21, 20, 23, 22}; + +// pulses per revolution for motors +long ppr[6] = {400, 400, 400, 400, 400, 400}; + +// Gear Reduction Ratios +float red[6] = {50.0, 161.0, 44.8, 93.07, 57.34, 57.34}; +float redIK[6] = {50.0, 161.0, 93.07, 44.8, 57.34, 57.34}; + + +// Encoder Variables +int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; +int pprEnc = 512; +int ENC_DIR[6] = {-1, -1, -1, -1, 1, 1}; +const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12}; +float ENC_STEPS_PER_DEG[NUM_AXES]; + +// Motor speeds and accelerations +int maxSpeed[8] = {1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200}; +int maxAccel[8] = {1300, 3500, 4600, 3300, 5000, 5000, 5000, 5000}; +int homeSpeed[8] = {300, 1000, 1000, 400, 2000, 2000, 2000, 2000}; // {500, 1200, 600, 400, 2000, 2000, 2000, 2000}; +int homeAccel[8] = {500, 2000, 1500, 1000, 1500, 1500, 1500, 1500}; //{500, 2000, 1000, 1500, 1500, 1500, 1500, 1500}; + + +// Time variables +const unsigned long readInterval = 10; +unsigned long currentTime; +unsigned long currentTimeJP; +unsigned long currentTimeEE; +unsigned long previousTime = 0; +unsigned long previousTimeEE = 0; +const unsigned long timeIntervalEE = 500; +const unsigned long timeIntervalJP = 250; +unsigned long previousTimeJP = 0; + +// stepper motor objects for AccelStepper library +AccelStepper steppers[8]; +AccelStepper endEff(1, EEstepPin, EEdirPin); +AccelStepper steppersIK[8]; + + +Encoder enc1(encPinA[0], encPinB[0]); +Encoder enc2(encPinA[1], encPinB[1]); +Encoder enc3(encPinA[2], encPinB[2]); +Encoder enc4(encPinA[3], encPinB[3]); +Encoder enc5(encPinA[4], encPinB[4]); +Encoder enc6(encPinA[5], encPinB[5]); + +Encoder encoders[] = {enc1, enc2, enc3, enc4, enc5, enc6}; + +// variable declarations + +int axisDir[8] = {1, -1, 1, -1, 1, 1, -1, 1}; +int axisDirIK[6] = {-1, -1, -1, 1, -1, -1}; +int currentAxis = 1; +int runFlags[] = {0, 0, 0, 0, 0, 0}; +int i; +bool initFlag = false; +bool jointFlag = true; +bool IKFlag = false; +bool J1Flag = false; +bool resetEE = false; +bool vertFlag = false; +bool horizFlag = false; + +// variables for homing / arm calibration +long homePosConst = -99000; +long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst, axisDir[6]*homePosConst, axisDir[7]*homePosConst}; +long homeCompAngles[] = {axisDir[0]*54, axisDir[1]*10, axisDir[2]*90, axisDir[3]*1, axisDir[4]*85, axisDir[5]*85, axisDir[6]*170, axisDir[7]*170}; +long homeCompConst[] = {500, 2000, 1000, 500, 500, 500, 500, 500}; +long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5], axisDir[6]*homeCompConst[6], axisDir[7]*homeCompConst[7]}; +long homeCompSteps[] = {homeCompAngles[0]*red[0]*ppr[0]/360.0, homeCompAngles[1]*red[1]*ppr[1]/360.0, homeCompAngles[2]*red[2]*ppr[2]/360.0, homeCompAngles[3]*red[3]*ppr[3]/360.0, homeCompAngles[4]*red[4]*ppr[4]/360.0, homeCompAngles[5]*red[5]*ppr[5]/360.0, homeCompAngles[6]*red[4]*ppr[4]/360.0, homeCompAngles[7]*red[5]*ppr[5]/360.0}; +// Range of motion (degrees) for each axis +int maxAngles[6] = {190, 160, 180, 120, 160, 180}; +long max_steps[] = {axisDir[0]*red[0]*maxAngles[0]/360.0*ppr[0], axisDir[1]*red[1]*maxAngles[1]/360.0*ppr[1], axisDir[2]*red[2]*maxAngles[2]/360.0*ppr[2], axisDir[3]*red[3]*maxAngles[3]/360.0*ppr[3], red[4]*maxAngles[4]/360.0*ppr[4], red[5]*maxAngles[5]/360.0*ppr[5]}; +long min_steps[NUM_AXES]; +char value; + +// values for changing speed +const int maxSpeedIndex = 2; +int speedVals[maxSpeedIndex+1][NUM_AXES_EFF] = {{600, 900, 1500, 1250, 1050, 1050, 1050, 1050}, {900, 1200, 2000, 1665, 1460, 1460, 1460, 1460}, {900, 1600, 2500, 2200, 2000, 2000, 2000, 2000}}; +int speedIndex = maxSpeedIndex; + +// Cartesian mode speed settings +float IKspeeds[] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2}; +float IKaccs[] = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; + +void setup() { // setup function to initialize pins and provide initial homing to the arm + + Serial.begin(9600); + + for(int i=0; i0) + { + do { + recieved = Serial.read(); + inData += String(recieved); + } while(recieved != '\n'); + } + + if(recieved == '\n') + { + + parseMessage(inData); + } +} + +void parseMessage(String inMsg) +{ + String function = inMsg.substring(0, 2); + + if(function == "MT") + { + cartesianCommands(inMsg); + } + + else if(function == "JM") + { + jointCommands(inMsg); + } + + else if(function == "EE") + { + endEffectorCommands(inMsg); + } + + else if(function == "DM") + { + drillCommands(inMsg); + } + + else if(function == "FB") + { + sendFeedback(inMsg); + } + + else if(function == "HM") + { + homeArm(); + } +} + +void sendMessage(char outChar) +{ + String outMsg = String(outChar); + Serial.print(outMsg); +} + +void sendFeedback(String inMsg) +{ + char function = inMsg[2]; + + if(function == joint) + { + readEncPos(curEncSteps); + sendCurrentPosition(); + } +} + +//****//CARTESIAN MODE FUNCTIONS//****// + +void cartesianCommands(String inMsg) +{ + // read current joint positions + readEncPos(curEncSteps); + + // update host with joint positions + sendCurrentPosition(); + + if(!IKFlag) + { + setCartesianSpeed(); + IKFlag = true; + jointFlag = false; + } + + // get new position commands + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt(); + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt(); + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt(); + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt(); + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt(); + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt(); + + // update target joint positions + readEncPos(curEncSteps); + cmdArmBase(); + cmdArmWrist(); +} + +void setCartesianSpeed() +{ + float JOINT_MAX_SPEED[NUM_AXES]; + float MOTOR_MAX_SPEED[NUM_AXES]; + float JOINT_MAX_ACCEL[NUM_AXES]; + float MOTOR_MAX_ACCEL[NUM_AXES]; + + for(int i=0; i 6) + { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < ppr[i]) + { + // for the last rev of motor, introduce artificial decceleration + // to help prevent overshoot + diffMotSteps = diffMotSteps / 2; + } + + steppersIK[i].move(diffMotSteps*axisDirIK[i]); + } + } +} + +void cmdArmWrist() +{ + int diffMotStepsA5, diffMotStepsA6, diffEncStepsA5, diffEncStepsA6; + + diffEncStepsA5 = cmdEncSteps[4] - curEncSteps[4]; + diffEncStepsA6 = cmdEncSteps[5] - curEncSteps[5]; + + if(abs(diffEncStepsA5) > 10) + { + diffMotStepsA5 = diffEncStepsA5 / ENC_MULT[4]; + if(diffMotStepsA5 < ppr[4]) + { + diffMotStepsA5 = diffEncStepsA5 / 2; + } + } + + if(abs(diffEncStepsA6) > 10) + { + diffMotStepsA6 = diffEncStepsA6 / ENC_MULT[5]; + if(diffMotStepsA6 < ppr[5]) + { + diffMotStepsA6 = diffEncStepsA6 / 2; + } + } + + int actualMotStepsA5 = diffMotStepsA6/2 - diffMotStepsA5/2; + int actualMotStepsA6 = diffMotStepsA6/2 + diffMotStepsA5/2; + + steppersIK[4].move(actualMotStepsA5*axisDirIK[4]); + steppersIK[5].move(actualMotStepsA6*axisDirIK[5]); +} + + + +//****//JOINT SPACE MODE FUNCTIONS//****// + +// sets target position for axes in joint space mode +void runAxes(int dir, int axis) { // assigns run flags to indicate forward / reverse motion and sets target position + + if((axis == 3) || (axis == 1) || (axis == 2)) { + dir = !dir; + } + + if(runFlags[axis-1] == 1 && dir == FWD) { + } + + else if(runFlags[axis-1] == -1 && dir == REV) { + } + + else if(dir == FWD) { + steppers[axis-1].moveTo(max_steps[axis-1]); + runFlags[axis-1] = 1; + } + + else { + steppers[axis-1].moveTo(min_steps[axis-1]); + runFlags[axis-1] = -1; + } +} + +void runWrist(int dir, int axis) { // assigns target position for selected axis based on user input. + + if(axis == 5) { + if(runFlags[5] == 1 && dir == FWD) { + } + + else if(runFlags[5] == -1 && dir == REV) { + } + + else if(dir == FWD) { + steppers[6].moveTo(axisDir[6]*max_steps[5]); + steppers[7].moveTo(axisDir[7]*max_steps[5]); + runFlags[5] = 1; + } + + else { + steppers[6].moveTo(axisDir[6]*min_steps[5]); + steppers[7].moveTo(axisDir[7]*min_steps[5]); + runFlags[5] = -1; + } + } + + else if(axis == 6) { + dir = !dir; + if(runFlags[4] == 1 && dir == FWD) { + } + + else if(runFlags[4] == -1 && dir == REV) { + } + + else if(dir == FWD) { + steppers[4].moveTo(axisDir[4]*max_steps[4]); + steppers[5].moveTo(axisDir[5]*max_steps[4]); + runFlags[4] = 1; + } + + else { + steppers[4].moveTo(axisDir[4]*min_steps[4]); + steppers[5].moveTo(axisDir[5]*min_steps[4]); + runFlags[4] = -1; + } + } +} + +void changeAxis(int dir) { // when user hits specified button, axis targets change + + if((dir == up) && (currentAxis == 1)) + { + currentAxis = 3; + zeroRunFlags(); + } + + else if((dir == down) && (currentAxis == 3)) + { + currentAxis = 1; + zeroRunFlags(); + } +} + +void releaseEvent(char joystick, char dir) { // when user releases a joystick serial sends a character + + if(joystick == wrist) + { + if ((dir == left) || (dir == right)) + { + steppers[6].stop(); + steppers[7].stop(); + runFlags[5] = 0; + } + + else + { + steppers[4].stop(); + steppers[5].stop(); + runFlags[4] = 0; + } + } + + else if(joystick == left) + { + steppers[currentAxis-1].stop(); + runFlags[currentAxis-1] = 0; + } + + else if(joystick == right) + { + steppers[currentAxis].stop(); + runFlags[currentAxis] = 0; + } +} + +void changeSpeed(char speedVal) { // changes speed of all axes based on user input + + if(speedVal == faster){ + if(speedIndex < maxSpeedIndex) { + speedIndex++; + for(i=0;i 0) { + speedIndex--; + for(i=0;i (homeCompSteps[0] + homeComp[0])/axisDir[0])) + { + steppers[0].move(-homePos[i]); + } + else + { + steppers[0].move(homePos[i]); + } + } + else + { + steppers[0].move(homePos[i]); + } + } +} + +void initializeWristHomingMotion() { // sets homing speed and acceleration for axes 5-6 and sets target homing position + + for(i = 4; i0) + { + do { + recieved = Serial.read(); + inData += String(recieved); + } while(recieved != '\n'); + } + + if(recieved == '\n') + { + if(inData.substring(0, 2) == "HM") + homeArm(); + initFlag = true; + } + } +} + +// updated aug 22, 2022