diff --git a/.rosinstall b/.rosinstall index 637c9a3db..63275a7da 100644 --- a/.rosinstall +++ b/.rosinstall @@ -1,6 +1,7 @@ - git: local-name: src/external_pkgs/nmea_navsat_driver uri: https://github.com/UBC-Snowbots/nmea_navsat_driver.git + version: noetic-snowbots-mods - git: local-name: src/external_pkgs/realsense-ros uri: https://github.com/IntelRealSense/realsense-ros.git @@ -10,12 +11,7 @@ - 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 + version: noetic-snowbots-mods - git: local-name: src/external_pkgs/sicktoolbox uri: https://github.com/UBC-Snowbots/sicktoolbox.git @@ -34,4 +30,11 @@ uri: 'https://github.com/nicman23/dkms-hid-nintendo' - git: local-name: external_libs/joycond - uri: https://github.com/DanielOgorchock/joycond \ No newline at end of file + uri: https://github.com/DanielOgorchock/joycond +- git: + local-name: src/external_pkgs/qt_ros + uri: https://github.com/stonier/qt_ros.git +- git: + local-name: src/external_pkgs/phidgets_drivers + uri: https://github.com/ros-drivers/phidgets_drivers + version: noetic diff --git a/.travis.yml b/.travis.yml index 8741bc8b9..f3ebeb4f6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,7 +3,7 @@ cache: language: generic # currently running on ubuntu 18.04 os: linux -dist: bionic +dist: focal # install all dependencies necessary to build our project install: @@ -14,7 +14,7 @@ install: # compile and run tests script: # setup ROS - - source /opt/ros/melodic/setup.bash + - source /opt/ros/noetic/setup.bash # create workspace - catkin_make # setup catkin path variables diff --git a/README.md b/README.md index f3488230d..e191a8f11 100644 --- a/README.md +++ b/README.md @@ -32,9 +32,9 @@ You will be downloading an Ubuntu ISO and multiple ROS packages with their respe It is highly recommended that you have access to high speed internet while doing this entire setup; if you're on campus use the `ubcsecure` or `resnet` networks for best results. -1. Install Ubuntu 18.04 (**Note**: Download 18.04, **not** another version, as other Ubuntu versions will make setting up this repo much more painful) (**Backup your important data first**) (We recommend you use a minimum of 30GB of space) - - For dual-booting: [Windows Instructions](https://askubuntu.com/questions/1031993/how-to-install-ubuntu-18-04-alongside-windows-10), [Mac Instructions](https://www.maketecheasier.com/install-dual-boot-ubuntu-mac) -2. If you haven't done so already, setup your UBC alumni email account [here](https://id.ubc.ca/) +1. Install Ubuntu 20.04 (**Note**: Download 20.04, **not** another version, as other Ubuntu versions will make setting up this repo much more painful) (**Backup your important data first**) (We recommend you use a minimum of 30GB of space) + - For dual-booting: [Windows Instructions](https://linuxconfig.org/how-to-install-ubuntu-20-04-alongside-windows-10-dual-boot), [Mac Instructions](https://www.maketecheasier.com/install-dual-boot-ubuntu-mac) +2. If you haven't done so already, setup your UBC email account [here](https://id.ubc.ca/) 3. Using your UBC email account, get a JetBrains education account [here](https://www.jetbrains.com/shop/eform/students) - _JetBrains will send an initial email to confirm the UBC email you inputted, once you've confirmed another email will be sent to activate your new education account; @@ -45,7 +45,7 @@ if you're on campus use the `ubcsecure` or `resnet` networks for best results. 7. Clone your server-side repository from the terminal by running `git clone --recursive https://github.com/YOUR_USERNAME/Snowflake.git` (`YOUR_USERNAME` is your github username) 8. To start the setup run `cd ~/Snowflake && ./setup_scripts/install_tools.sh` (Do not run this script as root). - *Just choose yes and enter your password when the terminal prompts you* -9. Run `source /opt/ros/melodic/setup.sh`. +9. Run `source /opt/ros/noetic/setup.sh`. 10. To build the workspace run `catkin_make`. If everything compiles correctly and you don't get any errors, then you're good to go! 11. If catkin_make fails, and mentions missing .cmake files, run `./setup_scripts/install_dependencies.sh`. @@ -75,6 +75,8 @@ Current packages included under wstool: - NMEA_Navsat_Driver: NMEA msg driver - Zed_ROS_Wrapper: Wrapper for interfacing Zed Stereo Cam with ROS - realsense: Wrapper for interfacing the Realsense D415 with ROS +- qt_ros: Interface for integrating ROS with QT +- phidgets_drivers: Interface for integrating ROS and Phidgets drivers ### Nodelets Quick Guide See [here](nodelets.md) diff --git a/clang-format/clang-format-4.0 b/clang-format/clang-format-4.0 deleted file mode 100755 index 701ca0527..000000000 Binary files a/clang-format/clang-format-4.0 and /dev/null differ diff --git a/clang-format/fix_formatting.sh b/clang-format/fix_formatting.sh index f83592a91..915e050a1 100755 --- a/clang-format/fix_formatting.sh +++ b/clang-format/fix_formatting.sh @@ -1,8 +1,5 @@ #!/bin/bash -# The version of the clang executable to use -export CLANG_VERSION=4.0 - # The current directory CURR_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" @@ -14,7 +11,7 @@ fi TARGET_BRANCH=$1 # Fix formatting on all changes between this branch and the target branch -OUTPUT="$($CURR_DIR/git-clang-format --binary $CURR_DIR/clang-format-$CLANG_VERSION --commit $TARGET_BRANCH)" +OUTPUT="$($CURR_DIR/git-clang-format --binary clang-format --commit $TARGET_BRANCH)" # Print the output (the files changed), replacing spaces with newlines printf '%s\n' $OUTPUT diff --git a/setup_scripts/install_dependencies.sh b/setup_scripts/install_dependencies.sh index 47fff4118..cc4bba138 100755 --- a/setup_scripts/install_dependencies.sh +++ b/setup_scripts/install_dependencies.sh @@ -14,15 +14,18 @@ CURR_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" echo "================================================================" -echo "Installing ROS Melodic" +echo "Installing ROS Noetic" echo "================================================================" +sudo apt-get install -y software-properties-common +sudo add-apt-repository ppa:rock-core/qt4 -y + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' -sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc -O - | sudo apt-key add - sudo apt-get update -y -sudo apt-get install python-catkin-pkg python-wstool python-rosdep python-rosinstall-generator ros-melodic-desktop-full -y -source /opt/ros/melodic/setup.sh +sudo apt-get install python3-catkin-pkg python3-wstool python3-rosdep python3-rosinstall-generator ros-noetic-desktop-full -y +source /opt/ros/noetic/setup.sh # Prepare resdep to install dependencies sudo rosdep init @@ -51,18 +54,19 @@ cd $CURR_DIR rosdep install --from-paths \ $CURR_DIR/../src \ $CURR_DIR/../src/external_pkgs \ - --ignore-src --rosdistro melodic --skip-keys=librealsense2 --skip-keys=libswiftnav -y + --ignore-src --rosdistro noetic --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.sh -cd $CURR_DIR -sudo ./install_phidgets.sh -cd $CURR_DIR -sudo ./install_libsbp.sh +#cd $CURR_DIR +#sudo ./install_phidgets.sh +#cd $CURR_DIR +#sudo ./install_libsbp.sh cd $CURR_DIR echo "================================================================" @@ -71,7 +75,13 @@ echo "================================================================" sudo apt-get install -y\ clang-format\ - python-rosinstall + python3-rosinstall + +sudo apt-get update -y +sudo apt-get install -y libalglib-dev +sudo apt-get install -y libdlib-dev +sudo apt-get install -y libgsl-dev +sudo apt-get install -y libserial-dev echo "================================================================" echo "Installing Robotic Arm Dependencies" diff --git a/setup_scripts/install_tools.sh b/setup_scripts/install_tools.sh index 4fba974ec..0f964263d 100755 --- a/setup_scripts/install_tools.sh +++ b/setup_scripts/install_tools.sh @@ -33,7 +33,7 @@ SHELL_CONFIG_FILES=( # listed above, if they are not present already declare -a new_shell_config_lines=( # Source the ROS Environment Variables Automatically - "source /opt/ros/melodic/setup.sh"\ + "source /opt/ros/noetic/setup.sh"\ # Make sure that all shells know where to find our custom gazebo models, # plugins, and resources. Make sure to preserve the path that already exists as well "export GAZEBO_MODEL_PATH=$DIR/../src/sb_gazebo/models:${GAZEBO_MODEL_PATH}"\ @@ -67,10 +67,10 @@ if [ "$install_clion" = true ] ; then fi ################### -# Install Arduino # +# SKIPPING Install Arduino # ################### -cd $DIR -./install_arduino.sh +#cd $DIR +#./install_arduino.sh ############################## # Install Other Dependencies # diff --git a/setup_scripts/setup_realsense.sh b/setup_scripts/setup_realsense.sh index 20823b301..674740f8b 100755 --- a/setup_scripts/setup_realsense.sh +++ b/setup_scripts/setup_realsense.sh @@ -7,10 +7,8 @@ echo "================================================================" echo "Configuring realsense repository and drivers." echo "================================================================" -sudo apt-get install -y software-properties-common -sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE -sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u -sudo rm -f /etc/apt/sources.list.d/realsense-public.list +sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE +sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u sudo apt-get update -y @@ -18,13 +16,13 @@ sudo apt-get update -y # the ros realsense packages are updated to work with librealsense2 2.16.2 # see https://github.com/intel-ros/realsense/issues/502 for updates. # (Right now we just peg the version to 2.16.1) -# sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg -version="2.16.3-0~realsense0.116" -sudo apt-get install librealsense2-dkms -y -sudo apt install librealsense2=${version} -y -sudo apt-get install librealsense2-utils=${version} -y -sudo apt-get install librealsense2-dev=${version} -y -sudo apt-get install librealsense2-dbg=${version} -y +sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg +# version="2.16.3-0~realsense0.116" +# sudo apt-get install librealsense2-dkms -y +# sudo apt install librealsense2=${version} -y +# sudo apt-get install librealsense2-utils=${version} -y +# sudo apt-get install librealsense2-dev=${version} -y +# sudo apt-get install librealsense2-dbg=${version} -y echo "================================================================" echo "Finished configuring realsense. " diff --git a/setup_scripts/setup_udev_rules.sh b/setup_scripts/setup_udev_rules.sh index 093d7cf90..24e923395 100755 --- a/setup_scripts/setup_udev_rules.sh +++ b/setup_scripts/setup_udev_rules.sh @@ -47,7 +47,7 @@ fi # Copy the new phidgets udev rules to the rule folder # From the installation to the udev folder # Apt should theoretically do this but it doesn't -sudo cp /opt/ros/melodic/share/phidgets_api/udev/99-phidgets.rules $FILE +sudo cp /opt/ros/noetic/share/phidgets_api/udev/99-phidgets.rules $FILE echo "================================================================" echo "Finished Installing phidgets udev rules" diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 66dd650ac..201681686 120000 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/noetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file 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..bca2832ef --- /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::SerialPort teensy; + + // The Port the teensy is connected to + std::string port; +}; +#endif // ARM_HARDWARE_DRIVER_MYNODE_H diff --git a/src/challenge/package.xml b/src/arm_hardware_driver/package.xml similarity index 79% rename from src/challenge/package.xml rename to src/arm_hardware_driver/package.xml index dfc54515a..79f713ecf 100644 --- a/src/challenge/package.xml +++ b/src/arm_hardware_driver/package.xml @@ -1,13 +1,13 @@ - challenge + arm_hardware_driver 0.0.0 - The challenge package + The arm_hardware_driver package - gareth + tate @@ -19,7 +19,7 @@ - + @@ -40,12 +40,16 @@ catkin - geometry_msgs roscpp std_msgs - geometry_msgs + sb_utils + sb_msgs + libserial-dev roscpp std_msgs + sb_utils + sb_msgs + libserial-dev @@ -53,4 +57,4 @@ - \ No newline at end of file + diff --git a/src/arm_hardware_driver/src/armHardwareDriver.cpp b/src/arm_hardware_driver/src/armHardwareDriver.cpp new file mode 100644 index 000000000..dddd0f364 --- /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::BaudRate::BAUD_9600); + teensy.SetCharacterSize(LibSerial::CharacterSize::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.Write(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.WriteByte(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/decision_igvc/package.xml b/src/arm_hardware_interface/package.xml similarity index 72% rename from src/decision_igvc/package.xml rename to src/arm_hardware_interface/package.xml index 6656bee5d..df2324a11 100644 --- a/src/decision_igvc/package.xml +++ b/src/arm_hardware_interface/package.xml @@ -1,13 +1,13 @@ - decision_igvc + arm_hardware_interface 0.0.0 - The decision package + The arm_hardware_interface package - gareth + tate @@ -19,7 +19,7 @@ - + @@ -40,18 +40,20 @@ catkin - geometry_msgs roscpp + std_msgs sb_utils - sensor_msgs - gps_common - tf2_geometry_msgs - geometry_msgs + sb_msgs + hardware_interface + joint_limits_interface + controller_manager roscpp - sensor_msgs + std_msgs sb_utils - gps_common - tf2_geometry_msgs + 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/challenge/CMakeLists.txt b/src/challenge/CMakeLists.txt deleted file mode 100644 index 3779bd606..000000000 --- a/src/challenge/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(challenge) - -## 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 - geometry_msgs - roscpp - std_msgs -) - -catkin_package( -# INCLUDE_DIRS include -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ executable -# add_executable(challenge_node src/challenge_node.cpp) -add_executable(solution src/solution.cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(challenge_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(solution ${catkin_LIBRARIES}) - - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_challenge.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() diff --git a/src/challenge/src/solution.cpp b/src/challenge/src/solution.cpp deleted file mode 100644 index d3434daf6..000000000 --- a/src/challenge/src/solution.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include - -int main(int argc, char** argv) { - ros::init(argc, argv, "solution"); - ros::NodeHandle nh; - - ros::Rate loop_rate = 10; - - while (ros::ok()) { - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} diff --git a/src/decision_igvc/CMakeLists.txt b/src/decision_igvc/CMakeLists.txt deleted file mode 100644 index 105c19145..000000000 --- a/src/decision_igvc/CMakeLists.txt +++ /dev/null @@ -1,122 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(decision_igvc) - -# 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 -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - cv_bridge - tf2_ros - gps_common - tf2_geometry_msgs - sb_utils - mapping_msgs_urc - ) -find_package(OpenCV REQUIRED) - -add_definitions(-std=c++14) - -catkin_package( - INCLUDE_DIRS include -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ./include -) - -## Declare a C++ executable -# add_executable(sample_package_node src/sample_package_node.cpp) -add_executable(lidar_decision - src/lidar_decision.cpp - src/LidarDecision.cpp - include/LidarDecision.h - src/LidarObstacle.cpp - include/LidarObstacle.h - ) -add_executable(vision_decision - src/vision_decision.cpp - src/VisionDecision.cpp - include/VisionDecision.h - ) -add_executable(gps_decision - src/gps_decision.cpp - src/GpsDecision.cpp - src/GpsMover.cpp - include/GpsDecision.h - include/GpsMover.h - ) -add_executable(final_decision - src/final_decision.cpp - src/FinalDecision.cpp - include/FinalDecision.h - ) -add_executable(gps_manager - src/gps_manager.cpp - src/GpsManager.cpp - include/GpsManager.h - ) - -## Specify libraries to link a library or executable target against -# target_link_libraries(sample_package_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(lidar_decision ${catkin_LIBRARIES}) -target_link_libraries(vision_decision ${catkin_LIBRARIES}) -target_link_libraries(gps_decision ${catkin_LIBRARIES}) -target_link_libraries(final_decision ${catkin_LIBRARIES}) -target_link_libraries(gps_manager ${catkin_LIBRARIES}) - - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - # gTest - catkin_add_gtest(lidar-decision-test test/lidar-decision-test.cpp src/LidarDecision.cpp src/LidarObstacle.cpp) - target_link_libraries(lidar-decision-test ${catkin_LIBRARIES}) - - catkin_add_gtest(lidar-obstacle-test test/lidar-obstacle-test.cpp src/LidarObstacle.cpp) - target_link_libraries(lidar-obstacle-test ${catkin_LIBRARIES}) - - catkin_add_gtest(vision-decision-test test/vision-decision-test.cpp src/VisionDecision.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) - target_link_libraries(vision-decision-test ${catkin_LIBRARIES}) - - catkin_add_gtest(gps-mover-test test/gps-mover-test.cpp src/GpsMover.cpp) - target_link_libraries(gps-mover-test ${catkin_LIBRARIES}) - - catkin_add_gtest(final-decision-test test/final-decision-test.cpp src/FinalDecision.cpp) - target_link_libraries(final-decision-test ${catkin_LIBRARIES}) - - catkin_add_gtest(gps-manager-test test/gps-manager-test.cpp src/GpsManager.cpp) - target_link_libraries(gps-manager-test ${catkin_LIBRARIES}) - - #TODO: gps_decision tests - - # rostest - find_package(rostest REQUIRED) - - add_rostest_gtest(lidar_decision_rostest test/lidar_decision_rostest.test test/lidar_decision_rostest.cpp) - target_link_libraries(lidar_decision_rostest ${catkin_LIBRARIES}) - -endif() diff --git a/src/decision_igvc/include/FinalDecision.h b/src/decision_igvc/include/FinalDecision.h deleted file mode 100644 index 416ce799c..000000000 --- a/src/decision_igvc/include/FinalDecision.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Created By: Kevin Luo - * Modified by: Valerian Ratu - * Created On: September 22, 2016 - * Description: The Final Decision node, arbitrates between the decisions of - * other nodes. - */ - -#ifndef DECISION_FINAL_DECISION_H -#define DECISION_FINAL_DECISION_H - -#include -#include -#include -#include -#include - -class FinalDecision { - public: - // The constructor - FinalDecision(int argc, char** argv, std::string node_name); - - /** - * Arbitrates the three messages received from Lidar, Vision, and GPS - * - * If there is a change in one or more of the messages, the function decides - * which one has the highest priority and it is published. - * The messages are held in the following order of significance: - * -Lidar - * -Vision - * -GPS - * - * @param recent_lidar Twist message from Lidar - * @param recent_vision Twist message from the camera, telling the robot - * where lines and boundaries are - * @param recent_gps Twist message from the GPS - * - * @return The Twist message with the highest priority. - */ - static geometry_msgs::Twist arbitrator(geometry_msgs::Twist recent_lidar, - geometry_msgs::Twist recent_vision, - geometry_msgs::Twist recent_gps); - - private: - // Sensor callbacks - void gpsCallBack(const geometry_msgs::Twist::ConstPtr& gps_decision); - void lidarCallBack(const geometry_msgs::Twist::ConstPtr& lidar_decision); - void visionCallBack(const geometry_msgs::Twist::ConstPtr& vision_decision); - - // Publishes twist - void publishTwist(geometry_msgs::Twist twist); - - // Subscribers - ros::Subscriber lidar_subscriber; - ros::Subscriber vision_subscriber; - ros::Subscriber gps_subscriber; - - // Publishers - ros::Publisher twist_publisher; - - // Sensor data placeholders - geometry_msgs::Twist recent_lidar; - geometry_msgs::Twist recent_vision; - geometry_msgs::Twist recent_gps; -}; - -#endif // DECISION_FINAL_DECISION_H diff --git a/src/decision_igvc/include/GpsDecision.h b/src/decision_igvc/include/GpsDecision.h deleted file mode 100644 index 5703abe01..000000000 --- a/src/decision_igvc/include/GpsDecision.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: May 18, 2017 - * Description: The Decision Node for GPS, takes in a point relative to - * the robots location and heading and broadcasts a - * recommended twist message. Revised to use TF's - */ - -#ifndef DECISION_GPS_DECISION_H -#define DECISION_GPS_DECISION_H - -// STD -#include - -// Snowbots -#include -#include - -// ROS -#include -#include -#include -#include -#include -#include -#include - -class GpsDecision { - public: - GpsDecision(int argc, char** argv, std::string node_name); - - private: - // callback for our destination waypoint - void - waypointCallback(const geometry_msgs::PointStamped::ConstPtr& waypoint); - - // Subscribes to the next waypoint the robot has to go to - ros::Subscriber waypoint_subscriber; - - // Publishes a twist message telling the robot how to move - ros::Publisher twist_publisher; - - // The class that generates our twist messages - GpsMover mover; - - // The base frame of the robot ("base_link", "base_footprint", etc.) - std::string base_frame; - // The global frame ("map", "odom", etc.) - std::string global_frame; -}; - -#endif // DECISION_GPS_DECISION_H diff --git a/src/decision_igvc/include/GpsManager.h b/src/decision_igvc/include/GpsManager.h deleted file mode 100644 index 6d7e66cc8..000000000 --- a/src/decision_igvc/include/GpsManager.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: May 17, 2017 - * Description: Manages the GPS waypoints we are given - * (publishing the next one once we've arrived - * at the current one) - */ - -#ifndef DECISION_GPSManager_H -#define DECISION_GPSManager_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// A struct to hold lat/lon coordinates (a waypoint) -// lat/lon should be in degrees -struct Waypoint { - double lat; - double lon; -}; - -class GpsManager { - public: - GpsManager(int argc, char** argv, std::string node_name); - /** - * Parses a raw list of waypoints (in order lat,lon,lat,lon,...) into a - * vector of pairs - * - * @param waypoints_raw the raw waypoints to be parsed - * @return a vector of pairs of lat/lon coordinates parsed from - * raw_waypoints - */ - static std::vector - parseWaypoints(std::vector raw_waypoints); - - private: - /** - * Called whenever a new tf message is published - * - * Checks if we're at the waypoint yet, and starts publishing the next - * waypoint if we are - * - * @param tf_message contains all the tf's currently being published on a - * given topic - */ - void tfCallBack(const tf2_msgs::TFMessageConstPtr tf_message); - /** - * Clears waypoint_stack, then puts all the elements from waypoint_list onto - * the waypoint_stack, - * with the 0'th element from waypoint_list at the top of waypoint_stack - */ - void populateWaypointStack(std::vector waypoint_list); - - /** - * Publishes a marker to the next waypoint in Rviz - * @param p the next waypoint - * @param global_to_local_transform the transform from global to local frame - * NOTE: Have to modify the transform so that the Z point is zeroed out - * due to the original transform also translating in the Z direction. - */ - void publishRvizWaypointMarker( - geometry_msgs::PointStamped p, - geometry_msgs::TransformStamped global_to_local_transform); - - ros::Subscriber tf_subscriber; - ros::Publisher current_waypoint_publisher; - ros::Publisher rviz_marker_publisher; - - std::string base_frame; // The base frame of the robot ("base_link", - // "base_footprint", etc.) - std::string global_frame; // The global frame ("map", "odom", etc.) - - // Params - float at_goal_tolerance; - std::stack waypoint_stack; // A stack of all - // unvisited - // waypoints stored - // in the UTM - // coordinate frame -}; - -#endif // DECISION_GPSManager_H diff --git a/src/decision_igvc/include/GpsMover.h b/src/decision_igvc/include/GpsMover.h deleted file mode 100644 index 3a4c23431..000000000 --- a/src/decision_igvc/include/GpsMover.h +++ /dev/null @@ -1,148 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: April 1, 2017 - * Description: The declaration for the GpsMover class. This class - * contains all the methods required to decide on the - * appropriate twist message to make the robot move smoothly - * towards a given point from it's current location - */ - -#ifndef MOVER_H -#define MOVER_H - -#include -#include - -class GpsMover { - public: - /** - * A constructor for a GpsMover object - * - * See GpsMover::setFactors for param descriptions - * - * @param linear_distance_factor - * @param linear_heading_factor - * @param angular_distance_factor - * @param angular_heading_factor - */ - GpsMover(double linear_distance_factor, - double linear_heading_factor, - double angular_distance_factor, - double angular_heading_factor); - - /** - * An empty constructor will default to factors of 1 - */ - GpsMover() : GpsMover(1, 1, 1, 1){}; - - /** - * Sets relations between distance/angle to object, and linear/angular - * speeds - * - * @param linear_distance_factor how much distance from the obstacle - * influence - * linear speed - * @param linear_heading_factor how much the angle between the current - * heading and - * the angle to the obstacle influence the linear speed - * linear speed - * @param angular_distance_factor how much distance from the obstacle - * influence - * angular speed - * @param angular_heading_factor how much the angle between the current - * heading and - * the angle to the obstacle influence the angular speed - * angular speed - */ - void setFactors(double linear_distance_factor, - double linear_heading_factor, - double angular_distance_factor, - double angular_heading_factor); - - /** - * Sets the caps for linear and angular speed - * - * @param max_linear_speed the max linear speed for any command this mover - * returns - * @param max_angular_speed the max angular speed for any command this mover - * returns - */ - void setMaxSpeeds(double max_linear_speed, double max_angular_speed); - - /** - * Creates a recommended twist command, based on the robots current heading, - * location, and dest. waypoint - * - * @param current_location the current location of the robot - * @param current_heading the current heading of the robot - * @param waypoint the waypoint we're trying to get to - * - * @return a twist message that moves the robot towards the waypoint - */ - geometry_msgs::Twist - createTwistMessage(geometry_msgs::Point current_location, - double current_heading, - geometry_msgs::Point waypoint); - /** - * Computes the average of 1/x and sqrt(y), multiplied by their respective - * scaling factors - * - * @param x - * @param y - * @param x_scale the value to multiply the x value by - * @param y_scale the value to multiply the y value by - * - * @return the average of: [x_scale * 1/x] and [y_scale * sqrt(y)] - */ - double magicFunction(double x, double y, double x_scale, double y_scale); - - /** - * Finds the minimum angular change required to turn from one heading to - * another - * - * @param from_heading the heading we're going from (in radians) - * @param to_heading the heading we're going to (in radians) - * - * @return the minimum angular change FROM from_heading TO to_heading - */ - double minAngularChange(double from_heading, double to_heading); - - /** - * Find the angle between two points, based on the ROS coordinate system - * (see main README) - * - * @param startPoint - * @param endPoint - * - * @return the angle FROM startPoint TO endPoint - */ - double angleBetweenPoints(geometry_msgs::Point startPoint, - geometry_msgs::Point endPoint); - - private: - /** - * Reduces a value to an specified absolute maximum - * - * @param val the value to cap - * @param cap the absolute cap - */ - void capValue(double& val, double cap); - - // How much effect the distance to the waypoint has on the linear speed - double linear_distance_factor; - // How much effect the angle between our heading and the - // angle to the waypoint has on the linear speed - double linear_heading_factor; - // How much effect the distance to the waypoint has on the angular speed - double angular_distance_factor; - // How much effect the heading between our heading and the - // heading to the waypoint has on the angular speed - double angular_heading_factor; - - // The max linear speed allowed - double max_linear_speed; - // The max angular speed allowed - double max_angular_speed; -}; - -#endif // MOVER_H diff --git a/src/decision_igvc/include/LidarDecision.h b/src/decision_igvc/include/LidarDecision.h deleted file mode 100644 index 219df0283..000000000 --- a/src/decision_igvc/include/LidarDecision.h +++ /dev/null @@ -1,127 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: January 26, 2016 - * Description: The Lidar decision node, takes in a raw Lidar scan - * and broadcasts a recommended Twist message - */ - -#ifndef DECISION_LIDAR_DECISION_H -#define DECISION_LIDAR_DECISION_H - -// STD -#include -#include - -// Snowbots -#include -#include - -// ROS -#include -#include -#include -#include - -class LidarDecision { - public: - LidarDecision(int argc, char** argv, std::string node_name); - - /** - * Creates a twist command from a given lidar scan that will best avoid - * obstacles in the scan - * - * @return A twist command from a given lidar scan that will best avoid - * obstacles in the scan - */ - geometry_msgs::Twist - generate_twist_message(const sensor_msgs::LaserScan::ConstPtr& raw_scan); - - /** - * Finds all obstacles in a given scan - * - * @param sensor_msgs\LaserScan a laser scan in which to find obstacles - * @param max_obstacle_angle_diff the maximum possible angular difference - * between two lidar hits - * for them to be considered the same obstacle - * - * @return a vector of all obstacles - */ - static std::vector - findObstacles(const sensor_msgs::LaserScan& scan, - float max_obstacle_angle_diff, - float max_obstacle_distance_diff); - - /** - * Finds the obstacle most dangerous to the robot - * - * @param obstacles all obstacles to be considered - * - * @return the most dangerous obstacle out of those given - */ - static LidarObstacle - mostDangerousObstacle(const std::vector obstacles); - - /** - * Merges obstacles that are separated by less then max_angle_diff together - * - * The merge occurs in place - * - * @param obstacles the list of obstacles to be merged (if similar) - * @param max_angle_diff the max difference that two obstacles may differ by - * and still be considered - * part of the same obstacle - * @param max_distance_diff the max difference that two obstacles may differ - * by and still be considered - * part of the same obstacle - */ - static void mergeSimilarObstacles(std::vector& obstacles, - float max_angle_diff, - float max_distance_diff); - - /** - * Creates a twist message from a given obstacle - * - * - If obstacle is not within danger distance, ignore - * - If obstacle is to left, turn right - * - If obstacle is to right, turn left - * - If obstacle is exactly in front, turn left (arbitrary decision) - * - * @param max_obstacle_danger_distance distance which the obstacle must be - * within to be considered - * @param obstacle the obstacle to be considered - * - * @return a twist message - */ - static geometry_msgs::Twist - twist_message_from_obstacle(LidarObstacle obstacle, - distance_t danger_distance, - angle_t danger_angle, - float linear_vel_multiplier, - float angular_vel_multiplier); - - private: - void scanCallBack(const sensor_msgs::LaserScan::ConstPtr& raw_scan); - - // The maximum angle which two scans can be different by to be considered - // the same obstacle - float max_obstacle_angle_diff; - // The maximum di which two scans can be different by to be considered the - // same obstacle - float max_obstacle_distance_diff; - // The distance at which an obstacle can be and be considered a danger - float max_obstacle_danger_distance; - // The angle, measured from 0 being directly in front of the robot, at which - // an obstacle - // is considered a danger to the robot - float obstacle_danger_angle; - // The multiplier for angular velocity that that robot uses to create twist - // messages - float twist_turn_rate; - // The multiplier for linear velocity that that robot uses to create twist - // messages - float twist_velocity; - ros::Subscriber scan_subscriber; - ros::Publisher twist_publisher; -}; - -#endif // DECISION_LIDAR_DECISION_H diff --git a/src/decision_igvc/include/LidarObstacle.h b/src/decision_igvc/include/LidarObstacle.h deleted file mode 100644 index 232c20ef9..000000000 --- a/src/decision_igvc/include/LidarObstacle.h +++ /dev/null @@ -1,146 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: January 22, 2017 - * Description: This class represents an Obstacle, as detected by the lidar, - * and provides various utilities for working with obstacles - */ - -#ifndef LIDAROBSTACLE_H -#define LIDAROBSTACLE_H - -// STD -#include -#include - -using distance_t = float; -using angle_t = float; - -struct Reading { - angle_t angle; - distance_t range; -}; - -class LidarObstacle { - public: - /** - * Creates a LidarObstacle with no readings - */ - LidarObstacle(); - - /** - * Creates a LidarObstacle with a given distance and angle - * - * @param distance The distance to the obstacle - * @param angle The angle to the obstacle - */ - LidarObstacle(angle_t angle, distance_t distance); - - /** - * Creates a LidarObstacle from a given set of readings (pair) - * - * @param readings a vector of pairs of the form pair - */ - LidarObstacle(std::vector readings); - - /** - * Gets the distance to the rightmost point of the Obstacle - * - * @return the distance to the rightmost point of the Obstacle - */ - distance_t getFirstDistance(); - - /** - * Gets the distance to the leftmost point of the Obstacle - * - * @return the distance to the leftmost point of the obstacle - */ - - distance_t getLastDistance(); - /** - * Gets the average distance of the Obstacle from the robot - * - * The distance from the robot is the average of all scan distances - * - * @return the distance of the Obstacle from the robot - */ - float getAvgDistance(); - - /** - * Gets the angle of the Obstacle from the robot - * - * The angle from the robot is the average of all scan angles - * - * @return the angle of the Obstacle from the robot - */ - float getAvgAngle(); - - /** - * Gets the minimum angle from of an object from the robot - * - * @return the minimum angle of the obstacle from the robot - */ - float getMinAngle(); - - /** - * Gets the maximum angle from of an object from the robot - * - * @return the maximum angle of the obstacle from the robot - */ - float getMaxAngle(); - - /** - * Gets the minimum distance from of an object from the robot - * - * @return the minimum distance of the obstacle from the robot - */ - float getMinDistance(); - - /** - * Gets the maximum distance from of an object from the robot - * - * @return the maximum distance of the obstacle from the robot - */ - float getMaxDistance(); - - /** - * Calculates a danger score for the obstacle - * - * ie. how dangerous the obstacle is to the robot - * - * @return danger_score how dangerous the obstacle is to the robot - */ - float dangerScore(); - - /** - * Gets all laser readings comprising the obstacle - * - * @return readings A list of pairs of all laser readings - */ - const std::vector& getAllLaserReadings(); - - /** - * Merges the given LidarObstacle in to this LidarObstacle - * - * Adds the given LidarObstacle's scan distances and scan angles to - * this LidarObstacles scan distances and scan angles respectively - * - * @param obstacle The LidarObstacle to be merged in - */ - void mergeInLidarObstacle(LidarObstacle obstacle); - - private: - /** - * Merges a given set of readings into the obstacle - * - * @param readings the readings to be merged in - */ - void mergeInReadings(std::vector& new_readings); - - // The distances and angles of all the laser scan hits that comprise the - // object. - // pairs are stored in sorted order, from min to max angle. - std::vector readings; -}; - -#endif // LIDAROBSTACLE_H diff --git a/src/decision_igvc/include/VisionDecision.h b/src/decision_igvc/include/VisionDecision.h deleted file mode 100644 index 057e053c0..000000000 --- a/src/decision_igvc/include/VisionDecision.h +++ /dev/null @@ -1,239 +0,0 @@ -/* - * Created By: Robyn Castro - * Created On: September 22, 2016 - * Description: The vision decision node, takes in an image from the robot's - * camera and produces a recommended twist message - */ - -#ifndef DECISION_VISION_DECISION_H -#define DECISION_VISION_DECISION_H - -// STD -#include - -// Snowbots -#include - -// ROS -#include -#include -#include -#include -#include -#include - -// This constant is used when verifying if a pixel is noise. -// It determines the maximum consecutive number of pixels that will still -// be considered noise. The lower the number, the smaller the expected noise -// size. -const int NOISE_MAX = 10; - -const bool DEBUG = false; - -// Since 90 can never be returned by arctan. 90 will be used as a special angle -// to signal desired angular speed and desired linear speed functions to return -// 0. -const int STOP_SIGNAL_ANGLE = 90; - -class VisionDecision { - public: - VisionDecision(int argc, char** argv, std::string node_name); - - /** - * Determines the turning angle in relation to the orientation of - * the white line in the image. - * - * @param numSamples determines the number of samples to average when - * determining - * the angle. - * @param image_scan the image to parse - * - * @return the angle of the line to the positive y-axis. - */ - static int getDesiredAngle(double numSamples, - const sensor_msgs::Image::ConstPtr& image, - double rolling_average_constant, - double percent_of_image_sampled, - double percent_of_samples_needed, - double& confidence_value, - double move_away_threshold, - double percent_of_white_needed); - - /** - * Determines the angle of the line parsed from the left or right side. - * - * @param rightSide determines whether to parse from the left or from the - * right side. - * @param numSamples how many slopes to sample the angle. - * @param image_scan the image to parse. - * - * @returns the angle of the line, or INVALID if line is invalid. - */ - static int getAngleOfLine(bool rightSide, - double numSamples, - const sensor_msgs::Image::ConstPtr& image, - double rolling_average_constant, - double percent_of_samples_needed, - double& validSamples); - - /** - * Returns a rotation speed based on the imageRatio - * - * @param imageRatio - * rightBlackPixels - leftBlackPixels - * @returns double - * rotation speed of robot - */ - static double getDesiredAngularSpeed(double desiredAngle); - - /** - * Returns the desired forward speed - * - * @param desiredAngle - * angle robot will turn - * @returns double - * moving speed percentage of robot - */ - static double getDesiredLinearSpeed(double desiredAngle); - - private: - /** - * Determines the middle of the white line given a row - * - * @param startingPos which column to start parsing in - * @param row the row to parse - * @param rightSide determines whether to parse from the right or the left - * @param image_scan the image to parse - */ - static int getMiddle(int startingPos, - int row, - bool rightSide, - const sensor_msgs::Image::ConstPtr& image); - - /* - * Returns the edge pixel of the line. Which side depends on parameter - * isStartPixel. - * - * @param startingPos column to start parsing - * @param NOISE+MAX how large noise can be - * @param incrementer decides whether to parse from the left or from the - * right - * @param row determines the row to parse - * @param image_scan the image to parse - * @param isStartPixel determines which edge pixel of the white line to - * return. - * - * @returns the white pixel's column position, -1 if none found - */ - static int getEdgePixel(int startingPos, - int incrementer, - int row, - const sensor_msgs::Image::ConstPtr& image, - bool isStartPixel); - - /** - * Re-maps a number from one range to another - * - * @param x - * the number to map - * @param inMin - * lower bound of value's current range - * @param inMax - * upper bound of value's current range - * @param outMin - * lower bound of value's target range - * @param outMax - * upper bound of value's target range - * @return double - * the mapped number - */ - static double mapRange( - double x, double inMin, double inMax, double outMin, double outMax); - - /** - * Initializes the incrementer's starting position and how it will parse. - * - * @param image_scan the image to parse. - * @param rightSide determines whether to parse from the left or the right - * side of the image. - * @param startingPos where the parser will start parsing the image. - */ - static int initializeIncrementerPosition( - bool rightSide, - const sensor_msgs::Image::ConstPtr& image_scan, - int* startingPos); - - /** - * Checks whether the line is perpendicular to the robot's vision. - * - * @param image_scan the image to parse. - */ - static bool isPerpendicular(const sensor_msgs::Image::ConstPtr& image_scan); - - /** - * Gets the first valid white pixel bottom to top at the specified column. - * - * @param image_scan the image to parse. - * @param column the column to parse at. - * - */ - static int - getVerticalEdgePixel(const sensor_msgs::Image::ConstPtr& image_scan, - int column); - - /* - * Returns an angle that moves away from the line - * - * @param image_scan the image to parse - */ - static int moveAwayFromLine(const sensor_msgs::Image::ConstPtr& image); - - /* - * Returns the difference of rightWhitePixels - leftWhitePixels - * - * @param image_scan the image to parse - */ - static int - getLeftToRightPixelRatio(const sensor_msgs::Image::ConstPtr& image); - - void imageCallBack(const sensor_msgs::Image::ConstPtr& image); - - void publishTwist(geometry_msgs::Twist twist); - - static double getConfidence(const sensor_msgs::Image::ConstPtr& image_scan, - double percent_of_image_sampled, - double percent_of_samples_needed, - double valid_samples); - - ros::Subscriber image_subscriber; - ros::Publisher twist_publisher; - - // The value to scale the angular velocity on the - // twist message we create by - double angular_velocity_multiplier; - - // The max value of angular velocity given by twist - // message. - double angular_velocity_cap; - - // Dictates how much new samples will influence the current - // average (Smaller value means less influence). - double rolling_average_constant; - - // Dictates how much of the image samples need to be valid. - double percent_of_samples_needed; - - // Dictates how much of the image is sampled - double percent_of_image_sampled; - - // Dicates the angle to just ignore angle and turn away from the line - double move_away_threshold; - - // Dictates how confident vision has to be to move - double confidence_threshold; - - // Dictates how much white needs to be onscreen to be confident - double percent_of_white_needed; -}; - -#endif // DECISION_VISION_DECISION_H diff --git a/src/decision_igvc/launch/decision_igvc.launch b/src/decision_igvc/launch/decision_igvc.launch deleted file mode 100644 index cd0d8be0d..000000000 --- a/src/decision_igvc/launch/decision_igvc.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/src/decision_igvc/launch/final_decision.launch b/src/decision_igvc/launch/final_decision.launch deleted file mode 100755 index c2140c226..000000000 --- a/src/decision_igvc/launch/final_decision.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/src/decision_igvc/launch/gps_decision.launch b/src/decision_igvc/launch/gps_decision.launch deleted file mode 100755 index 6894c82d8..000000000 --- a/src/decision_igvc/launch/gps_decision.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - 1.0 - - - 0.7 - - - 0.1 - - - - 1.0 - - - 0.2 - - - - 0.4 - - base_link - map - - - diff --git a/src/decision_igvc/launch/gps_manager.launch b/src/decision_igvc/launch/gps_manager.launch deleted file mode 100755 index e25e537f9..000000000 --- a/src/decision_igvc/launch/gps_manager.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - 0.5 - - [ - 49.2626396517, -123.248471077, - 49.2624439204, -123.248579687 - ] - - base_link - map - - - diff --git a/src/decision_igvc/launch/lidar_decision.launch b/src/decision_igvc/launch/lidar_decision.launch deleted file mode 100644 index a75d0df8c..000000000 --- a/src/decision_igvc/launch/lidar_decision.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - 0.08727 - 10 - - - 0.7854 - - 10 - 10 - - diff --git a/src/decision_igvc/launch/vision_decision.launch b/src/decision_igvc/launch/vision_decision.launch deleted file mode 100644 index 67699e207..000000000 --- a/src/decision_igvc/launch/vision_decision.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - 1.0 - 1.0 - 0.25 - 0.125 - 0.25 - 25.0 - 60.0 - 0.05 - - - - diff --git a/src/decision_igvc/launch/waypoint_navigation.launch b/src/decision_igvc/launch/waypoint_navigation.launch deleted file mode 100755 index 93667b7d0..000000000 --- a/src/decision_igvc/launch/waypoint_navigation.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - diff --git a/src/decision_igvc/src/FinalDecision.cpp b/src/decision_igvc/src/FinalDecision.cpp deleted file mode 100644 index 4db86d23d..000000000 --- a/src/decision_igvc/src/FinalDecision.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Created By: Kevin Luo - * Modified by: Valerian Ratu - * Created On: September 22, 2016 - * Description: The Final Decision node, arbitrates between the decisions of - * other nodes. - */ - -#include - -FinalDecision::FinalDecision(int argc, char** argv, std::string node_name) { - // Setup NodeHandles - ros::init(argc, argv, node_name); - ros::NodeHandle public_nh; - ros::NodeHandle private_nh("~"); - - // Setup Subscriber(s) - std::string lidar_decision_topic_name = "/lidar_decision/twist"; - std::string vision_decision_topic_name = "/vision_decision/twist"; - std::string gps_decision_topic_name = "/gps_decision/twist"; - - uint32_t queue_size = 1; - lidar_subscriber = public_nh.subscribe( - lidar_decision_topic_name, queue_size, &FinalDecision::lidarCallBack, this); - vision_subscriber = public_nh.subscribe(vision_decision_topic_name, - queue_size, - &FinalDecision::visionCallBack, - this); - gps_subscriber = public_nh.subscribe( - gps_decision_topic_name, queue_size, &FinalDecision::gpsCallBack, this); - - // Setup Publisher(s) - std::string twist_topic = "/cmd_vel"; - twist_publisher = - public_nh.advertise(twist_topic, queue_size); -} - -void FinalDecision::lidarCallBack( -const geometry_msgs::Twist::ConstPtr& lidar_decision) { - recent_lidar = *lidar_decision; - publishTwist(arbitrator(recent_lidar, recent_vision, recent_gps)); -} - -void FinalDecision::visionCallBack( -const geometry_msgs::Twist::ConstPtr& vision_decision) { - recent_vision = *vision_decision; - publishTwist(arbitrator(recent_lidar, recent_vision, recent_gps)); -} - -void FinalDecision::gpsCallBack( -const geometry_msgs::Twist::ConstPtr& gps_decision) { - recent_gps = *gps_decision; - publishTwist(arbitrator(recent_lidar, recent_vision, recent_gps)); -} - -void FinalDecision::publishTwist(geometry_msgs::Twist twist) { - twist_publisher.publish(twist); -} - -geometry_msgs::Twist -FinalDecision::arbitrator(geometry_msgs::Twist recent_lidar, - geometry_msgs::Twist recent_vision, - geometry_msgs::Twist recent_gps) { - if (recent_lidar.angular.z != 0) { - return recent_lidar; - } else if (fabs(recent_vision.angular.z) != 0) { - return recent_vision; - } else { - return recent_gps; - } -} diff --git a/src/decision_igvc/src/GpsDecision.cpp b/src/decision_igvc/src/GpsDecision.cpp deleted file mode 100644 index 970cfc9fd..000000000 --- a/src/decision_igvc/src/GpsDecision.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: May 18, 2017 - * Description: The Decision Node for GPS, takes in a point relative to - * the robots location and heading and broadcasts a - * recommended twist message. Revised to use TF's - */ - -#include -#include - -GpsDecision::GpsDecision(int argc, char** argv, std::string node_name) { - // Setup NodeHandles - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - // Setup Subscriber(s) - uint32_t queue_size = 1; - std::string waypoint_topic = "/gps_manager/current_waypoint"; - waypoint_subscriber = nh.subscribe( - waypoint_topic, queue_size, &GpsDecision::waypointCallback, this); - - // Setup Publisher(s) - std::string twist_publisher_topic = private_nh.resolveName("twist"); - twist_publisher = private_nh.advertise( - twist_publisher_topic, queue_size); - - // Setup the GpsMover class, which will figure out what command to send to - // the robot - // based on our distance and heading to the destination GPS waypoint - double linear_heading_factor, linear_distance_factor, - angular_heading_factor, angular_distance_factor; - SB_getParam( - private_nh, "linear_heading_factor", linear_heading_factor, 1.0); - SB_getParam( - private_nh, "linear_distance_factor", linear_distance_factor, 1.0); - SB_getParam( - private_nh, "angular_heading_factor", angular_heading_factor, 1.0); - SB_getParam( - private_nh, "angular_distance_factor", angular_distance_factor, 1.0); - mover.setFactors(linear_distance_factor, - linear_heading_factor, - angular_distance_factor, - angular_heading_factor); - - double linear_cap, angular_cap; - SB_getParam(private_nh, "max_linear_speed", linear_cap, 1.0); - SB_getParam(private_nh, "max_angular_speed", angular_cap, 1.0); - mover.setMaxSpeeds(linear_cap, angular_cap); - - SB_getParam( - private_nh, "base_frame", base_frame, (std::string) "base_link"); - SB_getParam( - private_nh, "global_frame", global_frame, (std::string) "odom_combined"); -} - -void GpsDecision::waypointCallback( -const geometry_msgs::PointStamped::ConstPtr& waypoint) { - // Get the current position and heading of the robot - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); - try { - geometry_msgs::TransformStamped tfStamped = tf_buffer.lookupTransform( - global_frame, base_frame, ros::Time(0), ros::Duration(1.0)); - // Get our current heading and location from the global_frame <-> - // base_frame tf - geometry_msgs::Point current_location; - current_location.x = tfStamped.transform.translation.x; - current_location.y = tfStamped.transform.translation.y; - double current_heading = tf::getYaw(tfStamped.transform.rotation); - // Get the non-stamped waypoint, because we don't care about the - // stamped info at this point - geometry_msgs::Point non_stamped_waypoint = waypoint->point; - // Create a new twist message and publish it - geometry_msgs::Twist twist = mover.createTwistMessage( - current_location, current_heading, non_stamped_waypoint); - twist_publisher.publish(twist); - } catch (tf2::LookupException e) { - // If we can't lookup the tf, then warn the user and tell robot to stop - ROS_WARN_STREAM("Could not lookup tf between " << global_frame - << " and " - << base_frame); - // TODO: Confirm that this is initialized to 0 (or find a nice way to - // init it to 0) - geometry_msgs::Twist allZeroTwist; - twist_publisher.publish(allZeroTwist); - } -} diff --git a/src/decision_igvc/src/GpsManager.cpp b/src/decision_igvc/src/GpsManager.cpp deleted file mode 100644 index ad838420a..000000000 --- a/src/decision_igvc/src/GpsManager.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: May 17, 2017 - * Description: Manages the GPS waypoints we are given - * (publishing the next one once we've arrived - * at the current one) - */ - -#include -#include -#include - -GpsManager::GpsManager(int argc, char** argv, std::string node_name) { - // Setup NodeHandles - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - // Setup Subscribers - uint32_t queue_size = 1; - std::string tf_topic_name = "/tf"; - tf_subscriber = - nh.subscribe(tf_topic_name, queue_size, &GpsManager::tfCallBack, this); - - // Setup Publishers - std::string current_waypoint_topic = - private_nh.resolveName("current_waypoint"); - current_waypoint_publisher = nh.advertise( - current_waypoint_topic, queue_size); - std::string marker_topic = - private_nh.resolveName("current_waypoint_marker"); - rviz_marker_publisher = - nh.advertise(marker_topic, queue_size); - - // Get Params - SB_getParam( - private_nh, "base_frame", base_frame, (std::string) "base_link"); - SB_getParam( - private_nh, "global_frame", global_frame, (std::string) "odom_combined"); - SB_getParam( - private_nh, "at_goal_tolerance", at_goal_tolerance, (float) 1.0); - std::vector - waypoints_raw; // The raw list of waypoints retrieved from the param server - if (!SB_getParam(private_nh, "waypoints", waypoints_raw)) { - ROS_ERROR( - "Waypoints should be a list in the form [lat, lon, lat, lon, ...]"); - ros::shutdown(); - } else { - std::vector waypoint_list = parseWaypoints(waypoints_raw); - populateWaypointStack(waypoint_list); - } -} - -void GpsManager::tfCallBack(const tf2_msgs::TFMessageConstPtr tf_message) { - // If we've visited all the waypoints, no need to do anything here, - // In fact, we can celebrate and shutdown - if (waypoint_stack.size() <= 0) { - ROS_INFO_STREAM("Reached all waypoints! *cue digital clapping*"); - ros::shutdown(); - return; - } - - // Set the TimeStamp on the current waypoint and publish it - waypoint_stack.top().header.stamp = ros::Time::now(); - current_waypoint_publisher.publish(waypoint_stack.top()); - - // Check if the tf_message contains the transform we're looking for - for (geometry_msgs::TransformStamped tf_stamped : tf_message->transforms) { - if (tf_stamped.header.frame_id == global_frame && - tf_stamped.child_frame_id == base_frame) { - // We've found the transform we're looking for, so see how close we - // are to the waypoint - geometry_msgs::PointStamped curr_waypoint = waypoint_stack.top(); - double dx = - tf_stamped.transform.translation.x - curr_waypoint.point.x; - double dy = - tf_stamped.transform.translation.y - curr_waypoint.point.y; - double distance = sqrt(pow(dx, 2) + pow(dy, 2)); - if (distance < at_goal_tolerance) { - waypoint_stack.pop(); - ROS_INFO_STREAM("Hit waypoint " << waypoint_stack.size()); - } - publishRvizWaypointMarker(curr_waypoint, tf_stamped); - } - } -} - -void GpsManager::publishRvizWaypointMarker( -geometry_msgs::PointStamped p, -geometry_msgs::TransformStamped global_to_local_transform) { - // Inverse the transform - tf2::Transform t; - tf2::fromMsg(global_to_local_transform.transform, t); - t = t.inverse(); - - // Create a new geometry_msgs::Transform - geometry_msgs::TransformStamped t_stamped; - t_stamped.transform = tf2::toMsg(t); - t_stamped.header = global_to_local_transform.header; - t_stamped.header.frame_id = base_frame; - - // Define a new point relative to base frame - geometry_msgs::PointStamped output; - tf2::doTransform(p, output, t_stamped); - output.point.z = 0; // Necessary to remove vertical transform of waypoint - - // Create marker - std::vector colors; - std_msgs::ColorRGBA color; - color.a = 1.0f; - color.r = 1.0f; - colors.push_back(color); - rviz_marker_publisher.publish(snowbots::RvizUtils::createMarker( - output.point, - colors, - snowbots::RvizUtils::createMarkerScale(0.5, 0.5, 0.5), - base_frame, - "debug", - visualization_msgs::Marker::POINTS)); -} - -std::vector -GpsManager::parseWaypoints(std::vector waypoints_raw) { - if (waypoints_raw.size() % 2 != 0) { - ROS_ERROR( - "Given odd length list of waypoints, check that \"waypoints\" " - "parameter " - "has an (numerically) even length. As this is a series of lat/lon " - "coordinates," - "it should always be even"); - ros::shutdown(); - return {}; - } - - std::vector waypoints; - for (int i = 0; i < waypoints_raw.size(); i += 2) { - Waypoint curr_waypoint; - curr_waypoint.lat = waypoints_raw[i]; - curr_waypoint.lon = waypoints_raw[i + 1]; - waypoints.push_back(curr_waypoint); - } - - return waypoints; -} - -void GpsManager::populateWaypointStack(std::vector waypoint_list) { - for (int i = (int) waypoint_list.size() - 1; i >= 0; i--) { - // Convert the waypoint to UTM - double northing, easting; - std::string zone_throwaway; - gps_common::LLtoUTM(waypoint_list[i].lat, - waypoint_list[i].lon, - northing, - easting, - zone_throwaway); - - // Build a point with our UTM coordinates - geometry_msgs::PointStamped point_stamped; - point_stamped.header.frame_id = global_frame; - point_stamped.header.stamp = ros::Time(); - point_stamped.point.x = easting; - point_stamped.point.y = northing; - - waypoint_stack.push(point_stamped); - } - - ROS_INFO_STREAM("Ready to rock 'n' roll with " << waypoint_stack.size() - << " waypoints!"); -} diff --git a/src/decision_igvc/src/GpsMover.cpp b/src/decision_igvc/src/GpsMover.cpp deleted file mode 100644 index dfe8d9861..000000000 --- a/src/decision_igvc/src/GpsMover.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: April 1, 2017 - * Description: The implementation for the GpsMover class. This class - * contains all the methods required to decide on the - * appropriate twist message to make the robot move smoothly - * towards a given point from it's current location - */ - -#include - -double -GpsMover::magicFunction(double x, double y, double x_scale, double y_scale) { - return (1 / fabs(x) * x_scale + sqrt(fabs(y)) * y_scale) / 2; -} - -GpsMover::GpsMover(double linear_distance_factor, - double linear_heading_factor, - double angular_distance_factor, - double angular_heading_factor) { - setFactors(linear_distance_factor, - linear_heading_factor, - angular_distance_factor, - angular_heading_factor); -} - -void GpsMover::setFactors(double linear_distance_factor, - double linear_heading_factor, - double angular_distance_factor, - double angular_heading_factor) { - this->linear_distance_factor = linear_distance_factor; - this->angular_distance_factor = angular_distance_factor; - this->linear_heading_factor = linear_heading_factor; - this->angular_heading_factor = angular_heading_factor; -} - -void GpsMover::setMaxSpeeds(double max_linear_speed, double max_angular_speed) { - this->max_linear_speed = max_linear_speed; - this->max_angular_speed = max_angular_speed; -} - -geometry_msgs::Twist -GpsMover::createTwistMessage(geometry_msgs::Point current_location, - double current_heading, - geometry_msgs::Point waypoint) { - // The command to return - geometry_msgs::Twist command; - - // Set components we don't care about to 0 - command.linear.y = 0; - command.linear.z = 0; - command.angular.x = 0; - command.angular.y = 0; - - // Figure out the minimum we have to turn to point directly at the waypoint - double angle_to_waypoint = angleBetweenPoints(current_location, waypoint); - double min_turning_angle = - minAngularChange(current_heading, angle_to_waypoint); - - // Figure out how far we are from the waypoint - double dx = waypoint.x - current_location.x; - double dy = waypoint.y - current_location.y; - double distance = sqrt(pow(dx, 2) + pow(dy, 2)); - - // Figure out how fast we should turn - command.angular.z = magicFunction( - distance, min_turning_angle, linear_distance_factor, linear_heading_factor); - - // Figure out if we should be turning left or right - command.angular.z *= (min_turning_angle > 0) ? 1 : -1; - - // Figure out how fast we should move forward - command.linear.x = magicFunction( - min_turning_angle, distance, linear_heading_factor, linear_distance_factor); - - // Cap our angular and linear speeds - capValue(command.linear.x, max_linear_speed); - capValue(command.angular.z, max_angular_speed); - - return command; -} - -double GpsMover::minAngularChange(double from_heading, double to_heading) { - // Find the angular difference (mod 2*Pi) - double angle_diff = fmod((to_heading - from_heading), 2 * M_PI); - // If obtuse angle, then get the acute angle - if (fabs(angle_diff) > M_PI) - return (2.0 * M_PI - fabs(angle_diff)) * ((angle_diff > 0) ? -1 : 1); - else - return angle_diff; -} - -double GpsMover::angleBetweenPoints(geometry_msgs::Point startPoint, - geometry_msgs::Point endPoint) { - double dx = endPoint.x - startPoint.x; - double dy = endPoint.y - startPoint.y; - double angle = atan(dy / dx); - // If the endpoint is behind and to the left - if (dx < 0 && dy > 0) angle += M_PI; - // If the endpoint is behind and to the right - else if (dx < 0 && dy < 0) - angle -= M_PI; - return angle; -} - -void GpsMover::capValue(double& val, double cap) { - if (fabs(val) > cap) val = cap * val / fabs(val); -} diff --git a/src/decision_igvc/src/LidarDecision.cpp b/src/decision_igvc/src/LidarDecision.cpp deleted file mode 100644 index 2559d3a83..000000000 --- a/src/decision_igvc/src/LidarDecision.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: January 26, 2016 - * Description: The Lidar decision node, takes in a raw Lidar scan - * and broadcasts a recommended Twist message - */ - -#include - -// The constructor -LidarDecision::LidarDecision(int argc, char** argv, std::string node_name) { - // Setup NodeHandles - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle public_nh("~"); - - // Setup Subscriber(s) - std::string laserscan_topic_name = "/robot/laser/scan"; - uint32_t refresh_rate = 10; - scan_subscriber = public_nh.subscribe( - laserscan_topic_name, refresh_rate, &LidarDecision::scanCallBack, this); - - // Setup Publisher(s) - std::string twist_topic = public_nh.resolveName("twist"); - uint32_t queue_size = 10; - twist_publisher = - nh.advertise(twist_topic, queue_size); - - // Get Param(s) - SB_getParam(public_nh, - "max_obstacle_angle_diff", - max_obstacle_angle_diff, - (float) M_PI / 36); - SB_getParam(public_nh, - "max_obstacle_danger_distance", - max_obstacle_danger_distance, - (float) 10); - SB_getParam(public_nh, - "obstacle_danger_angle", - obstacle_danger_angle, - (float) M_PI / 4); - SB_getParam(public_nh, "twist_turn_rate", twist_turn_rate, (float) 10); - SB_getParam(public_nh, "twist_velocity", twist_velocity, (float) 10); -} - -// This is called whenever a new message is received -void LidarDecision::scanCallBack( -const sensor_msgs::LaserScan::ConstPtr& raw_scan) { - // Deal with new messages here - twist_publisher.publish(generate_twist_message(raw_scan)); -} - -geometry_msgs::Twist LidarDecision::generate_twist_message( -const sensor_msgs::LaserScan::ConstPtr& raw_scan) { - // Find all the obstacles - std::vector obstacles = findObstacles( - *raw_scan, max_obstacle_angle_diff, max_obstacle_distance_diff); - - // Check that we have at least one obstacle - if (obstacles.size() >= 1) { - // Choose the most dangerous obstacle - LidarObstacle most_dangerous_obstacle = - mostDangerousObstacle(obstacles); - - // Create and return a twist message based on the most dangerous - // obstacle - return twist_message_from_obstacle(most_dangerous_obstacle, - max_obstacle_danger_distance, - obstacle_danger_angle, - twist_velocity, - twist_turn_rate); - } else { - // We don't have any obstacles, return a all zero twist message - geometry_msgs::Twist all_zero; - all_zero.linear.x = 0; - all_zero.linear.y = 0; - all_zero.linear.z = 0; - all_zero.angular.x = 0; - all_zero.angular.y = 0; - all_zero.angular.z = 0; - return all_zero; - } -} - -std::vector -LidarDecision::findObstacles(const sensor_msgs::LaserScan& scan, - float max_obstacle_angle_diff, - float max_obstacle_distance_diff) { - // Get the raw scan data - std::vector scan_data = scan.ranges; - - // Find all ranges (lidar hits) that could be obstacles (initially each - // laser hit is an obstacle) - std::vector obstacles = {}; - for (int i = 0; i < scan_data.size(); i++) { - // Check that obstacle is within valid range - if (scan_data[i] < scan.range_max && scan_data[i] > scan.range_min) { - // If so, add it to the obstacles - obstacles.emplace_back(LidarObstacle( - scan.angle_increment * i + scan.angle_min, scan_data[i])); - } - } - - // Merge together obstacles that could be the same obstacle - mergeSimilarObstacles( - obstacles, max_obstacle_angle_diff, max_obstacle_distance_diff); - - return obstacles; -} - -LidarObstacle LidarDecision::mostDangerousObstacle( -const std::vector obstacles) { - // Return obstacle with the greatest danger score - return *std::max_element(obstacles.begin(), - obstacles.end(), - [&](LidarObstacle obs1, LidarObstacle obs2) { - return obs1.dangerScore() < obs2.dangerScore(); - }); -} - -void LidarDecision::mergeSimilarObstacles(std::vector& obstacles, - float max_angle_diff, - float max_distance_diff) { - // Ensure the list of obstacles is sorted in order of ascending angle - std::sort(obstacles.begin(), - obstacles.end(), - [&](LidarObstacle l1, LidarObstacle l2) { - return l1.getAvgAngle() < l2.getAvgAngle(); - }); - - // Merge similar obstacles - int i = 0; - while (i < (long) obstacles.size() - 1) { - // Check if angle difference between two consecutive scans is less than - // max_angle_diff and max_distance_diff - if (fabs(obstacles[i + 1].getMinAngle() - obstacles[i].getMaxAngle()) < - max_angle_diff && - fabs(obstacles[i + 1].getFirstDistance() - - obstacles[i].getLastDistance()) < max_distance_diff) { - // Merge next obstacle into current one - obstacles[i].mergeInLidarObstacle(obstacles[i + 1]); - obstacles.erase(obstacles.begin() + i + 1); - } else { - i++; - } - } -} - -geometry_msgs::Twist -LidarDecision::twist_message_from_obstacle(LidarObstacle obstacle, - distance_t danger_distance, - angle_t danger_angle, - float linear_vel_multiplier, - float angular_vel_multiplier) { - geometry_msgs::Twist twist; - twist.linear.x = 0; - twist.linear.y = 0; - twist.linear.z = 0; - twist.angular.x = 0; - twist.angular.y = 0; - twist.angular.z = 0; - - if (obstacle.getMinDistance() <= danger_distance && - abs(obstacle.getAvgAngle()) <= danger_angle) { - float minDistance = obstacle.getMinDistance(); - // Calculate linear x dependent on how close obstacle is to the robot - twist.linear.x = sqrt(minDistance); - // Calculate angular z dependent on how aligned obstacle is to the robot - float angle_score = fabs(cos(obstacle.getAvgAngle())); - // Calculate angular z dependent on how close obstacle is to the robot - float distance_score = 1 / minDistance; - // Choose one score determined if angle or distance is more dangerous - if (angle_score >= distance_score) - twist.angular.z = angular_vel_multiplier * angle_score; - else { - // If distance score is too large robot will oversteer and come back - // around, so just set as - // multiplier value instead (should be set to 1.5). This translates - // angular z to ~90° turn - twist.angular.z = (distance_score > 1) - ? linear_vel_multiplier - : linear_vel_multiplier * distance_score; - } - // Check if we should be turning left or right away from obstacle - if (obstacle.getAvgAngle() > 0) twist.angular.z *= -1; - } - return twist; -} diff --git a/src/decision_igvc/src/LidarObstacle.cpp b/src/decision_igvc/src/LidarObstacle.cpp deleted file mode 100644 index 78534aab4..000000000 --- a/src/decision_igvc/src/LidarObstacle.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: January 22, 2017 - * Description: This class represents an Obstacle, as detected by the lidar, - * and provides various utilities for working with obstacles - */ - -// STD Includes -#include -#include - -// Snowbots Includes -#include - -LidarObstacle::LidarObstacle(){}; - -LidarObstacle::LidarObstacle(angle_t angle, distance_t distance) { - readings.emplace_back(Reading{angle, distance}); -} - -LidarObstacle::LidarObstacle(std::vector readings) { - this->mergeInReadings(readings); -} - -distance_t LidarObstacle::getAvgDistance() { - float total_distance = std::accumulate( - readings.begin(), readings.end(), 0, [](int accumulator, auto reading) { - return accumulator + reading.range; - }); - return total_distance / readings.size(); -} - -angle_t LidarObstacle::getAvgAngle() { - float total_angle = std::accumulate( - readings.begin(), readings.end(), 0.0, [](float accumulator, auto reading) { - return accumulator + reading.angle; - }); - return total_angle / readings.size(); -} - -angle_t LidarObstacle::getMinAngle() { - // Readings are sorted, so min angle is just the first Reading - return readings[0].angle; -} - -angle_t LidarObstacle::getMaxAngle() { - // Readings are sorted, so max angle is just the last Reading - return readings[readings.size() - 1].angle; -} - -distance_t LidarObstacle::getLastDistance() { - return readings[readings.size() - 1].range; -} - -distance_t LidarObstacle::getFirstDistance() { - return readings[0].range; -} - -const std::vector& LidarObstacle::getAllLaserReadings() { - return readings; -}; - -distance_t LidarObstacle::getMinDistance() { - std::vector readings = getAllLaserReadings(); - Reading reading_with_min_distance = - *std::min_element(readings.begin(), - readings.end(), - [&](auto const& reading1, auto const& reading2) { - return reading1.range < reading2.range; - }); - return reading_with_min_distance.range; -} - -distance_t LidarObstacle::getMaxDistance() { - std::vector readings = getAllLaserReadings(); - Reading reading_with_max_distance = - *std::max_element(readings.begin(), - readings.end(), - [&](auto const& reading1, auto const& reading2) { - return reading1.range > reading2.range; - }); - return reading_with_max_distance.range; -} - -float LidarObstacle::dangerScore() { - // angle score increases as an obstacle's angle relative to the robot - // increases - float angle_score = std::cos(getAvgAngle()); - // distance score increases as an obstacle's distance relative to the robot - // increases - float distance_score = (1 / getMinDistance()); - // danger score is sum of angle score and distance score - return (angle_score + distance_score); -} - -void LidarObstacle::mergeInReadings(std::vector& new_readings) { - this->readings.insert( - readings.end(), new_readings.begin(), new_readings.end()); - // Ensure that the readings are still sorted - std::sort(readings.begin(), - readings.end(), - [&](auto const& reading1, auto const& reading2) { - return reading1.angle < reading2.angle; - }); -} - -void LidarObstacle::mergeInLidarObstacle(LidarObstacle obstacle) { - // Get readings from obstacle being merged in - std::vector new_readings = obstacle.getAllLaserReadings(); - this->mergeInReadings(new_readings); -} diff --git a/src/decision_igvc/src/VisionDecision.cpp b/src/decision_igvc/src/VisionDecision.cpp deleted file mode 100644 index ff9f0aa9e..000000000 --- a/src/decision_igvc/src/VisionDecision.cpp +++ /dev/null @@ -1,451 +0,0 @@ -/* - * Created By: Robyn Castro - * Created On: September 22, 2016 - * Description: The vision decision node, takes in an image from the robot's - * camera and produces a recommended twist message - */ -#include - -// The constructor -VisionDecision::VisionDecision(int argc, char** argv, std::string node_name) { - ros::init(argc, argv, node_name); - - // Setup NodeHandles - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - // Setup Subscriber(s) - std::string camera_image_topic_name = "/vision/filtered_image"; - uint32_t queue_size = 1; - image_subscriber = private_nh.subscribe( - camera_image_topic_name, queue_size, &VisionDecision::imageCallBack, this); - - // Setup Publisher(s) - std::string twist_topic = private_nh.resolveName("twist"); - twist_publisher = - nh.advertise(twist_topic, queue_size); - - // Get Param(s) - SB_getParam( - private_nh, "angular_vel_multiplier", angular_velocity_multiplier, 1.0); - SB_getParam(private_nh, "angular_vel_cap", angular_velocity_cap, 1.0); - SB_getParam( - private_nh, "rolling_average_constant", rolling_average_constant, 0.25); - SB_getParam( - private_nh, "percent_of_samples_needed", percent_of_samples_needed, 0.125); - SB_getParam( - private_nh, "percent_of_image_sampled", percent_of_image_sampled, 0.25); - SB_getParam(private_nh, "move_away_threshold", move_away_threshold, 25.0); - SB_getParam(private_nh, "confidence_threshold", confidence_threshold, 60.0); - SB_getParam( - private_nh, "percent_of_white_needed", percent_of_white_needed, 0.05); -} - -// This is called whenever a new message is received -void VisionDecision::imageCallBack( -const sensor_msgs::Image::ConstPtr& image_scan) { - // Deal with new messages here - geometry_msgs::Twist twistMsg; - - double confidence_value = 0; - // Decide how much to turn - int relativeAngle = - getDesiredAngle(image_scan->height * percent_of_image_sampled, - image_scan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed); - - // Initialize linear velocities to 0 - twistMsg.linear.y = 0; - twistMsg.linear.z = 0; - - // Initialize x and y angular velocities to 0 - twistMsg.angular.x = 0; - twistMsg.angular.y = 0; - - // Decide how fast to move - twistMsg.linear.x = getDesiredLinearSpeed(relativeAngle); - - // Decide how fast to turn - twistMsg.angular.z = - -angular_velocity_multiplier * getDesiredAngularSpeed(relativeAngle); - - // Cap the absolute value of the turning velocity - if (fabs(twistMsg.angular.z) > angular_velocity_cap) - twistMsg.angular.z = - angular_velocity_cap * twistMsg.angular.z / fabs(twistMsg.angular.z); - - // Publish the twist message - publishTwist(twistMsg); -} - -void VisionDecision::publishTwist(geometry_msgs::Twist twist) { - twist_publisher.publish(twist); -} - -/* Functions to determine robot movement */ - -int VisionDecision::getDesiredAngle( -double numSamples, -const sensor_msgs::Image::ConstPtr& image_scan, -double rolling_average_constant, -double percent_of_image_sampled, -double percent_of_samples_needed, -double& confidence_value, -double move_away_threshold, -double percent_of_white_needed) { - int row, col; - int whiteCount = 0; - confidence_value = 0; - - // Check if there is a white line in the way of the robot - for (row = 0; row < image_scan->height; row++) { - for (col = 0; col < image_scan->width; col++) - if (image_scan->data[row * image_scan->width + col] != 0) - whiteCount++; - } - - double valid_samples; - - int leftToRightAngle = getAngleOfLine(false, - numSamples, - image_scan, - rolling_average_constant, - percent_of_samples_needed, - valid_samples); - - double leftConfidence = getConfidence(image_scan, - percent_of_image_sampled, - percent_of_samples_needed, - valid_samples); - - double rightToLeftAngle = getAngleOfLine(true, - numSamples, - image_scan, - rolling_average_constant, - percent_of_samples_needed, - valid_samples); - - double rightConfidence = getConfidence(image_scan, - percent_of_image_sampled, - percent_of_samples_needed, - valid_samples); - - int desiredAngle; - - if (rightConfidence > leftConfidence) { - desiredAngle = rightToLeftAngle; - confidence_value = rightConfidence; - } else { - desiredAngle = leftToRightAngle; - confidence_value = leftConfidence; - } - - if (fabs(desiredAngle) <= move_away_threshold) - desiredAngle = moveAwayFromLine(image_scan); - - // If there is a perpendicular line in front of the robot, stop. - if (isPerpendicular(image_scan)) confidence_value = 0; - - double num_of_white_needed = - image_scan->height * image_scan->width * percent_of_white_needed; - if (whiteCount < num_of_white_needed) - desiredAngle = moveAwayFromLine(image_scan); - - return desiredAngle; -} - -int VisionDecision::getAngleOfLine( -bool rightSide, -double numSamples, -const sensor_msgs::Image::ConstPtr& image_scan, -double rolling_average_constant, -double percent_of_samples_needed, -double& validSamples) { - // initialization of local variables. - double imageHeight = image_scan->height; - int incrementer; - int startingPos = 0; - validSamples = 0; - - // Assign garbage values - int bottomRow = -1; - double x1 = -1; - - // Initialize how and where to parse. - incrementer = - initializeIncrementerPosition(rightSide, image_scan, &startingPos); - - // starts parsing from the right and finds where the lowest white line - // begins. - for (int i = imageHeight - 1; i > 0; i--) { - int startPixel = - getEdgePixel(startingPos, incrementer, i, image_scan, true); - if (startPixel != -1 && bottomRow == -1) { - // Each slope will be compared to the bottom point of the lowest - // white line - bottomRow = i; - x1 = getMiddle(startingPos, bottomRow, rightSide, image_scan); - break; - } - } - - double currentAngle = 0; - - // Finds slopes (corresponding to number of samples given) then returns the - // sum of all slopes - // also counts how many of them are valid. - for (double division = 1; - (division < numSamples) && (bottomRow - division > 0); - division++) { - double yCompared = bottomRow - division; - double xCompared = - getMiddle(startingPos, (int) yCompared, rightSide, image_scan); - - double foundAngle; - double foundSlope; - - if (xCompared != -1 && x1 != -1) { - // slope is valid, find the angle compared the the positive y-axis. - foundSlope = -(xCompared - x1) / (yCompared - bottomRow); - foundAngle = atan(foundSlope); - // increment amount of valid samples - validSamples++; - - if (DEBUG) - printf( - "curAngle: %f, x1: %f, bottomRow: %d, xCompared: %f, " - "yCompared: %f, " - "Found Angle: %f, Valid: %f \n", - currentAngle * 180 / M_PI, - x1, - bottomRow, - xCompared, - yCompared, - foundAngle * 180 / M_PI, - validSamples); - - // Update the current angle if the change is not too sudden. - // (Found angle does not exceed a right angle turn of the current - // angle) - if (fabs(currentAngle - foundAngle) * 180 / M_PI < 90) - currentAngle = rolling_average_constant * foundAngle + - (1 - rolling_average_constant) * currentAngle; - else - validSamples--; - } - } - - if (x1 == -1) validSamples = 0; - - return (int) (currentAngle * 180.0 / M_PI); // returns the angle in degrees -} - -double VisionDecision::getDesiredAngularSpeed(double desiredAngle) { - // the higher the desired angle, the higher the angular speed - if (desiredAngle == STOP_SIGNAL_ANGLE) return 0; - - double mappedSpeed = pow(desiredAngle, 2.0); - - mappedSpeed = mappedSpeed / 10000.0; - - if (desiredAngle < 0) mappedSpeed = mappedSpeed * (-1.0); - - return mappedSpeed; -} - -double VisionDecision::getDesiredLinearSpeed(double desiredAngle) { - double speedToMap = abs((int) desiredAngle); - - if (desiredAngle == STOP_SIGNAL_ANGLE) return 0; - - // the higher the desired angle the lower the linear speed. - return 1 - mapRange(speedToMap, 0, 90, 0, 1); -} - -/* Helper functions for functions that determine robot movement. */ - -int VisionDecision::getMiddle(int startingPos, - int row, - bool rightSide, - const sensor_msgs::Image::ConstPtr& image_scan) { - int incrementer; - int startPixel, endPixel; - - // Depending on chosen side, determine where to start - // and how to iterate. - if (rightSide) - incrementer = -1; - else - incrementer = 1; - - // Find first pixel of the white line in a certain row. - startPixel = - VisionDecision::getEdgePixel(startingPos, incrementer, row, image_scan, 1); - - // Find last pixel of the white line in a certain row. - endPixel = - VisionDecision::getEdgePixel(startPixel, incrementer, row, image_scan, 0); - - // Return average of the two pixels. - return (startPixel + endPixel) / 2; -} - -int VisionDecision::getEdgePixel(int startingPos, - int incrementer, - int row, - const sensor_msgs::Image::ConstPtr& image_scan, - bool isStartPixel) { - // Initialization of local variables - int column = startingPos; - int whiteVerificationCount = 0; - int blackVerificationCount = 0; - - // Initialize these to be garbage values - int toBeChecked = -1; - - // Parse through image horizontally to find a valid pixel - while (column < image_scan->width && column >= 0) { - // If white pixel found start verifying if proper start. - if ((image_scan->data[row * image_scan->width + column] != 0 && - isStartPixel) || - (image_scan->data[row * image_scan->width + column] == 0 && - !isStartPixel)) { - blackVerificationCount = 0; - // This pixel is what we are checking - if (toBeChecked == -1) { toBeChecked = column; } - - // Determine whether toBeChecked is noise - whiteVerificationCount++; - if (whiteVerificationCount == NOISE_MAX) return toBeChecked; - } else { - blackVerificationCount++; - if (blackVerificationCount >= NOISE_MAX) { - whiteVerificationCount = - 0; // Reset verification if black pixel. - toBeChecked = -1; - } - } - // Go to next element - column += incrementer; - } - - // No value found return an error. - return -1; -} - -int VisionDecision::initializeIncrementerPosition( -bool rightSide, -const sensor_msgs::Image::ConstPtr& image_scan, -int* startingPos) { - // Decides how to parse depending on if rightSide is true or false. - if (rightSide) { - // starts at right side then increments to the left - *startingPos = image_scan->width - 1; - return -1; - } else { - // starts at left side then increments to th right - *startingPos = 0; - return 1; - } -} - -bool VisionDecision::isPerpendicular( -const sensor_msgs::Image::ConstPtr& image_scan) { - int leftSidePixel, rightSidePixel = -1; - int i, j; - - for (i = 0; i < image_scan->width; i++) { - leftSidePixel = getVerticalEdgePixel(image_scan, i); - if (leftSidePixel != -1) break; - } - - for (j = image_scan->width - 1; j > 0; j--) { - rightSidePixel = getVerticalEdgePixel(image_scan, j); - if (rightSidePixel != -1) break; - } - - if (rightSidePixel == 0 && leftSidePixel == 0) return 0; - - return (fabs(rightSidePixel - leftSidePixel) < image_scan->height / 10 && - fabs(j - i) > image_scan->width / 10); -} - -int VisionDecision::getVerticalEdgePixel( -const sensor_msgs::Image::ConstPtr& image_scan, int column) { - int row; - int whiteVerificationCount = 0; - int blackVerificationCount = 0; - - // Initialize these to be garbage values - int toBeChecked = -1; - - // Parse vertically to find a valid starting white pixel. - for (row = image_scan->height - 1; row >= 0; row--) { - // If white pixel found start verifying if proper start. - if ((image_scan->data[row * image_scan->width + column] != 0)) { - blackVerificationCount = 0; - // This pixel is what we are checking - if (toBeChecked == -1) toBeChecked = row; - - // Determine whether toBeChecked is noise - whiteVerificationCount++; - if (whiteVerificationCount == NOISE_MAX) return toBeChecked; - } else { - blackVerificationCount++; - if (blackVerificationCount == NOISE_MAX) { - whiteVerificationCount = - 0; // Reset verification if black pixel. - toBeChecked = -1; - } - } - } -} - -int VisionDecision::getLeftToRightPixelRatio( -const sensor_msgs::Image::ConstPtr& image_scan) { - int leftCount = 0; - int rightCount = 0; - - for (int row = 0; row < image_scan->height; row++) { - for (int column = 0; column < image_scan->width; column++) { - if (image_scan->data[row * image_scan->width + column] != 0 && - column <= image_scan->width / 2) { - leftCount++; - } - if (image_scan->data[row * image_scan->width + column] != 0 && - column > image_scan->width / 2) - rightCount++; - } - } - - return rightCount - leftCount; -} - -int VisionDecision::moveAwayFromLine( -const sensor_msgs::Image::ConstPtr& image_scan) { - if (getLeftToRightPixelRatio(image_scan) < 0) - return 45; - else - return -45; -} - -double VisionDecision::mapRange( -double x, double inMin, double inMax, double outMin, double outMax) { - return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; -} - -double -VisionDecision::getConfidence(const sensor_msgs::Image::ConstPtr& image_scan, - double percent_of_image_sampled, - double percent_of_samples_needed, - double valid_samples) { - double samples_needed = image_scan->height * percent_of_samples_needed; - - if (valid_samples > samples_needed) valid_samples = samples_needed; - return mapRange(valid_samples, 0, samples_needed, 0, 100); -} \ No newline at end of file diff --git a/src/decision_igvc/src/final_decision.cpp b/src/decision_igvc/src/final_decision.cpp deleted file mode 100644 index f5a07f2db..000000000 --- a/src/decision_igvc/src/final_decision.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Created By: YOUR NAME HERE - * Created On: September 22, 2016 - * Description: The Decision Node for GPS, takes in a point relative to - * the robots location and heading and broadcasts a - * recommended twist message - */ - -#include - -int main(int argc, char** argv) { - // Setup your ROS node - std::string node_name = "final_decision"; - - // Create an instance of your class - FinalDecision gps_decision(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/decision_igvc/src/gps_decision.cpp b/src/decision_igvc/src/gps_decision.cpp deleted file mode 100644 index 2f5516f8e..000000000 --- a/src/decision_igvc/src/gps_decision.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: May 18, 2017 - * Description: The Decision Node for GPS, takes in a point relative to - * the robots location and heading and broadcasts a - * recommended twist message. Revised to use TF's - */ - -#include - -int main(int argc, char** argv) { - // Setup your ROS node - std::string node_name = "gps_decision"; - - // Create an instance of your class - GpsDecision gps_decision(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/decision_igvc/src/gps_manager.cpp b/src/decision_igvc/src/gps_manager.cpp deleted file mode 100644 index bdecc5c23..000000000 --- a/src/decision_igvc/src/gps_manager.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: May 17, 2016 - * Description: Manages the GPS waypoints we are given - * (publishing the next one once we've arrived - * at the current one) - */ - -#include - -int main(int argc, char** argv) { - // Setup your ROS node - std::string node_name = "gps_manager"; - - // Create an instance of your class - GpsManager gps_manager(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/decision_igvc/src/lidar_decision.cpp b/src/decision_igvc/src/lidar_decision.cpp deleted file mode 100644 index 6f7b48eaf..000000000 --- a/src/decision_igvc/src/lidar_decision.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: September 22, 2016 - * Description: The Lidar decision node, takes in a raw Lidar scan - * and broadcasts a recommended Twist message - */ - -#include - -int main(int argc, char** argv) { - // Setup your ROS node - std::string node_name = "lidar_decision"; - - // Create an instance of your class - LidarDecision lidar_decision(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/decision_igvc/src/vision_decision.cpp b/src/decision_igvc/src/vision_decision.cpp deleted file mode 100644 index 917dd82e7..000000000 --- a/src/decision_igvc/src/vision_decision.cpp +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: September 22, 2016 - * Description: The vision decision node, takes in an image from the robot's - * camera and produces a recommended twist message - */ - -#include - -int main(int argc, char** argv) { - // Setup your ROS node - std::string node_name = "vision_decision"; - - // ros::init(argc, argv, node_name); - - // Create an instance of your class - VisionDecision vision_decision(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/decision_igvc/test/final-decision-test.cpp b/src/decision_igvc/test/final-decision-test.cpp deleted file mode 100644 index b4f1b4727..000000000 --- a/src/decision_igvc/test/final-decision-test.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July 22, 2016 - * Description: Tests for Final Decision - */ - -#include -#include - -TEST(FinalDecision, arbitrator) { - geometry_msgs::Twist lidar1; - geometry_msgs::Twist lidar2; - geometry_msgs::Twist lidar3; - geometry_msgs::Twist vision1; - geometry_msgs::Twist vision2; - geometry_msgs::Twist vision3; - geometry_msgs::Twist gps1; - geometry_msgs::Twist gps2; - geometry_msgs::Twist gps3; - - lidar1.angular.z = 5; - lidar2.angular.z = 0; - lidar3.angular.z = -5; - vision1.angular.z = 5; - vision2.angular.z = 0; - vision3.angular.z = -5; - gps1.angular.z = 5; - gps2.angular.z = 0; - gps3.angular.z = -5; - - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision1, gps1).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision1, gps2).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision1, gps3).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision2, gps1).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision2, gps2).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision2, gps3).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision3, gps1).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision3, gps2).angular.z); - EXPECT_EQ(lidar1.angular.z, - FinalDecision::arbitrator(lidar1, vision3, gps3).angular.z); - EXPECT_EQ(vision1.angular.z, - FinalDecision::arbitrator(lidar2, vision1, gps1).angular.z); - EXPECT_EQ(vision1.angular.z, - FinalDecision::arbitrator(lidar2, vision1, gps2).angular.z); - EXPECT_EQ(vision1.angular.z, - FinalDecision::arbitrator(lidar2, vision1, gps3).angular.z); - EXPECT_EQ(gps1.angular.z, - FinalDecision::arbitrator(lidar2, vision2, gps1).angular.z); - EXPECT_EQ(gps2.angular.z, - FinalDecision::arbitrator(lidar2, vision2, gps2).angular.z); - EXPECT_EQ(gps3.angular.z, - FinalDecision::arbitrator(lidar2, vision2, gps3).angular.z); - EXPECT_EQ(vision3.angular.z, - FinalDecision::arbitrator(lidar2, vision3, gps1).angular.z); - EXPECT_EQ(vision3.angular.z, - FinalDecision::arbitrator(lidar2, vision3, gps2).angular.z); - EXPECT_EQ(vision3.angular.z, - FinalDecision::arbitrator(lidar2, vision3, gps3).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision1, gps1).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision1, gps2).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision1, gps3).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision2, gps1).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision2, gps2).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision2, gps3).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision3, gps1).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision3, gps2).angular.z); - EXPECT_EQ(lidar3.angular.z, - FinalDecision::arbitrator(lidar3, vision3, gps3).angular.z); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/decision_igvc/test/gps-manager-test.cpp b/src/decision_igvc/test/gps-manager-test.cpp deleted file mode 100644 index b88f16eca..000000000 --- a/src/decision_igvc/test/gps-manager-test.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: October 21, 2016 - * Description: Tests for GpsManager - * Notes: - * Some really great sites for generating test data: - * REMEMBER YOU'RE USING A SPHERE MODEL FOR THE EARTH - * http://www.geomidpoint.com/destination/ - * http://www.movable-type.co.uk/scripts/latlong.html - */ - -#include -#include - -/** - * Converts a vector of waypoints to pairs for testing with the form (lat,lon) - * - * @param waypoints vector of waypoints - * - * @return the waypoints as a vector of doubles - */ -std::vector> -pairsFromWaypointVector(std::vector waypoints) { - std::vector> new_waypoints; - for (int i = 0; i < waypoints.size(); i++) { - std::pair new_waypoint(waypoints[i].lat, - waypoints[i].lon); - new_waypoints.push_back(new_waypoint); - } - return new_waypoints; -} - -TEST(GpsManager, parseWaypoints) { - std::vector> one_waypoint_result = { - std::pair(9.87, 10.98)}; - std::vector one_waypoint_input = {9.87, 10.98}; - - std::vector> multi_waypoint_result = { - std::pair(9.87, 10.98), - std::pair(98.123, 97.122), - std::pair(87.12, 12.12)}; - std::vector multi_waypoint_input = { - 9.87, 10.98, 98.123, 97.122, 87.12, 12.12}; - - EXPECT_EQ( - one_waypoint_result, - pairsFromWaypointVector(GpsManager::parseWaypoints(one_waypoint_input))); - EXPECT_EQ( - multi_waypoint_result, - pairsFromWaypointVector(GpsManager::parseWaypoints(multi_waypoint_input))); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/decision_igvc/test/gps-mover-test.cpp b/src/decision_igvc/test/gps-mover-test.cpp deleted file mode 100644 index 685d74f96..000000000 --- a/src/decision_igvc/test/gps-mover-test.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July 22, 2016 - * Description: Tests for the GpsMover class - */ - -#include -#include - -/** - * Creates a geometry_msgs::Point with given x and y coordinates - * - * @param x the x coordinate of the point - * @param y the y coordinate of the point - * @return a point with given x and y coordinates - */ -geometry_msgs::Point createPoint(double x, double y) { - geometry_msgs::Point p; - p.x = x; - p.y = y; - p.z = 0; - return p; -} - -class GpsMoverTest : public testing::Test { - protected: - virtual void SetUp() {} - - GpsMover mover1; -}; - -// Test with robot facing straight ahead, with the waypoint to -// the right and forwards of the robot -TEST_F(GpsMoverTest, createTwistMessage_heading0) { - auto current_location = createPoint(0, 0); - auto waypoint = createPoint(2, 2); - - auto twist = mover1.createTwistMessage(current_location, 0, waypoint); - - // We should be trying to turn left - EXPECT_GE(twist.angular.z, 0); -} - -// Test of the robot facing "backwards" relative to initial heading, -// with the waypoint (relative to the robot) behind and to the left -TEST_F(GpsMoverTest, createTwistMessage_headingPiObstacleBehindAndLeft) { - auto current_location = createPoint(0, 0); - auto waypoint = createPoint(2, -2); - - auto twist = mover1.createTwistMessage(current_location, M_PI, waypoint); - - // We should be trying to turn left - EXPECT_GE(twist.angular.z, 0); -} - -// Test of the robot facing "backwards" relative to initial heading, -// with the waypoint behind and to the right (relative to the robot) -TEST_F(GpsMoverTest, createTwistMessage_headingPiObstacleBehindAndRight) { - auto current_location = createPoint(0, 0); - auto waypoint = createPoint(2, 2); - - auto twist = mover1.createTwistMessage(current_location, M_PI, waypoint); - - // We should be trying to turn right - EXPECT_LE(twist.angular.z, 0); -} - -// Test with the robot facing straight forward (relative to initial heading) -// with the waypoint far behind and to the left -TEST_F(GpsMoverTest, createTwistMessage_headingLeftWaypointBehindAndLeft) { - auto current_location = createPoint(18.9, -6.96); - auto waypoint = createPoint(9.28, 2.47); - - auto twist = mover1.createTwistMessage(current_location, -0.5876, waypoint); - - // We should be trying to turn left - EXPECT_GE(twist.angular.z, 0); -} - -// Test with the robot facing straight forward (relative to initial heading) -// with the waypoint far behind and to the right -TEST_F(GpsMoverTest, createTwistMessage_headingLeftWaypointBehindAndRight) { - auto current_location = createPoint(18.9, 0); - auto waypoint = createPoint(9.28, -8.47); - - auto twist = mover1.createTwistMessage(current_location, -0.5876, waypoint); - - // We should be trying to turn right - EXPECT_LE(twist.angular.z, 0); -} - -// Test of angleBetweenPoints with the destination point -// ahead and to the left of the start point -TEST_F(GpsMoverTest, angleBetweenPoints_FrontLeft) { - auto startPoint = createPoint(0, 0); - auto endPoint = createPoint(1, 0.5); - double angle = mover1.angleBetweenPoints(startPoint, endPoint); - EXPECT_DOUBLE_EQ(atan(0.5), angle); -} - -// Test of angleBetweenPoints with the destination point -// behind and to the left of the start point -TEST_F(GpsMoverTest, angleBetweenPoints_BackLeft) { - auto startPoint = createPoint(0, 0); - auto endPoint = createPoint(-1, 0.5); - double angle = mover1.angleBetweenPoints(startPoint, endPoint); - EXPECT_DOUBLE_EQ(M_PI - atan(0.5), angle); -} - -// Test of angleBetweenPoints with the destination point -// ahead and to the right of the start point -TEST_F(GpsMoverTest, angleBetweenPoints_FrontRight) { - auto startPoint = createPoint(0, 0); - auto endPoint = createPoint(1, -0.5); - double angle = mover1.angleBetweenPoints(startPoint, endPoint); - EXPECT_DOUBLE_EQ(-atan(0.5), angle); -} - -// Test of angleBetweenPoints with the destination point -// behind and to the right of the start point -TEST_F(GpsMoverTest, angleBetweenPoints_BackRight) { - auto startPoint = createPoint(0, 0); - auto endPoint = createPoint(-1, -0.5); - double angle = mover1.angleBetweenPoints(startPoint, endPoint); - EXPECT_DOUBLE_EQ(-M_PI + atan(0.5), angle); -} - -// Simple test where the angle are already separated by an acute angle if -// just subtracted -TEST_F(GpsMoverTest, minAngularChangeTest_acuteAngle) { - double angle = mover1.minAngularChange(-M_PI, -M_PI / 4); - EXPECT_DOUBLE_EQ(3.0 / 4.0 * M_PI, angle); -} - -// Test where the angle returned should be negative -TEST_F(GpsMoverTest, minAngularChangeTest_negativeReturnAngle) { - double angle = mover1.minAngularChange(M_PI, M_PI / 4); - EXPECT_DOUBLE_EQ(-3.0 / 4.0 * M_PI, angle); -} - -// Test where angles are separated by an obtuse angle (if just subtracted) -TEST_F(GpsMoverTest, minAngularChangeTest_obtuseAngle) { - double should_be_acute = mover1.minAngularChange(M_PI, -M_PI / 4); - EXPECT_DOUBLE_EQ(3.0 / 4.0 * M_PI, should_be_acute); -} - -// Test where angle are separated by a negative obtuse angle (if just -// subtracted) -TEST_F(GpsMoverTest, minAngularChangeTest_negativeObtuseAngle) { - double should_be_acute = mover1.minAngularChange(-M_PI, M_PI / 4); - EXPECT_DOUBLE_EQ(-3.0 / 4.0 * M_PI, should_be_acute); -} - -// Test out our magic function...... -TEST_F(GpsMoverTest, magicFunctionTest_basic) { - EXPECT_DOUBLE_EQ((1 / (double) 10 + sqrt((double) 5)) / 2, - mover1.magicFunction(10, 5, 1, 1)); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/decision_igvc/test/imageTests/testAvoidCone.jpg b/src/decision_igvc/test/imageTests/testAvoidCone.jpg deleted file mode 100644 index 1131b3490..000000000 Binary files a/src/decision_igvc/test/imageTests/testAvoidCone.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testAvoidConeRight.jpg b/src/decision_igvc/test/imageTests/testAvoidConeRight.jpg deleted file mode 100644 index 673439765..000000000 Binary files a/src/decision_igvc/test/imageTests/testAvoidConeRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testBigCornerTurnLeft.jpg b/src/decision_igvc/test/imageTests/testBigCornerTurnLeft.jpg deleted file mode 100644 index cd06debc0..000000000 Binary files a/src/decision_igvc/test/imageTests/testBigCornerTurnLeft.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testBigCornerTurnRight.jpg b/src/decision_igvc/test/imageTests/testBigCornerTurnRight.jpg deleted file mode 100644 index b3af3f2ce..000000000 Binary files a/src/decision_igvc/test/imageTests/testBigCornerTurnRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testConeTurnLeft.jpg b/src/decision_igvc/test/imageTests/testConeTurnLeft.jpg deleted file mode 100644 index c431aa5a7..000000000 Binary files a/src/decision_igvc/test/imageTests/testConeTurnLeft.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testConeTurnRight.jpg b/src/decision_igvc/test/imageTests/testConeTurnRight.jpg deleted file mode 100644 index 56876b1b7..000000000 Binary files a/src/decision_igvc/test/imageTests/testConeTurnRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testCornerTurnLeft.jpg b/src/decision_igvc/test/imageTests/testCornerTurnLeft.jpg deleted file mode 100644 index a46fc5046..000000000 Binary files a/src/decision_igvc/test/imageTests/testCornerTurnLeft.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testCornerTurnRight.jpg b/src/decision_igvc/test/imageTests/testCornerTurnRight.jpg deleted file mode 100644 index cad6d794b..000000000 Binary files a/src/decision_igvc/test/imageTests/testCornerTurnRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testCurvedLine.jpg b/src/decision_igvc/test/imageTests/testCurvedLine.jpg deleted file mode 100644 index 8fff4312c..000000000 Binary files a/src/decision_igvc/test/imageTests/testCurvedLine.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testElevatedLeftLine.jpg b/src/decision_igvc/test/imageTests/testElevatedLeftLine.jpg deleted file mode 100644 index 5d3ffe695..000000000 Binary files a/src/decision_igvc/test/imageTests/testElevatedLeftLine.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testElevatedRightLine.jpg b/src/decision_igvc/test/imageTests/testElevatedRightLine.jpg deleted file mode 100644 index 50cc89863..000000000 Binary files a/src/decision_igvc/test/imageTests/testElevatedRightLine.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testLeftImage.jpg b/src/decision_igvc/test/imageTests/testLeftImage.jpg deleted file mode 100644 index 11ef25cba..000000000 Binary files a/src/decision_igvc/test/imageTests/testLeftImage.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testMultipleLinesLeft.jpg b/src/decision_igvc/test/imageTests/testMultipleLinesLeft.jpg deleted file mode 100644 index e5ac64302..000000000 Binary files a/src/decision_igvc/test/imageTests/testMultipleLinesLeft.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testMultipleLinesRight.jpg b/src/decision_igvc/test/imageTests/testMultipleLinesRight.jpg deleted file mode 100644 index aaf0399f8..000000000 Binary files a/src/decision_igvc/test/imageTests/testMultipleLinesRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testNoisyRightImage.jpg b/src/decision_igvc/test/imageTests/testNoisyRightImage.jpg deleted file mode 100644 index 35538d1c2..000000000 Binary files a/src/decision_igvc/test/imageTests/testNoisyRightImage.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testOppositeConeTurnRight.jpg b/src/decision_igvc/test/imageTests/testOppositeConeTurnRight.jpg deleted file mode 100644 index 829c61720..000000000 Binary files a/src/decision_igvc/test/imageTests/testOppositeConeTurnRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testPerpendicular.jpg b/src/decision_igvc/test/imageTests/testPerpendicular.jpg deleted file mode 100644 index f4666d4ba..000000000 Binary files a/src/decision_igvc/test/imageTests/testPerpendicular.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testRightAngleTurnLeft.jpg b/src/decision_igvc/test/imageTests/testRightAngleTurnLeft.jpg deleted file mode 100644 index fc6df5b36..000000000 Binary files a/src/decision_igvc/test/imageTests/testRightAngleTurnLeft.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testRightAngleTurnRight.jpg b/src/decision_igvc/test/imageTests/testRightAngleTurnRight.jpg deleted file mode 100644 index 9346a8f9e..000000000 Binary files a/src/decision_igvc/test/imageTests/testRightAngleTurnRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testSmallRight.jpg b/src/decision_igvc/test/imageTests/testSmallRight.jpg deleted file mode 100644 index 4c95d2caa..000000000 Binary files a/src/decision_igvc/test/imageTests/testSmallRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testSplitLines.jpg b/src/decision_igvc/test/imageTests/testSplitLines.jpg deleted file mode 100644 index 16733f94f..000000000 Binary files a/src/decision_igvc/test/imageTests/testSplitLines.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testSplitLinesRight.jpg b/src/decision_igvc/test/imageTests/testSplitLinesRight.jpg deleted file mode 100644 index 68da0601c..000000000 Binary files a/src/decision_igvc/test/imageTests/testSplitLinesRight.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testStraightButLineNearEdge.jpg b/src/decision_igvc/test/imageTests/testStraightButLineNearEdge.jpg deleted file mode 100644 index 70bd8dff5..000000000 Binary files a/src/decision_igvc/test/imageTests/testStraightButLineNearEdge.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testStraightImage.jpg b/src/decision_igvc/test/imageTests/testStraightImage.jpg deleted file mode 100644 index 741352394..000000000 Binary files a/src/decision_igvc/test/imageTests/testStraightImage.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testStraightOnRightSide.jpg b/src/decision_igvc/test/imageTests/testStraightOnRightSide.jpg deleted file mode 100644 index c34a4e8ae..000000000 Binary files a/src/decision_igvc/test/imageTests/testStraightOnRightSide.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testVeryNoisyLeftImage.jpg b/src/decision_igvc/test/imageTests/testVeryNoisyLeftImage.jpg deleted file mode 100644 index b36679529..000000000 Binary files a/src/decision_igvc/test/imageTests/testVeryNoisyLeftImage.jpg and /dev/null differ diff --git a/src/decision_igvc/test/imageTests/testVeryNoisyStraightImage.jpg b/src/decision_igvc/test/imageTests/testVeryNoisyStraightImage.jpg deleted file mode 100644 index f82d466e6..000000000 Binary files a/src/decision_igvc/test/imageTests/testVeryNoisyStraightImage.jpg and /dev/null differ diff --git a/src/decision_igvc/test/lidar-decision-test.cpp b/src/decision_igvc/test/lidar-decision-test.cpp deleted file mode 100644 index 79b0fac48..000000000 --- a/src/decision_igvc/test/lidar-decision-test.cpp +++ /dev/null @@ -1,187 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: September 22, 2016 - * Description: Tests for LidarDecision and LidarObstacle - */ - -#include -#include - -class LidarDecisionTest : public testing::Test { - protected: - virtual void SetUp() { - // Create our fake lidar scan1 - ulong num_rays = 300; - scan1.angle_min = 0; - scan1.angle_max = (float) M_PI; - scan1.angle_increment = (scan1.angle_max - scan1.angle_min) / num_rays; - // Set all the ranges to 0 initially - scan1.ranges = std::vector(num_rays, 0); - scan1.range_min = 20; - scan1.range_max = 40; - // Add the obstacles - std::fill( - scan1.ranges.begin(), scan1.ranges.begin() + 10, 36.123); // obstacle 1 - std::fill(scan1.ranges.begin() + 10, - scan1.ranges.begin() + 20, - 38.123); // obstacle 2 - std::fill(scan1.ranges.begin() + 100, - scan1.ranges.begin() + 105, - 24.123); // obstacle 3 - std::fill(scan1.ranges.begin() + 105, - scan1.ranges.begin() + 110, - 26.123); // obstacle 4 - std::fill(scan1.ranges.begin() + 125, - scan1.ranges.begin() + 130, - 25.123); // obstacle 5 - std::fill(scan1.ranges.begin() + 200, - scan1.ranges.begin() + 240, - 32); // obstacle 6 - // Make some ranges outside the min and max of the scan - std::fill( - scan1.ranges.begin() + 110, scan1.ranges.begin() + 125, 45.123); - std::fill(scan1.ranges.begin() + 50, scan1.ranges.begin() + 70, 4.123); - - // Create our fake lidar scan2 - scan2.angle_min = 0; - scan2.angle_max = (float) M_PI; - scan2.angle_increment = (scan2.angle_max - scan2.angle_min) / num_rays; - // Set all the ranges to 0 initially - scan2.ranges = std::vector(num_rays, 0); - scan2.range_min = 20; - scan2.range_max = 40; - // Add the obstacles - std::fill( - scan2.ranges.begin(), scan2.ranges.begin() + 10, 37.123); // obstacle 1 - std::fill(scan2.ranges.begin() + 15, - scan2.ranges.begin() + 20, - 37.123); // obstacle 2 - std::fill(scan2.ranges.begin() + 100, - scan2.ranges.begin() + 105, - 24.123); // obstacle 3 - std::fill(scan2.ranges.begin() + 105, - scan2.ranges.begin() + 110, - 26.123); // obstacle 4 - std::fill(scan2.ranges.begin() + 125, - scan2.ranges.begin() + 130, - 25.123); // obstacle 5 - std::fill(scan2.ranges.begin() + 200, - scan2.ranges.begin() + 240, - 32); // obstacle 6 - // Make some ranges outside the min and max of the scan - std::fill( - scan2.ranges.begin() + 110, scan2.ranges.begin() + 125, 45.123); - std::fill(scan2.ranges.begin() + 50, scan2.ranges.begin() + 70, 4.123); - - // Create some fake sets of obstacles - // Obstacles sorted in order of ascending angle - sorted_obstacles = { - LidarObstacle(0.0, 10), - LidarObstacle(0.1, 10), - LidarObstacle(0.2, 10), - LidarObstacle(0.5, 10), - LidarObstacle(1.1, 10), - LidarObstacle(1.2, 10), - LidarObstacle(1.6, 10), - }; - // Obstacles not sorted at all (in terms of angle) - unsorted_obstacles = { - LidarObstacle(0.1, 10), - LidarObstacle(0.5, 10), - LidarObstacle(1.6, 10), - LidarObstacle(0.2, 10), - LidarObstacle(0.0, 10), - LidarObstacle(1.2, 10), - LidarObstacle(1.1, 10), - }; - } - - sensor_msgs::LaserScan scan1, scan2; - std::vector sorted_obstacles, unsorted_obstacles; -}; - -TEST_F(LidarDecisionTest, mergeSimilarObstaclesPreSortedTest) { - LidarDecision::mergeSimilarObstacles(sorted_obstacles, 0.11, 1.0); - EXPECT_EQ(4, sorted_obstacles.size()); -} - -TEST_F(LidarDecisionTest, mergeSimilarObstaclesUnSortedTest) { - // Create some test obstacles in random order (in terms of angle) - LidarDecision::mergeSimilarObstacles(unsorted_obstacles, 0.11, 1.0); - EXPECT_EQ(4, unsorted_obstacles.size()); -} - -// test with angles close enough but not distance -TEST_F(LidarDecisionTest, findObstaclesDistanceTest) { - std::vector found_obstacles = - LidarDecision::findObstacles(scan1, 0.02, 1.0); - // The distance separates obstacle1 and obstacle2 making the size to be 6 - // instead of 5 - EXPECT_EQ(6, found_obstacles.size()); - // Check that all obstacles are at acceptable ranges - for (LidarObstacle obstacle : found_obstacles) { - EXPECT_GE(obstacle.getAvgDistance(), 20); - EXPECT_LE(obstacle.getAvgDistance(), 40); - } -} - -// test with distance close enough but not angles -TEST_F(LidarDecisionTest, findObstaclesAngleTest) { - std::vector found_obstacles = - LidarDecision::findObstacles(scan2, 0.02, 1.0); - // The angle separates obstacle1 and obstacle2 making the size to be 6 - // instead of 5 - EXPECT_EQ(6, found_obstacles.size()); - // Check that all obstacles are at acceptable ranges - for (LidarObstacle obstacle : found_obstacles) { - EXPECT_GE(obstacle.getAvgDistance(), 20); - EXPECT_LE(obstacle.getAvgDistance(), 40); - } -} - -TEST_F(LidarDecisionTest, angular_twist_message_from_obstacleTest) { - // Check that the correct twist.angular.z values are calculated - // based on test obstacle's parameters - EXPECT_NEAR(1.0606602, - LidarDecision::twist_message_from_obstacle( - LidarObstacle(-M_PI / 4, 3.0), 4.0, M_PI / 2, 1.5, 1.5) - .angular.z, - 0.000001); - EXPECT_NEAR(1.5, - LidarDecision::twist_message_from_obstacle( - LidarObstacle(0.0, 4.7), 5.0, M_PI / 2, 1.5, 1.5) - .angular.z, - 0.000001); - EXPECT_NEAR(-1.5, - LidarDecision::twist_message_from_obstacle( - LidarObstacle(M_PI / 2, 0.9), 2.0, M_PI / 2, 1.5, 1.5) - .angular.z, - 0.000001); -} - -TEST_F(LidarDecisionTest, linear_twist_message_from_obstacleTest) { - // Check that the correct twist.linear.x values are calculated - // based on test obstacle's parameters - EXPECT_NEAR(0.0, - LidarDecision::twist_message_from_obstacle( - LidarObstacle(3 * M_PI / 4, 2.1), 3.0, M_PI / 2, 1.5, 1.5) - .linear.x, - 0.000001); - EXPECT_NEAR(1.7320508, - LidarDecision::twist_message_from_obstacle( - LidarObstacle(-M_PI / 3, 3.0), 4.0, M_PI / 2, 1.5, 1.5) - .linear.x, - 0.000001); - EXPECT_NEAR(0.6324555, - LidarDecision::twist_message_from_obstacle( - LidarObstacle(0.0, 0.4), 5.0, M_PI / 2, 1.5, 1.5) - .linear.x, - 0.000001); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); - return 0; -} diff --git a/src/decision_igvc/test/lidar-obstacle-test.cpp b/src/decision_igvc/test/lidar-obstacle-test.cpp deleted file mode 100644 index 65bad4ca4..000000000 --- a/src/decision_igvc/test/lidar-obstacle-test.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: September 22, 2016 - * Description: Tests for LidarDecision and LidarObstacle - */ - -// Snowbots Includes -#include - -// Library Includes -#include - -// STD Includes -#include - -class LidarObstacleTest : public testing::Test { - protected: - virtual void SetUp() { - readings1 = {{4, 44}, {3, 99}, {6, 2}}; - readings2 = {{-1, 2}, {10, 4}, {-15, 6}}; - - obstacle1 = LidarObstacle(0.1, 10); - obstacle2 = LidarObstacle(0.2, 20); - obstacle3 = LidarObstacle(0.15, 15); - obstacle4 = LidarObstacle(readings1); - obstacle5 = LidarObstacle(readings2); - } - - std::vector readings1, readings2; - LidarObstacle obstacle1, obstacle2, obstacle3, obstacle4, obstacle5; -}; - -TEST_F(LidarObstacleTest, ConstructorTest1) { - auto readings = obstacle1.getAllLaserReadings(); - EXPECT_EQ(1, readings.size()); - EXPECT_EQ(10, readings[0].range); - EXPECT_NEAR(0.1, readings[0].angle, 0.000001); -} - -TEST_F(LidarObstacleTest, ConstructorTest2) { - auto readings = obstacle4.getAllLaserReadings(); - // Check that the correct number of readings were input - EXPECT_EQ(3, readings.size()); - // Check that they are in the correct order - // (sorted in ascending order by angle) - EXPECT_EQ(3, readings[0].angle); - EXPECT_EQ(4, readings[1].angle); - EXPECT_EQ(6, readings[2].angle); -} - -TEST_F(LidarObstacleTest, getAvgAngleTest) { - EXPECT_NEAR(0.1, obstacle1.getAvgAngle(), 0.00001); - EXPECT_NEAR(4.333333333, obstacle4.getAvgAngle(), 0.00001); - EXPECT_NEAR(-2, obstacle5.getAvgAngle(), 0.00001); -} - -TEST_F(LidarObstacleTest, mergeInReadingsTest) { - obstacle1.mergeInLidarObstacle(obstacle2); - obstacle1.mergeInLidarObstacle(obstacle3); - auto readings = obstacle1.getAllLaserReadings(); - // Check that the merge obstacle has the correct number of readings - EXPECT_EQ(3, readings.size()); - // Check that they are in the correct order - // (sorted in ascending order by angle) - EXPECT_NEAR(0.1, readings[0].angle, 0.000001); - EXPECT_NEAR(0.15, readings[1].angle, 0.000001); - EXPECT_NEAR(0.2, readings[2].angle, 0.000001); -} - -TEST_F(LidarObstacleTest, dangerScoreTest) { - EXPECT_NEAR((cos(0.1) + 1.0 / 10), obstacle1.dangerScore(), 0.000001); - EXPECT_NEAR((cos(0.2) + 1.0 / 20), obstacle2.dangerScore(), 0.000001); - EXPECT_NEAR( - (cos(4.333333333) + 1.0 / 2), obstacle4.dangerScore(), 0.000001); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/decision_igvc/test/lidar_decision_rostest.cpp b/src/decision_igvc/test/lidar_decision_rostest.cpp deleted file mode 100644 index 26bffdd8e..000000000 --- a/src/decision_igvc/test/lidar_decision_rostest.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: February 4th, 2016 - * Description: Integration testing for the lidar_decision node - */ - -#include -#include -#include - -class LidarDecisionTest : public testing::Test { - protected: - virtual void SetUp() { - laser_scan_publisher = - nh_.advertise("/robot/laser/scan", 1); - twist_subscriber = nh_.subscribe( - "/lidar_decision/twist", 1, &LidarDecisionTest::callback, this); - - // Create a fake laserscan - ulong num_rays = 360; - test_scan.angle_min = (float) (-M_PI / 2); - test_scan.angle_max = (float) (M_PI / 2); - test_scan.angle_increment = - (test_scan.angle_max - test_scan.angle_min) / num_rays; - // Set all the ranges to 0 initially - test_scan.ranges = std::vector(num_rays, 0); - test_scan.range_min = 2; - test_scan.range_max = 40; - - ros::spinOnce(); - ros::Rate loop_rate(1); - loop_rate.sleep(); - } - - ros::NodeHandle nh_; - ros::Publisher laser_scan_publisher; - ros::Subscriber twist_subscriber; - - geometry_msgs::Twist command; - - sensor_msgs::LaserScan test_scan; - - public: - void callback(const geometry_msgs::Twist::ConstPtr msg) { command = *msg; } -}; - -TEST_F(LidarDecisionTest, oneObstacleStraightAheadTest) { - // Add a large obstacle directly in front - std::fill( - test_scan.ranges.begin() + 140, test_scan.ranges.begin() + 220, 3); - - laser_scan_publisher.publish(test_scan); - - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - // With the given laserscan, we would want to be turning - EXPECT_GE(abs(command.angular.z), 0.00001); - - // We would also want to slow down - EXPECT_NEAR(1.7320508, command.linear.x, 0.000001); - - // Everything else should always be 0 - EXPECT_EQ(0, command.linear.y); - EXPECT_EQ(0, command.linear.z); - EXPECT_EQ(0, command.angular.x); - EXPECT_EQ(0, command.angular.y); -} - -TEST_F(LidarDecisionTest, noObstacles) { - laser_scan_publisher.publish(test_scan); - - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - // With the given laserscan, we would expect no command, - // as there are no obstacles to base a command off of - EXPECT_EQ(0, command.linear.x); - EXPECT_EQ(0, command.linear.y); - EXPECT_EQ(0, command.linear.z); - EXPECT_EQ(0, command.angular.x); - EXPECT_EQ(0, command.angular.y); - EXPECT_EQ(0, command.angular.z); -} - -TEST_F(LidarDecisionTest, obstacleToRight) { - // Add a large obstacle to the right of the robot - std::fill( - test_scan.ranges.begin() + 100, test_scan.ranges.begin() + 140, 3); - - laser_scan_publisher.publish(test_scan); - - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - // With the given laserscan, we would want to be turning left - EXPECT_GE(command.angular.z, 0.0001); - - // We would also want to slow down - EXPECT_NEAR(1.7320508, command.linear.x, 0.000001); - - // Everything else should always be 0 - EXPECT_EQ(0, command.linear.y); - EXPECT_EQ(0, command.linear.z); - EXPECT_EQ(0, command.angular.x); - EXPECT_EQ(0, command.angular.y); -} - -TEST_F(LidarDecisionTest, obstacleToLeft) { - // Add a large obstacle to the left of the robot - std::fill( - test_scan.ranges.begin() + 220, test_scan.ranges.begin() + 260, 3); - - laser_scan_publisher.publish(test_scan); - - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - // With the given laserscan, we would want to be turning right - EXPECT_LE(command.angular.z, -0.0001); - - // We would also want to slow down - EXPECT_NEAR(1.7320508, command.linear.x, 0.000001); - - // Everything else should always be 0 - EXPECT_EQ(0, command.linear.y); - EXPECT_EQ(0, command.linear.z); - EXPECT_EQ(0, command.angular.x); - EXPECT_EQ(0, command.angular.y); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "lidar_decision_rostest"); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/decision_igvc/test/lidar_decision_rostest.test b/src/decision_igvc/test/lidar_decision_rostest.test deleted file mode 100644 index 3740a710e..000000000 --- a/src/decision_igvc/test/lidar_decision_rostest.test +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/decision_igvc/test/vision-decision-test.cpp b/src/decision_igvc/test/vision-decision-test.cpp deleted file mode 100644 index 0b041c6b5..000000000 --- a/src/decision_igvc/test/vision-decision-test.cpp +++ /dev/null @@ -1,568 +0,0 @@ -/* - * Created By: Robyn Castro - * Created On: September 22, 2016 - * Description: Tests for VisionDecision - */ - -#include -#include -#include -#include -#include - -using namespace cv; -using namespace std; - -sensor_msgs::Image convertToSensorMsg(Mat cvMatImage); - -double confidence_value; - -// Dictates how much new samples will influence the current -// average (Smaller value means less influence). -const double rolling_average_constant = 0.25; - -// Dictates how much of the image samples need to be valid. -const double percent_of_samples_needed = 0.125; - -// Dictates how much of the image is sampled -const double percent_of_image_sampled = 0.25; - -// Dicates the angle to just ignore angle and turn away from the line -const double move_away_threshold = 20; - -// Dictates how confident vision has to be to move -const double confidence_threshold = 60; - -// Dictates how much white needs to be onscreen to be confident -const double percent_of_white_needed = 0.05; - -// Dictates the amount of error allowed -const double error_margin = 20; - -TEST(imageTest, moveAwayFromLineAndTurnRight) { - string filename = "imageTests/testStraightImage.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, angleLeft) { - string filename = "imageTests/testLeftImage.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-30, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, angleRight) { - string filename = "imageTests/testNoisyRightImage.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(30, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, moveAwayFromLineAndTurnRightTwo) { - string filename = "imageTests/testVeryNoisyStraightImage.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, moveAwayFromLineAndTurnLeft) { - string filename = "imageTests/testStraightOnRightSide.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, noisyLeft) { - string filename = "imageTests/testVeryNoisyLeftImage.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -/** - * Tests for line that signals to turn left while starting higher than the - * bottom - * of the image. - */ -TEST(imageTest, elevatedLeftLine) { - string filename = "imageTests/testElevatedLeftLine.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-30, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -/** - * Tests for line that signals to turn right while starting higher than the - * bottom - * of the image. - */ -TEST(imageTest, elevatedRightLine) { - string filename = "imageTests/testElevatedRightLine.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(30, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, straightButLineNearEdge) { - string filename = "imageTests/testStraightButLineNearEdge.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, perpendicular) { - string filename = "imageTests/testPerpendicular.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed); - - EXPECT_EQ(0, confidence_value); -} - -TEST(imageTest, curved) { - string filename = "imageTests/testCurvedLine.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, multiLineLeft) { - string filename = "imageTests/testMultipleLinesLeft.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-30, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, multiLineRight) { - string filename = "imageTests/testMultipleLinesRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(30, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, rightAngleLeft) { - string filename = "imageTests/testRightAngleTurnLeft.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, rightAngleRight) { - string filename = "imageTests/testRightAngleTurnRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, cornerAngleRight) { - string filename = "imageTests/testCornerTurnRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, cornerBigAngleRight) { - string filename = "imageTests/testCornerTurnRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, cornerBigAngleLeft) { - string filename = "imageTests/testBigCornerTurnLeft.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, cornerAngleLeft) { - string filename = "imageTests/testCornerTurnLeft.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, coneAngleLeft) { - string filename = "imageTests/testConeTurnLeft.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(-45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, coneAngleRight) { - string filename = "imageTests/testConeTurnRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, oppositeConeAngleRight) { - string filename = "imageTests/testOppositeConeTurnRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, avoidConeLeft) { - string filename = "imageTests/testAvoidCone.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(imageTest, avoidConeRight) { - string filename = "imageTests/testAvoidConeRight.jpg"; - Mat image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE); - - sensor_msgs::Image sensorMsg = convertToSensorMsg(image); - - sensor_msgs::ImageConstPtr testImageScan(new sensor_msgs::Image(sensorMsg)); - - EXPECT_NEAR(45, - VisionDecision::getDesiredAngle(testImageScan->height / 8.0, - testImageScan, - rolling_average_constant, - percent_of_image_sampled, - percent_of_samples_needed, - confidence_value, - move_away_threshold, - percent_of_white_needed), - error_margin); -} - -TEST(speedTest, linear) { - EXPECT_EQ(1, VisionDecision::getDesiredLinearSpeed(0)); - EXPECT_EQ(0, VisionDecision::getDesiredLinearSpeed(90)); - EXPECT_EQ(0, VisionDecision::getDesiredLinearSpeed(-90)); - EXPECT_EQ(0.5, VisionDecision::getDesiredLinearSpeed(45)); -} - -/** - * Helper function to turn a cvMatImage into a sensor image. - */ -sensor_msgs::Image convertToSensorMsg(Mat cvMatImage) { - sensor_msgs::Image img_msg; // >> message to be sent - MatIterator_ it, end; - - img_msg.height = cvMatImage.rows; - img_msg.width = cvMatImage.cols; - img_msg.encoding = "8UC1"; - img_msg.is_bigendian = false; - img_msg.step = (int) cvMatImage.step; - - for (it = cvMatImage.begin(), end = cvMatImage.end(); - it != end; - it++) { - if ((int) *it == 0) - img_msg.data.push_back(0); - else - img_msg.data.push_back(255); - } - - return img_msg; -} - -int main(int aimageTests, char** argv) { - std::cout << argv[0] << std::endl; - testing::InitGoogleTest(&aimageTests, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/drag_race_iarrc/CMakeLists.txt b/src/drag_race_iarrc/CMakeLists.txt deleted file mode 100644 index 0c9167d97..000000000 --- a/src/drag_race_iarrc/CMakeLists.txt +++ /dev/null @@ -1,96 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(drag_race_iarrc) - -# 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() - -# This exports the compile commands so people using Vim with YouCompleteMe -# can have easy autocompletion -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -add_definitions(--std=c++14) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - ) -find_package(sb_utils REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - -# TODO: Wait. Do we need this line if we're not "exporting" the headers from the package -catkin_package( - INCLUDE_DIRS include -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ${sb_utils_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ./include - ) - -## Declare a C++ executable -# add_executable(${PROJECT_NAME}_node src/drag_race_node.cpp) - -# TODO: Perhaps rename to something like `drag_race_controller_node` -add_executable(drag_race_node - src/drag_race_node.cpp - src/DragRaceNode.cpp - src/DragRaceController.cpp - src/LidarObstacleManager.cpp - src/LidarObstacle.cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -target_link_libraries(drag_race_node - ${catkin_LIBRARIES} - ${sb_utils_LIBRARIES} - ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_drag_race.cpp) -# 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(lidar-obstacle-manager-test - test/lidar-obstacle-manager-test.cpp - src/LidarObstacleManager.cpp - src/LidarObstacle.cpp) - target_link_libraries(lidar-obstacle-manager-test ${catkin_LIBRARIES}) - - catkin_add_gtest(drag-race-lidar-obstacle-test - test/lidar-obstacle-test.cpp - src/LidarObstacle) - target_link_libraries(drag-race-lidar-obstacle-test ${catkin_LIBRARIES}) - - catkin_add_gtest(drag-race-controller-test - test/drag-race-controller-test.cpp - src/DragRaceController.cpp) - target_link_libraries(drag-race-controller-test ${catkin_LIBRARIES}) - - # TODO: Maybe some rostests? - -endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/drag_race_iarrc/include/DragRaceController.h b/src/drag_race_iarrc/include/DragRaceController.h deleted file mode 100644 index a53b7cb38..000000000 --- a/src/drag_race_iarrc/include/DragRaceController.h +++ /dev/null @@ -1,78 +0,0 @@ -/* - * Created By: Robyn Castro - * Created On: July 9th, 2017 - * Description: Given a line and a set of parameters, determines - * how the car should move in the drag race. - */ - -#ifndef DRAG_RACE_CONTROLLER_H -#define DRAG_RACE_CONTROLLER_H - -// ROS Includes -#include - -// SB Includes -#include - -class DragRaceController { - public: - /** - * Empty Constructor - */ - DragRaceController(); - - /** - * Initializes parameters of the robot. - * - * @param targetDistance - * @param lineToTheRight - * @param theta_scaling_multiplier - * @param angular_speed_multiplier - * @param linear_speed_multiplier - * @param angular_vel_cap - * @param linear_vel_cap - */ - DragRaceController(double targetDistance, - bool lineToTheRight, - double theta_scaling_multiplier, - double angular_speed_multiplier, - double linear_speed_multiplier, - double angular_vel_cap, - double linear_vel_cap); - - /** - * Determines the optimal movement to stay within target distance of - * the given line. - * - * @param longestConeLine - * @return the optimal angular and linear acceleration. - */ - geometry_msgs::Twist determineDesiredMotion(LineOfBestFit longestConeLine, - bool no_line_on_expected_side); - - private: - /** - * Finds the minimum distance from given line and the origin. - * - * @param line - * @return the minimum distance from given line and the origin. - */ - static double determineDistanceFromLine(LineOfBestFit line); - - // How far from the target line the robot should be - double target_distance; - - // Where the target line is - bool line_to_the_right; - - // Velocity limits - double angular_vel_cap; - double linear_vel_cap; - - // Scaling - double theta_scaling_multiplier; - double angular_speed_multiplier; - double linear_speed_multiplier; -}; - -#endif // DRAG_RACE_CONTROLLER_H \ No newline at end of file diff --git a/src/drag_race_iarrc/include/DragRaceNode.h b/src/drag_race_iarrc/include/DragRaceNode.h deleted file mode 100644 index 2a26e7a9a..000000000 --- a/src/drag_race_iarrc/include/DragRaceNode.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July ??, 2017 - * Description: A node that runs a drag race outlined with cones, - * starting when the green light is detected (a boolean topic) - * stopping before the wall at the end of the drag strip - */ - -#ifndef DRAG_RACE_NODE_DRAG_RACE_H -#define DRAG_RACE_NODE_DRAG_RACE_H - -// STD Includes -#include -#include - -// ROS Includes -// TODO: Sort me for neatness -#include -#include -#include -#include -#include - -// SB Includes -#include -#include -#include - -class DragRaceNode { - public: - DragRaceNode(int argc, char** argv, std::string node_name); - - private: - // TODO: Doc comment - void scanCallBack(const sensor_msgs::LaserScan::ConstPtr& scan); - - void greenLightCallBack(const std_msgs::Bool& green_light_detected); - - // Manages obstacles, including the cones and wall - LidarObstacleManager obstacle_manager; - - // Manages line handling and movement - DragRaceController drag_race_controller; - - // How far lines can be before being considered invalid - double max_distance_from_robot_accepted; - - // How far from the target line the robot should be - double target_distance; - - // Where the target line is - bool line_to_the_right; - - // Velocity limits - double angular_vel_cap; - double linear_vel_cap; - - // Scaling - double theta_scaling_multiplier; - double angular_speed_multiplier; - double linear_speed_multiplier; - - // Traffic light detection - int minimum_green_recognised_count; - int green_count_recognised; - - // End zone collision detection - // TODO: Could make all these local variables? - int incoming_obstacle_ticks; - int obstacle_ticks_threshold; - double collision_distance; - double front_angle; - double side_angle_max; - double side_angle_min; - double region_fill_percentage; - bool front_collision_only; - - // Signals that we're at the end of the course - bool end_of_course; - - // Subscribes to the LaserScan - ros::Subscriber scan_subscriber; - // Subscribes to traffic light detection - ros::Subscriber traffic_light_subscriber; - - // Publishes Twist messages to control the robot - ros::Publisher twist_publisher; - // Publishes the obstacles so we can see them in RViz - ros::Publisher cone_debug_publisher; - // Publishes the cone lines so we can see them in RViz - ros::Publisher cone_line_debug_publisher; - // Publishes the cone line we're using to determine the twist message - // so we can see it in RViz - ros::Publisher best_line_debug_publisher; -}; - -#endif // DRAG_RACE_NODE_DRAG_RACE_H diff --git a/src/drag_race_iarrc/include/LidarObstacle.h b/src/drag_race_iarrc/include/LidarObstacle.h deleted file mode 100644 index 6a756beda..000000000 --- a/src/drag_race_iarrc/include/LidarObstacle.h +++ /dev/null @@ -1,245 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: January 22, 2017 - * Description: This class represents an Obstacle, as detected by the lidar, - * and provides various utilities for working with obstacles - */ - -#ifndef DRAG_RACE_IARRC_LIDAROBSTACLE_H -#define DRAG_RACE_IARRC_LIDAROBSTACLE_H - -// STD -#include -#include - -using distance_t = double; -using angle_t = double; - -// Types of Obstacle -enum ObstacleType { NONE, CONE, WALL }; - -struct Reading { - angle_t angle; - distance_t range; -}; - -// TODO: We should just use geometry_msgs::Point -struct Point { - double x; - double y; -}; -double distanceBetweenPoints(const Point& p1, const Point& p2); -bool operator==(const Point& p1, const Point& p2); - -class LidarObstacle { - public: - // TODO: see what functions we can delete here.... probably don't want all - // them - /** - * Creates a LidarObstacle with no readings - */ - LidarObstacle(); - - /** - * Creates a LidarObstacle with no readings - * - * @param min_wall_length the minimum length for the obstacle to be - * considered a wall - */ - // TODO: default constructors suck (not really but still...). we should - // require min_wall_length - LidarObstacle(double min_wall_length); - - /** - * Creates a LidarObstacle with a given distance and angle - * - * @param distance The distance to the obstacle - * @param angle The angle to the obstacle - */ - LidarObstacle(angle_t angle, distance_t distance); - - /** - * Creates a LidarObstacle with a given distance and angle - * - * @param min_wall_length the minimum length for the obstacle to be - * considered a wall - * @param distance The distance to the obstacle - * @param angle The angle to the obstacle - */ - LidarObstacle(double min_wall_length, angle_t angle, distance_t distance); - - /** - * Creates a LidarObstacle from a given set of readings - * - * @param readings readings to initialize the obstacle with - */ - LidarObstacle(std::vector readings); - - /** - * Creates a LidarObstacle from a given set of readings - * - * @param min_wall_length the minimum length for the obstacle to be - * considered a wall - * @param readings readings to initialize the obstacle with - */ - LidarObstacle(double min_wall_length, std::vector readings); - - /** - * Gets the distance to the rightmost point of the Obstacle - * - * @return the distance to the rightmost point of the Obstacle - */ - distance_t getFirstDistance(); - - /** - * Gets the distance to the leftmost point of the Obstacle - * - * @return the distance to the leftmost point of the obstacle - */ - - distance_t getLastDistance(); - /** - * Gets the average distance of the Obstacle from the robot - * - * The distance from the robot is the average of all scan distances - * - * @return the distance of the Obstacle from the robot - */ - double getAvgDistance(); - - /** - * Gets the angle of the Obstacle from the robot - * - * The angle from the robot is the average of all scan angles - * - * @return the angle of the Obstacle from the robot - */ - double getAvgAngle(); - - /** - * Gets the minimum angle from of an object from the robot - * - * @return the minimum angle of the obstacle from the robot - */ - double getMinAngle(); - - /** - * Gets the maximum angle from of an object from the robot - * - * @return the maximum angle of the obstacle from the robot - */ - double getMaxAngle(); - - /** - * Gets the minimum distance from of an object from the robot - * - * @return the minimum distance of the obstacle from the robot - */ - double getMinDistance(); - - /** - * Gets the maximum distance from of an object from the robot - * - * @return the maximum distance of the obstacle from the robot - */ - double getMaxDistance(); - - /** - * Gets all laser readings comprising the obstacle - * - * @return readings A list of pairs of all laser readings - */ - const std::vector& getAllLaserReadings(); - - /** - * Merges the given LidarObstacle in to this LidarObstacle - * - * Adds the given LidarObstacle's scan distances and scan angles to - * this LidarObstacles scan distances and scan angles respectively - * - * @param obstacle The LidarObstacle to be merged in - */ - void mergeInLidarObstacle(LidarObstacle& obstacle); - - /** - * Gets the type of the this obstacle - * - * @return the type of this obstacle - */ - ObstacleType getObstacleType(); - - /** - * Determines and sets what the obstacle type should be - */ - void determineObstacleType(); - - /** - * Returns the length of the obstacle - * - * Obstacle length is in this case defined as the distance between - * the leftmost and rightmost readings - * - * @return the length of the obstacle - */ - // TODO: This is a terrible definition. A reading slightly to the right of - // the leftmost reading could have infited range - double getLength(); - - /** - * Get the minimum length for this obstacle to be considered a wall - * - * @return the minimum length for this obstacle to be considered a wall - */ - double getMinWallLength(); - - /** - * Gets the readings composing this obstacle as 2D Points - * - * @return readings composing this obstacle as 2D Points - */ - std::vector getReadingsAsPoints(); - - /** - * Computes a (x,y) point from a given reading - * - * @param reading the reading to compute the point for - * @return the point interpretation of the given reading - */ - static Point pointFromReading(const Reading& reading); - - /** - * Gets the center of the obstacle - * @return the center of the obstacle - */ - Point getCenter(); - - // TODO: Write functions for modeling obstacles as circles and squares to - // reduce comparison times - private: - /** - * Merges a given set of readings into the obstacle - * - * @param readings the readings to be merged in - */ - void mergeInReadings(std::vector& new_readings); - - /** - * Updates the center of the obstacle based on our current readings - */ - void updateCenter(); - // TODO: we should maybe consider moving to storing obstacles as points - // The distances and angles of all the laser scan hits that comprise the - // object. - // readings are stored in sorted order, from min to max angle. - std::vector readings; - - // The type of the obstacle - ObstacleType obstacle_type; - - // The center of the obstacle - Point center; - - double min_wall_length; -}; - -#endif // DRAG_RACE_IARRC_LIDAROBSTACLE_H diff --git a/src/drag_race_iarrc/include/LidarObstacleManager.h b/src/drag_race_iarrc/include/LidarObstacleManager.h deleted file mode 100644 index 90ec9bee0..000000000 --- a/src/drag_race_iarrc/include/LidarObstacleManager.h +++ /dev/null @@ -1,241 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July 4, 2017 - * Description: TODO - */ - -#ifndef DRAG_RACE_LIDAROBSTACLEMANAGER_H -#define DRAG_RACE_LIDAROBSTACLEMANAGER_H - -// STD Includes -#include - -// ROS Includes -#include -#include - -// SB Includes -#include - -class SlopeInterceptLine { - public: - SlopeInterceptLine(double slope, double y_intercept) - : slope(slope), y_intercept(y_intercept) {} - - // TODO: DOC functions - inline double getSlope() { return slope; } - - inline double getYIntercept() { return y_intercept; } - - inline double getXIntercept() { return -y_intercept / slope; } - - inline double getXCoorAtY(double y) { - if (slope == 0) - return y_intercept; - else - return (y - y_intercept) / slope; - } - - inline double getYCoorAtX(double x) { return slope * x + y_intercept; } - - protected: - double slope; - double y_intercept; -}; - -/** - * A line with a Correlation Coefficient - */ -class LineOfBestFit : public SlopeInterceptLine { - public: - LineOfBestFit(double slope, double y_intercept, double correlation) - : SlopeInterceptLine(slope, y_intercept), correlation(correlation) {} - - // TODO: Add getter function and make private - double correlation; -}; - -struct FiniteLine { - double start_x; - double start_y; - double slope; - double length; -}; - -class LidarObstacleManager { - public: - LidarObstacleManager(); - - LidarObstacleManager(double max_obstacle_merging_distance, - double cone_grouping_tolerance, - double max_distance_from_robot_accepted, - double min_wall_length, - double collision_distance, - double front_angle, - double side_angle_max, - double side_angle_min, - double region_fill_percentage, - bool front_collision_only); - - /** - * Merges or adds the given obstacle to the already saved ones - * - * @param obstacle the obstacle to be added - */ - void addObstacle(LidarObstacle obstacle); - - /** - * Finds a saves obstacles from the given scan - * - * Gets obstacles from the given scan and merges or adds them to - * the already saved obstacles - * - * @param scan the scan to be merged in - */ - void addLaserScan(const sensor_msgs::LaserScan& scan); - - /** - * Determines whether there is an obstacle in front of the robot - * @param scan the laserscan containing the area data - * @param min_obstacle_distance the distance of obstacle in front to compare - * @return true if there is an obstancle in front, false otherwise - */ - bool obstacleInFront(const sensor_msgs::LaserScan& scan, - double min_obstacle_distance); - - /** - * Clears all saved obstacles - */ - void clearObstacles(); - - /** - * Gets all obstacles - * @return all saved obstacles - */ - std::vector getObstacles(); - - /** - * Gets all lines of cones in the saved obstacles - * @return all lines of cones in the saved obstacles - */ - std::vector getConeLines(); - - /** - * Gets a line of best fit for a given group of points - * - * @param points the points to fit the line to - */ - static LineOfBestFit getLineOfBestFit(const std::vector& points); - - LineOfBestFit getBestLine(bool lineToTheRight); - - /** - * Finds groups of points within a larger group of points - * - * @param points the points to find the groups in - * @param tolerance the maximum distance between any two points in a group - * - * @return a vector of groups of points (as vectors) - */ - std::vector> getPointGroupings(std::vector points, - double tolerance); - - /** - * Determines the minimum distance between two obstacles - * - * @param obstacle1 - * @param obstacle2 - * @return the minimum distance between obstacle1 and obstacle2 - */ - static double minDistanceBetweenObstacles(LidarObstacle obstacle1, - LidarObstacle obstacle2); - - /** - * Gets all stored obstacles as a marker of points that can be rendered in - * RViz - * - * @return all stored obstacles as a marker of points that can be rendered - * in RViz - */ - // TODO: when we update lidarObstacle to be a bit more obstract, improve - // this as well - // TODO: to represent things like radius - // TODO: TEST ME - visualization_msgs::Marker getConeRVizMarker(); - - /** - * Gets the lines determined from cones as a marker of lines that can be - * rendered in RViz - * - * @return the lines determined from cones as a marker of lines that can be - * rendered in RViz - */ - // TODO: Can we add line endings? - // TODO: TEST ME - visualization_msgs::Marker getConeLinesRVizMarker(); - - /** - * Gets the "best" line from all the cone lines as a marker we can visualize - * in RViz - * - * @return the lines determined from cones as a marker that can be rendered - * in RViz - */ - // TODO: Can we add line endings? - // TODO: TEST ME - // TODO: Add line_to_the_right to the constructor - visualization_msgs::Marker - getBestConeLineRVizMarker(bool line_to_the_right); - - bool collisionDetected(); - - private: - // **** END ZONE COLLISION DETECTION **** - // TODO: Move this to its own node later. - - // The threshold for object hitscan distance - double collision_distance; - - // If True, only uses front collision detection - bool front_collision_only; - - // The angle which defines the front region relative to the front of the - // robot - // e.g. If set to PI/2, the region extends from PI/4 to -PI/4 relative to a - // line - // extending from the front of the robot. - double front_angle; - - // Parameters for side obstacle detection - // The angles which define the side region of the robot - double side_angle_max; - double side_angle_min; - - // The percentage of the region to be filled before we consider it a - // collision - double region_fill_percentage; - - // ****************************** - - // All the obstacles we currently have - std::vector obstacles; - - // The maximum distance between two obstacles for them to be considered the - // same - double max_obstacle_merging_distance; - - // The maximum distance a line can have from the robot on the y-axis before - // being thrown out - double max_distance_from_robot_accepted; - - // The maximum permitted distance between cones in the same group - double cone_grouping_tolerance; - - // The mimimum length of an obstacle before it's considered a wall - double min_wall_length; - - // True if there is an obstacle within collision_distance away - bool collision_detected; -}; - -#endif // DRAG_RACE_LIDAROBSTACLEMANAGER_H diff --git a/src/drag_race_iarrc/launch/drag_race.launch b/src/drag_race_iarrc/launch/drag_race.launch deleted file mode 100644 index 16ac36d67..000000000 --- a/src/drag_race_iarrc/launch/drag_race.launch +++ /dev/null @@ -1,60 +0,0 @@ - - - - - - - - - - - - - - - - - - - -1 - - - - 0.3 - - 0.4 - 10 - - - 3.0 - 3.1415 - 1.57 - 1.30 - 0.5 - true - - - - - 1.8 - - true - - 2 - - - - 0.75 - 1 - 1 - 1.75 - 0.75 - 1 - - - - diff --git a/src/drag_race_iarrc/package.xml b/src/drag_race_iarrc/package.xml deleted file mode 100644 index 2df635459..000000000 --- a/src/drag_race_iarrc/package.xml +++ /dev/null @@ -1,64 +0,0 @@ - - - drag_race_iarrc - 0.0.0 - The drag_race package - - - - - gareth - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - roscpp - std_msgs - sensor_msgs - visualization_msgs - sb_utils - - roscpp - std_msgs - sensor_msgs - visualization_msgs - sb_utils - - urg_node - - - - - - - - diff --git a/src/drag_race_iarrc/src/DragRaceController.cpp b/src/drag_race_iarrc/src/DragRaceController.cpp deleted file mode 100644 index f5f0d5e95..000000000 --- a/src/drag_race_iarrc/src/DragRaceController.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Created By: Robyn Castro - * Created On: July 9th, 2017 - * Description: Given a line and a set of parameters, determines - * how the car should move in the drag race. - */ -#include - -DragRaceController::DragRaceController(){}; - -DragRaceController::DragRaceController(double targetDistance, - bool lineToTheRight, - double theta_scaling_multiplier, - double angular_speed_multiplier, - double linear_speed_multiplier, - double angular_vel_cap, - double linear_vel_cap) - : target_distance(targetDistance), - line_to_the_right(lineToTheRight), - theta_scaling_multiplier(theta_scaling_multiplier), - angular_speed_multiplier(angular_speed_multiplier), - linear_speed_multiplier(linear_speed_multiplier), - angular_vel_cap(angular_vel_cap), - linear_vel_cap(linear_vel_cap) {} - -geometry_msgs::Twist -DragRaceController::determineDesiredMotion(LineOfBestFit longestConeLine, - bool no_line_on_expected_side) { - // Determine angle of line. - double theta = atan(longestConeLine.getSlope()); - - double distanceError; - double minDistanceFromLine = determineDistanceFromLine(longestConeLine); - - if (no_line_on_expected_side) { - // Aim for three half-lanes across the farther line if in plan b. - double planBTargetDistance = target_distance * 3; - distanceError = planBTargetDistance - minDistanceFromLine; - } else - // Aim for one half-lane across the closer line if in plan a. - distanceError = target_distance - minDistanceFromLine; - - // Make distance relative to where the line is, turning it into - // displacement. - if (!line_to_the_right) distanceError *= -1.0; - - // Account for using the opposite line (Flip displacement). - if (no_line_on_expected_side) distanceError *= -1.0; - - geometry_msgs::Twist command; - - // Set components we don't care about to 0 - command.linear.y = 0; - command.linear.z = 0; - command.angular.x = 0; - command.angular.y = 0; - - // If no line then go straight. - if (longestConeLine.correlation == 0) - command.angular.z = 0; - else - // Figure out how fast we should be turning - command.angular.z = (theta_scaling_multiplier * theta + distanceError) * - angular_speed_multiplier; - - // Limit the angular velocity - if (fabs(command.angular.z) > angular_vel_cap) - command.angular.z = - angular_vel_cap * command.angular.z / fabs(command.angular.z); - - // Figure out how fast we should be moving forward - if (command.angular.z != 0) - command.linear.x = linear_speed_multiplier / fabs(command.angular.z); - else - command.linear.x = linear_vel_cap; - - // Limit the linear velocity - if (command.linear.x > linear_vel_cap) command.linear.x = linear_vel_cap; - - return command; -} - -double DragRaceController::determineDistanceFromLine(LineOfBestFit line) { - double negReciprocal = -1 / line.getSlope(); - - /* Find the intersection between the line and its perpendicular line. */ - - // Set the two sides equal then isolate x to one side. - double isolatedXSlope = negReciprocal - line.getSlope(); - - // Divide both sides by the isolated slope to get the x point intersection. - double xIntersection = line.getYIntercept() / isolatedXSlope; - - // Plug in the xIntersection to get the y point intersection. - double yIntersection = negReciprocal * xIntersection; - - // Return distance found - return sqrt(pow(xIntersection, 2) + pow(yIntersection, 2)); -} \ No newline at end of file diff --git a/src/drag_race_iarrc/src/DragRaceNode.cpp b/src/drag_race_iarrc/src/DragRaceNode.cpp deleted file mode 100644 index 441d0c63a..000000000 --- a/src/drag_race_iarrc/src/DragRaceNode.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July ??, 2017 - * Description: A node that runs a drag race outlined with cones, - * starting when the green light is detected (a boolean topic) - * stopping before the wall at the end of the drag strip - */ - -#include - -DragRaceNode::DragRaceNode(int argc, char** argv, std::string node_name) - : green_count_recognised(0) { - // Setup NodeHandles - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - // Setup Subscriber(s) - std::string scan_topic = "/scan"; - uint32_t queue_size = 1; - - scan_subscriber = - nh.subscribe(scan_topic, queue_size, &DragRaceNode::scanCallBack, this); - - // TODO: make sure this is the right topic name - std::string traffic_light_topic = "/robot/vision/activity_detected"; - - traffic_light_subscriber = nh.subscribe( - traffic_light_topic, queue_size, &DragRaceNode::greenLightCallBack, this); - - // Setup Publisher(s) - std::string twist_topic = nh.resolveName("cmd_vel"); - twist_publisher = - nh.advertise(twist_topic, queue_size); - std::string cone_debug_topic = private_nh.resolveName("debug/cone"); - cone_debug_publisher = private_nh.advertise( - cone_debug_topic, queue_size); - std::string cone_lines_debug_topic = - private_nh.resolveName("debug/cone_lines"); - cone_line_debug_publisher = - private_nh.advertise(cone_lines_debug_topic, - queue_size); - std::string best_line_debug_topic = - private_nh.resolveName("debug/best_line"); - best_line_debug_publisher = - private_nh.advertise(best_line_debug_topic, - queue_size); - - // Get Params - SB_getParam(private_nh, "target_distance", target_distance, 1.0); - SB_getParam(private_nh, "angular_vel_cap", angular_vel_cap, 1.0); - SB_getParam(private_nh, "linear_vel_cap", linear_vel_cap, 1.0); - SB_getParam( - private_nh, "theta_scaling_multiplier", theta_scaling_multiplier, 1.0); - SB_getParam( - private_nh, "angular_speed_multiplier", angular_speed_multiplier, 1.0); - SB_getParam( - private_nh, "linear_speed_multiplier", linear_speed_multiplier, 1.0); - SB_getParam(private_nh, "line_to_the_right", line_to_the_right, true); - double max_obstacle_merging_distance, cone_grouping_tolerance, - min_wall_length; - SB_getParam(private_nh, - "max_obstacle_merging_distance", - max_obstacle_merging_distance, - 0.3); - SB_getParam( - private_nh, "cone_grouping_tolerance", cone_grouping_tolerance, 1.8); - SB_getParam(private_nh, - "max_distance_from_robot_accepted", - max_distance_from_robot_accepted, - 2.0); - SB_getParam(private_nh, "min_wall_length", min_wall_length, 0.4); - SB_getParam(private_nh, - "minimum_green_count_recognised", - minimum_green_recognised_count, - 10); - SB_getParam( - private_nh, "obstacle_ticks_threshold", obstacle_ticks_threshold, 10); - SB_getParam(private_nh, "collision_distance", collision_distance, 3.0); - - // NOW IN RADIANS - SB_getParam(private_nh, "front_angle", front_angle, M_PI_2); - SB_getParam( - private_nh, "front_collision_only", front_collision_only, false); - SB_getParam(private_nh, "side_angle_max", side_angle_max, M_PI_2); - SB_getParam( - private_nh, "side_angle_min", side_angle_min, M_PI_2 - M_PI_4 / 2); - SB_getParam( - private_nh, "region_fill_percentage", region_fill_percentage, 0.5); - - // Setup drag race controller with given params - drag_race_controller = DragRaceController(target_distance, - line_to_the_right, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - - // Setup the obstacle manager with given params - obstacle_manager = LidarObstacleManager(max_obstacle_merging_distance, - cone_grouping_tolerance, - max_distance_from_robot_accepted, - min_wall_length, - collision_distance, - front_angle, - side_angle_max, - side_angle_min, - region_fill_percentage, - front_collision_only); - - end_of_course = false; - incoming_obstacle_ticks = 0; -} - -void DragRaceNode::greenLightCallBack( -const std_msgs::Bool& green_light_detected) { - if (green_light_detected.data) { green_count_recognised++; } -} - -void DragRaceNode::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& scan) { - // Clear any obstacles we already have - obstacle_manager.clearObstacles(); - - // Insert the scan we just received - obstacle_manager.addLaserScan(*scan); - - bool no_line_on_expected_side = false; - - // TODO: Option 1 - if (obstacle_manager.collisionDetected()) { - incoming_obstacle_ticks++; - } else { - // False alarm - incoming_obstacle_ticks = 0; - - // This is maybe better? Experiment - // incoming_obstacle_ticks--; - // if (incoming_obstacle_ticks < 0) incoming_obstacle_ticks = 0; - } - - if (incoming_obstacle_ticks > obstacle_ticks_threshold) { - if (!end_of_course) { - end_of_course = true; - ROS_INFO("END OF COURSE DETECTED"); - } - } - - // Get the best line for us - LineOfBestFit best_line = obstacle_manager.getBestLine(line_to_the_right); - - // If no good lines, initiate plan B and use lines on the other side. - if (best_line.correlation == 0) { - best_line = obstacle_manager.getBestLine(!line_to_the_right); - no_line_on_expected_side = true; - } - - // Avoid the line given while staying within the boundaries - geometry_msgs::Twist twist = drag_race_controller.determineDesiredMotion( - best_line, no_line_on_expected_side); - - // If no green light has been detected stop. - if (green_count_recognised < minimum_green_recognised_count) { - twist.angular.z = 0; - twist.linear.x = 0; - } - - if (end_of_course) { - twist.linear.x = twist.linear.y = twist.linear.z = 0; - twist.angular.x = twist.angular.y = twist.angular.z = 0; - } - - // Publish our desired twist message - twist_publisher.publish(twist); - - // TODO: have a debug param for this - // Broadcast a visualisable representation so we can see obstacles in RViz - cone_debug_publisher.publish(obstacle_manager.getConeRVizMarker()); - cone_line_debug_publisher.publish( - obstacle_manager.getConeLinesRVizMarker()); - best_line_debug_publisher.publish( - obstacle_manager.getBestConeLineRVizMarker(line_to_the_right)); -} diff --git a/src/drag_race_iarrc/src/LidarObstacle.cpp b/src/drag_race_iarrc/src/LidarObstacle.cpp deleted file mode 100644 index b53940a7f..000000000 --- a/src/drag_race_iarrc/src/LidarObstacle.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: January 22, 2017 - * Description: This class represents an Obstacle, as detected by the lidar, - * and provides various utilities for working with obstacles - */ - -// STD Includes -#include -#include - -// Snowbots Includes -#include - -// TODO: min wall length, and determination of whether to merge obstacles and -// what their type is, -// TODO: all really belongs in LidarObstacleManager -LidarObstacle::LidarObstacle() : LidarObstacle(std::vector()) {} - -LidarObstacle::LidarObstacle(double min_wall_length) - : LidarObstacle(min_wall_length, std::vector()) {} - -LidarObstacle::LidarObstacle(angle_t angle, distance_t distance) - : LidarObstacle({Reading{angle, distance}}) {} - -LidarObstacle::LidarObstacle(double min_wall_length, - angle_t angle, - distance_t distance) - : LidarObstacle(min_wall_length, {Reading{angle, distance}}) {} - -LidarObstacle::LidarObstacle(std::vector readings) - : LidarObstacle(1, readings) {} - -LidarObstacle::LidarObstacle(double min_wall_length, - std::vector readings) - : min_wall_length(min_wall_length), obstacle_type(NONE) { - this->mergeInReadings(readings); -} - -distance_t LidarObstacle::getAvgDistance() { - double total_distance = std::accumulate( - readings.begin(), readings.end(), 0, [](int accumulator, auto reading) { - return accumulator + reading.range; - }); - return total_distance / readings.size(); -} - -angle_t LidarObstacle::getAvgAngle() { - double total_angle = std::accumulate(readings.begin(), - readings.end(), - 0.0, - [](double accumulator, auto reading) { - return accumulator + reading.angle; - }); - return total_angle / readings.size(); -} - -angle_t LidarObstacle::getMinAngle() { - // Readings are sorted, so min angle is just the first Reading - return readings[0].angle; -} - -angle_t LidarObstacle::getMaxAngle() { - // Readings are sorted, so max angle is just the last Reading - return readings[readings.size() - 1].angle; -} - -distance_t LidarObstacle::getLastDistance() { - return readings[readings.size() - 1].range; -} - -distance_t LidarObstacle::getFirstDistance() { - return readings[0].range; -} - -const std::vector& LidarObstacle::getAllLaserReadings() { - return readings; -}; - -distance_t LidarObstacle::getMinDistance() { - std::vector readings = getAllLaserReadings(); - Reading reading_with_min_distance = - *std::min_element(readings.begin(), - readings.end(), - [&](auto const& reading1, auto const& reading2) { - return reading1.range < reading2.range; - }); - return reading_with_min_distance.range; -} - -distance_t LidarObstacle::getMaxDistance() { - std::vector readings = getAllLaserReadings(); - Reading reading_with_max_distance = - *std::max_element(readings.begin(), - readings.end(), - [&](auto const& reading1, auto const& reading2) { - return reading1.range > reading2.range; - }); - return reading_with_max_distance.range; -} - -void LidarObstacle::mergeInLidarObstacle(LidarObstacle& obstacle) { - // Get readings from obstacle being merged in - std::vector new_readings = obstacle.getAllLaserReadings(); - this->mergeInReadings(new_readings); -} - -void LidarObstacle::mergeInReadings(std::vector& new_readings) { - this->readings.insert( - readings.end(), new_readings.begin(), new_readings.end()); - // Ensure that the readings are still sorted - std::sort(readings.begin(), - readings.end(), - [&](auto const& reading1, auto const& reading2) { - return reading1.angle < reading2.angle; - }); - // Ensure the obstacle type is still correct - determineObstacleType(); - // Update the center of the obstacle - updateCenter(); -} - -void LidarObstacle::updateCenter() { - // Create points for all readings - std::vector points = getReadingsAsPoints(); - - // Average Points to get new center - Point averaged_point{0, 0}; - for (Point p : points) { - averaged_point.x += p.x; - averaged_point.y += p.y; - } - averaged_point.x /= points.size(); - averaged_point.y /= points.size(); - center = averaged_point; -} - -void LidarObstacle::determineObstacleType() { - // TODO: Setup some sort of min number of readings to be considered a cone - // If this obstacle has no readings, then it's NONE - if (readings.size() == 0) obstacle_type = NONE; - // If the obstacle is long enough, then it's a WALL - else if (getLength() > min_wall_length) - obstacle_type = WALL; - else // If it's not NONE or a WALL, then it's a CONE - obstacle_type = CONE; -} - -// TODO: Is there a more appropriate place for these functions? - -double LidarObstacle::getLength() { - // Using Law of Cosines (c^2 = a^2 + b^2 + 2ab*Cos(C)) to get the length - // from the leftmost to the rightmost point - double left_length = readings[0].range; - double right_length = readings.back().range; - double theta = std::abs(readings[0].angle - readings.back().angle); - return std::sqrt(std::pow(left_length, 2) + std::pow(right_length, 2) - - 2 * left_length * right_length * std::cos(theta)); -} - -double LidarObstacle::getMinWallLength() { - return min_wall_length; -} - -ObstacleType LidarObstacle::getObstacleType() { - return obstacle_type; -} - -Point LidarObstacle::getCenter() { - return center; -} - -std::vector LidarObstacle::getReadingsAsPoints() { - std::vector points; - for (Reading reading : readings) { - Point p = pointFromReading(reading); - points.emplace_back(p); - } - return points; -} - -Point LidarObstacle::pointFromReading(const Reading& reading) { - double x = reading.range * std::cos(reading.angle); - double y = reading.range * std::sin(reading.angle); - return Point{x, y}; -} - -double distanceBetweenPoints(const Point& p1, const Point& p2) { - return std::sqrt(std::pow(p1.x - p2.x, 2.0) + std::pow(p1.y - p2.y, 2.0)); -} -bool operator==(const Point& p1, const Point& p2) { - return (p1.x == p2.x && p1.y == p2.y); -} diff --git a/src/drag_race_iarrc/src/LidarObstacleManager.cpp b/src/drag_race_iarrc/src/LidarObstacleManager.cpp deleted file mode 100644 index 8ed052da1..000000000 --- a/src/drag_race_iarrc/src/LidarObstacleManager.cpp +++ /dev/null @@ -1,392 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July 4, 2017 - * Description: TODO - */ - -// STD Includes -#include -#include -#include - -// Snowbots Includes -#include - -// TODO: We should probably get rid of the default constructor here..... -LidarObstacleManager::LidarObstacleManager() - : max_obstacle_merging_distance(0.3), cone_grouping_tolerance(1.3){}; - -LidarObstacleManager::LidarObstacleManager( -double max_obstacle_merging_distance, -double max_distance_from_robot_accepted, -double cone_grouping_tolerance, -double min_wall_length, -double collision_distance, - -double front_angle, -double side_angle_max, -double side_angle_min, -double region_fill_percentage, -bool front_collision_only) - : max_obstacle_merging_distance(max_obstacle_merging_distance), - max_distance_from_robot_accepted(max_distance_from_robot_accepted), - cone_grouping_tolerance(cone_grouping_tolerance), - min_wall_length(min_wall_length), - collision_distance(collision_distance), - front_angle(front_angle), - side_angle_max(side_angle_max), - side_angle_min(side_angle_min), - region_fill_percentage(region_fill_percentage), - front_collision_only(front_collision_only) {} - -void LidarObstacleManager::addLaserScan(const sensor_msgs::LaserScan& scan) { - // Number of hits for each region - int left_side_hits = 0; - int right_side_hits = 0; - int front_side_hits = 0; - - // Create an obstacle for every hit in the lidar scan - for (int i = 0; i < scan.ranges.size(); ++i) { - // Check that the lidar hit is within acceptable bounds - double angle = scan.angle_min + i * scan.angle_increment; - double range = scan.ranges[i]; - if (range < scan.range_max && range > scan.range_min) { - addObstacle(LidarObstacle(min_wall_length, angle, range)); - - double abs_angle = std::abs(angle); - if (abs_angle < side_angle_max && abs_angle > side_angle_min) { - if (range < collision_distance * 2) { - if (angle > 0) - left_side_hits++; - else - right_side_hits++; - } - } - - if (abs_angle < front_angle / 2) { - if (range < collision_distance) front_side_hits++; - } - } - } - - int side_region_total_size = - (side_angle_max - side_angle_min) / scan.angle_increment; - - int front_region_total_size = front_angle / scan.angle_increment; - - if (front_collision_only) { - collision_detected = - (front_side_hits > (front_region_total_size * region_fill_percentage)); - } else { - collision_detected = - (left_side_hits > (side_region_total_size * region_fill_percentage)) && - (right_side_hits > (side_region_total_size * region_fill_percentage)) && - (front_side_hits > (front_region_total_size * region_fill_percentage)); - } -} - -std::vector LidarObstacleManager::getObstacles() { - return obstacles; -} - -void LidarObstacleManager::clearObstacles() { - obstacles.clear(); -} - -void LidarObstacleManager::addObstacle(LidarObstacle obstacle) { - // See if this obstacle is close enough to any other saved obstacle to be - // the same - // TODO: Should we be instead checking for the CLOSEST saved obstcle? - for (LidarObstacle& saved_obstacle : obstacles) { - if (minDistanceBetweenObstacles(saved_obstacle, obstacle) < - max_obstacle_merging_distance) { - saved_obstacle.mergeInLidarObstacle(obstacle); - return; - } - } - obstacles.emplace_back(obstacle); -} - -double -LidarObstacleManager::minDistanceBetweenObstacles(LidarObstacle obstacle1, - LidarObstacle obstacle2) { - // TODO: This is ABSURDLY ineffecient. We're doing n^2 operations on 2 - // potentially - // TODO: very large objects. We should be able to do some sort of obstacle - // approx to - // TODO: circles or boxes and compare them that way (need to update - // LidarObstacle for that approx) - - std::vector obstacle1_points = obstacle1.getReadingsAsPoints(); - std::vector obstacle2_points = obstacle2.getReadingsAsPoints(); - - // Compare every point to.... *shudders slightly* every other point.. - double min_distance = -1; - for (Point p1 : obstacle1_points) { - for (Point p2 : obstacle2_points) { - double dx = p1.x - p2.x; - double dy = p1.y - p2.y; - double distance = std::sqrt(std::pow(dx, 2.0) + std::pow(dy, 2.0)); - if (min_distance < 0 || distance < min_distance) - min_distance = distance; - }; - }; - - return min_distance; -} - -std::vector LidarObstacleManager::getConeLines() { - // Get all our cones as points - std::vector points; - for (LidarObstacle obstacle : obstacles) { - if (obstacle.getObstacleType() == CONE) { - points.emplace_back(obstacle.getCenter()); - } - } - - // Get groups of lines - std::vector> groups = - getPointGroupings(points, cone_grouping_tolerance); - - // Fit a line of best fit to each group - std::vector lines; - for (std::vector group : groups) - if (group.size() >= 2) lines.emplace_back(getLineOfBestFit(group)); - - return lines; -} - -std::vector> -LidarObstacleManager::getPointGroupings(std::vector points, - double tolerance) { - std::vector> groups; - - // Go through every point - while (points.size() > 0) { - // Start the current group off with the last point - std::vector group; - std::stack to_visit; - to_visit.emplace(points.back()); - points.pop_back(); - while (to_visit.size() > 0) { - // Visit the first point in to_visit - Point curr_point = to_visit.top(); - to_visit.pop(); - - // Figure out if there are any points within tolerance of the point - // we're visiting - for (int i = 0; i < points.size(); i++) { - Point p = points[i]; - if (distanceBetweenPoints(curr_point, p) < tolerance) { - // Add point to to_visit and remove from the given list of - // points - to_visit.emplace(p); - points.erase(points.begin() + i); - } - } - - // Add the current point to the group - group.emplace_back(curr_point); - - // Keep going if we've got more points to visit - } - - groups.emplace_back(group); - } - - return groups; -} - -LineOfBestFit -LidarObstacleManager::getLineOfBestFit(const std::vector& points) { - // Get line of best fit using linear regression formula - // http://www.statisticshowto.com/how-to-find-a-linear-regression-equation/ - - // TODO: Handle this more nicely - // WE SHOULD NEVER GET HERE - BOOST_ASSERT(points.size() >= 2); - - double x_sum = std::accumulate( - points.begin(), points.end(), 0.0, [](double accum, Point p) { - return accum + p.x; - }); - double y_sum = std::accumulate( - points.begin(), points.end(), 0.0, [](double accum, Point p) { - return accum + p.y; - }); - double x_squared_sum = std::accumulate( - points.begin(), points.end(), 0.0, [](double accum, Point p) { - return accum + std::pow(p.x, 2.0); - }); - double x_y_product_sum = std::accumulate( - points.begin(), points.end(), 0.0, [](double accum, Point p) { - return accum + p.x * p.y; - }); - double y_intercept = (y_sum * x_squared_sum - x_sum * x_y_product_sum) / - (points.size() * x_squared_sum - std::pow(x_sum, 2.0)); - double slope = (points.size() * x_y_product_sum - x_sum * y_sum) / - (points.size() * x_squared_sum - std::pow(x_sum, 2.0)); - - // Calculate means - double x_mean = x_sum / points.size(); - double y_mean = y_sum / points.size(); - - // Calculate Variances - double x_var = - std::accumulate(points.begin(), - points.end(), - 0.0, - [x_mean](double accum, Point p) { - return accum + std::pow((x_mean - p.x), 2.0); - }) / - (points.size() - 1); - double y_var = - std::accumulate(points.begin(), - points.end(), - 0.0, - [y_mean](double accum, Point p) { - return accum + std::pow((y_mean - p.y), 2.0); - }) / - (points.size() - 1); - // Calculate standard deviations - double x_sd = sqrt(x_var); - double y_sd = sqrt(y_var); - - // Calculate correlation coefficient using Slope = r*Sy/Sx - double r = slope * x_sd / y_sd; - - return LineOfBestFit(slope, y_intercept, r); -} - -LineOfBestFit LidarObstacleManager::getBestLine(bool lineToTheRight) { - LineOfBestFit bestLine(0, 0, 0); - - std::vector lines = getConeLines(); - - for (size_t i = 0; i < lines.size(); i++) { - // Only check lines where the y-intercept is on the correct side. - if ((lineToTheRight && lines[i].getYIntercept() < 0) || - (!lineToTheRight && lines[i].getYIntercept() >= 0)) { - // Don't accept lines that are too far from the robot. - if (fabs(lines[i].getYIntercept()) <= - max_distance_from_robot_accepted) { - // If correlation is stronger than the current best, update best - // line. - if (fabs(lines[i].correlation) > fabs(bestLine.correlation)) - bestLine = lines[i]; - } - } - } - return bestLine; -} - -visualization_msgs::Marker LidarObstacleManager::getConeRVizMarker() { - visualization_msgs::Marker points; - - // TODO: we shold differenetiat between walls and cones - - // TODO: Should be a param (currently in the default LaserScan frame) - points.header.frame_id = "laser"; - points.header.stamp = ros::Time::now(); - points.ns = "debug"; - points.action = visualization_msgs::Marker::ADD; - points.pose.orientation.w = 1.0; - points.id = 0; - // TODO: Make these round, not blocks - points.type = visualization_msgs::Marker::POINTS; - // TODO: Make the scale a param - points.scale.x = 0.1; - points.scale.y = 0.1; - // Points are green - // TODO: Make this a param - points.color.g = 1.0f; - points.color.a = 1.0; - - for (LidarObstacle obstacle : obstacles) { - if (obstacle.getObstacleType() == CONE) { - Point center = obstacle.getCenter(); - geometry_msgs::Point geom_point; - geom_point.x = center.x; - geom_point.y = center.y; - points.points.push_back(geom_point); - } - } - - return points; -} - -visualization_msgs::Marker LidarObstacleManager::getConeLinesRVizMarker() { - visualization_msgs::Marker lines; - - // TODO: Should be a param (currently in the default LaserScan frame) - lines.header.frame_id = "laser"; - lines.header.stamp = ros::Time::now(); - lines.ns = "debug"; - lines.action = visualization_msgs::Marker::ADD; - lines.pose.orientation.w = 1.0; - lines.id = 0; - lines.type = visualization_msgs::Marker::LINE_LIST; - lines.action = visualization_msgs::Marker::ADD; - // TODO: Make the scale a param - lines.scale.x = 0.1; - lines.scale.y = 0.1; - // TODO: Make this a param - lines.color.r = 1.0f; - lines.color.a = 1.0; - - // Get the cone lines - std::vector cone_lines = getConeLines(); - for (int i = 0; i < cone_lines.size(); i++) { - // Get two points to represent the line - geometry_msgs::Point p1, p2; - p1.x = -10; - p1.y = cone_lines[i].getYCoorAtX(p1.x); - // TODO: make length here a param - p2.x = 10; - p2.y = cone_lines[i].getYCoorAtX(p2.x); - lines.points.push_back(p1); - lines.points.push_back(p2); - } - return lines; -} - -// TODO: See comments in above visualisation functions -visualization_msgs::Marker -LidarObstacleManager::getBestConeLineRVizMarker(bool line_to_the_right) { - // We'll just put a single line in this - visualization_msgs::Marker line; - - // TODO: Should be a param (currently in the default LaserScan frame) - line.header.frame_id = "laser"; - line.header.stamp = ros::Time::now(); - line.ns = "debug"; - line.action = visualization_msgs::Marker::ADD; - line.pose.orientation.w = 1.0; - line.id = 0; - line.type = visualization_msgs::Marker::LINE_LIST; - line.action = visualization_msgs::Marker::ADD; - // TODO: Make the scale a param - line.scale.x = 0.1; - line.scale.y = 0.1; - // TODO: Make this a param - line.color.b = 1.0f; - line.color.a = 1.0; - - // TODO: Find a better way to sync this logic up. Copy pasting sucks - LineOfBestFit best_line = getBestLine(line_to_the_right); - // If no line found on desired side - if (best_line.correlation == 0) best_line = getBestLine(!line_to_the_right); - geometry_msgs::Point p1, p2; - p1.x = -10; - p1.y = best_line.getYCoorAtX(p1.x); - p2.x = 10; - p2.y = best_line.getYCoorAtX(p2.x); - line.points.push_back(p1); - line.points.push_back(p2); - - return line; -} - -bool LidarObstacleManager::collisionDetected() { - return collision_detected; -} diff --git a/src/drag_race_iarrc/src/drag_race_node.cpp b/src/drag_race_iarrc/src/drag_race_node.cpp deleted file mode 100644 index fbeb5bb3b..000000000 --- a/src/drag_race_iarrc/src/drag_race_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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 = "drag_race_node"; - - // Create an instance of your class - DragRaceNode my_class(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/drag_race_iarrc/test/drag-race-controller-test.cpp b/src/drag_race_iarrc/test/drag-race-controller-test.cpp deleted file mode 100644 index 45b78dd20..000000000 --- a/src/drag_race_iarrc/test/drag-race-controller-test.cpp +++ /dev/null @@ -1,177 +0,0 @@ -#include -#include -#include -#include - -/* - * Created By: Robyn Castro - * Created On: July 8, 2017 - * Description: Tests for Drag Race Controller - */ - -double more_than_target_distance = 0.0; -double less_than_target_distance = 0.65; - -// Velocity limits -double angular_vel_cap = 1.0; -double linear_vel_cap = 1.0; - -// Scaling -double theta_scaling_multiplier = 1.0 / (M_PI * 4); -double angular_speed_multiplier = 1.0; -double linear_speed_multiplier = 1.0; - -// Line values -// Slopes -double right_angle_slope = -3.0; -double left_angle_slope = 3.0; - -// X-Intercepts -double line_to_the_right = -1.0; -double line_to_the_left = 1.0; - -double correlation = 1.0; - -// TEST(Position of Line, Heading of the line, Robots Relative position to -// target distance) -TEST(LeftLineTest, angleRightMoreThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(right_angle_slope, line_to_the_left, correlation); - DragRaceController dragRaceController = - DragRaceController(more_than_target_distance, - false, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Left - EXPECT_GE(testCommand.angular.z, 0); -} - -TEST(LeftLineTest, angleLeftMoreThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(left_angle_slope, line_to_the_left, correlation); - DragRaceController dragRaceController = - DragRaceController(more_than_target_distance, - false, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Left - EXPECT_GE(testCommand.angular.z, 0); -} - -TEST(LeftLineTest, angleRightLessThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(right_angle_slope, line_to_the_left, correlation); - DragRaceController dragRaceController = - DragRaceController(less_than_target_distance, - false, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Right - EXPECT_GE(0, testCommand.angular.z); -} - -TEST(LeftLineTest, angleLeftLessThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(left_angle_slope, line_to_the_left, correlation); - DragRaceController dragRaceController = - DragRaceController(less_than_target_distance, - false, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Right - EXPECT_GE(0, testCommand.angular.z); -} - -TEST(RightLineTest, angleRightMoreThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(right_angle_slope, line_to_the_right, correlation); - DragRaceController dragRaceController = - DragRaceController(more_than_target_distance, - true, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Right - EXPECT_GE(0, testCommand.angular.z); -} - -TEST(RightLineTest, angleLeftMoreThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(left_angle_slope, line_to_the_right, correlation); - DragRaceController dragRaceController = - DragRaceController(more_than_target_distance, - true, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Right - EXPECT_GE(0, testCommand.angular.z); -} - -TEST(RightLineTest, angleRightLessThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(right_angle_slope, line_to_the_right, correlation); - DragRaceController dragRaceController = - DragRaceController(less_than_target_distance, - true, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Left - EXPECT_GE(testCommand.angular.z, 0); -} - -TEST(RightLineTest, angleLeftLessThanTargetDistance) { - LineOfBestFit testLine = - LineOfBestFit(left_angle_slope, line_to_the_right, correlation); - DragRaceController dragRaceController = - DragRaceController(less_than_target_distance, - true, - theta_scaling_multiplier, - angular_speed_multiplier, - linear_speed_multiplier, - angular_vel_cap, - linear_vel_cap); - geometry_msgs::Twist testCommand = - dragRaceController.determineDesiredMotion(testLine, false); - // Turn Left - EXPECT_GE(testCommand.angular.z, 0); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/drag_race_iarrc/test/lidar-obstacle-manager-test.cpp b/src/drag_race_iarrc/test/lidar-obstacle-manager-test.cpp deleted file mode 100644 index a5d74a87a..000000000 --- a/src/drag_race_iarrc/test/lidar-obstacle-manager-test.cpp +++ /dev/null @@ -1,275 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: July 4, 2017 - * Description: TODO - */ - -#include -#include - -class LidarObstacleManagerTest : public testing::Test { - protected: - LidarObstacleManagerTest() : obstacle_manager_1() {} - - virtual void SetUp() { - // Setup our cones - double angle_increment = M_PI / 300; - cone1 = LidarObstacle({ - {angle_increment * 0, 0.75}, - {angle_increment * 1, 0.75}, - {angle_increment * 2, 0.75}, - {angle_increment * 3, 0.75}, - {angle_increment * 4, 0.75}, - {angle_increment * 5, 0.75}, - {angle_increment * 6, 0.75}, - {angle_increment * 7, 0.75}, - }); - cone2 = LidarObstacle({ - {angle_increment * 88, 1.25}, - {angle_increment * 89, 1.25}, - {angle_increment * 90, 1.25}, - {angle_increment * 91, 1.25}, - {angle_increment * 92, 1.25}, - }); - cone3 = LidarObstacle({ - {angle_increment * 115, 2.13}, - {angle_increment * 115, 2.13}, - {angle_increment * 115, 2.13}, - }); - cone4 = LidarObstacle({ - {angle_increment * 126, 3.09}, {angle_increment * 127, 3.09}, - }); - - // Create a fake lidar scan - // TODO: Maybe a slightly more detailed description - // scan1 simulates having 4 cones at 1 meter increments from the robots - // current location, - // 0.75 meters to the left of the robot. Number of hits per-cone and the - // distance of hits - // has been properly calculated, and should be representative for a - // lidar with 300 hits - // over a 180 degree view - ulong num_rays = 300; - scan1.angle_min = (float) -M_PI / 2; - scan1.angle_max = (float) M_PI / 2; - scan1.angle_increment = (scan1.angle_max - scan1.angle_min) / num_rays; - // Set all the ranges to 0 initially - scan1.ranges = std::vector(num_rays, 0); - scan1.range_min = 0.02; - scan1.range_max = 5.6; - // Add the obstacles - std::fill( - scan1.ranges.begin(), scan1.ranges.begin() + 8, 0.75); // cone1 - std::fill( - scan1.ranges.begin() + 88, scan1.ranges.begin() + 93, 1.25); // cone2 - std::fill( - scan1.ranges.begin() + 115, scan1.ranges.begin() + 118, 2.13); // cone3 - std::fill( - scan1.ranges.begin() + 126, scan1.ranges.begin() + 128, 3.09); // cone4 - - // Make some ranges outside the min and max of the scan - std::fill( - scan1.ranges.begin() + 150, scan1.ranges.begin() + 160, 45.123); - std::fill(scan1.ranges.begin() + 50, scan1.ranges.begin() + 70, 0.01); - } - - sensor_msgs::LaserScan scan1; - LidarObstacle cone1, cone2, cone3, cone4; - LidarObstacleManager obstacle_manager_1; -}; - -TEST_F(LidarObstacleManagerTest, minDistanceBetweenObstaclesTest) { - LidarObstacle obstacle1(M_PI / 2, 1); - LidarObstacle obstacle2(-M_PI / 2, 1); - - EXPECT_DOUBLE_EQ( - 2, LidarObstacleManager::minDistanceBetweenObstacles(obstacle1, obstacle2)); - - // Distance between cone1 and cone2 should be about a meter - double distance = - LidarObstacleManager::minDistanceBetweenObstacles(cone1, cone2); - EXPECT_LE(0.93, distance); - EXPECT_GE(1.07, distance); -} - -TEST_F(LidarObstacleManagerTest, minDistanceBetweenObstaclesTest2) { - LidarObstacle obstacle1({{0, 1}, {M_PI / 2, 100}}); - LidarObstacle obstacle2({{M_PI, 1}, {-M_PI / 2, 100}}); - - EXPECT_DOUBLE_EQ( - 2.0, - LidarObstacleManager::minDistanceBetweenObstacles(obstacle1, obstacle2)); -} - -TEST_F(LidarObstacleManagerTest, addObstacleTest) { - // Check that adding a single obstacle works - obstacle_manager_1.addObstacle(cone1); - std::vector obstacles = obstacle_manager_1.getObstacles(); - ASSERT_EQ(1, obstacles.size()); - EXPECT_EQ(cone1.getCenter(), obstacles[0].getCenter()); - - // Add a couple more obstacles and check that we've got the expected number - // of obstacles - obstacle_manager_1.addObstacle(cone2); - obstacle_manager_1.addObstacle(cone3); - obstacle_manager_1.addObstacle(cone4); - obstacles = obstacle_manager_1.getObstacles(); - EXPECT_EQ(4, obstacles.size()); -} - -TEST_F(LidarObstacleManagerTest, addDuplicateObstacleTest) { - // We should automatically merge any duplicate obstacles that we get - obstacle_manager_1.addObstacle(cone1); - obstacle_manager_1.addObstacle(cone1); - EXPECT_EQ(1, obstacle_manager_1.getObstacles().size()); -} - -TEST_F(LidarObstacleManagerTest, addLaserScanTest) { - obstacle_manager_1.addLaserScan(scan1); - std::vector obstacles = obstacle_manager_1.getObstacles(); - - EXPECT_EQ(4, obstacles.size()); -} - -TEST_F(LidarObstacleManagerTest, getPointGroupingsTest) { - std::vector points = { - // First group - {0, 0}, - {0, 1}, - {1, 1}, - // Point just out of reach of the first group - {1, 2.2}, - // Second group - {10, 0}, - {11, 0}, - // Random point off in the distance - {15, 15}, - }; - std::vector> groups = - obstacle_manager_1.getPointGroupings(points, 1.1); - // We expect 4 groups of points - ASSERT_EQ(4, groups.size()); - - // Get the group sizes - std::vector sizes; - sizes.resize(groups.size()); - std::transform(groups.begin(), groups.end(), sizes.begin(), [](auto group) { - return group.size(); - }); - // We don't care what order we get the groups in, so sort in ascending order - // (`getPointGroupings` returning groups in a different order should not - // break this test) - std::sort(sizes.begin(), sizes.end()); - EXPECT_EQ(std::vector({1, 1, 2, 3}), sizes); -} - -TEST_F(LidarObstacleManagerTest, getLineOfBestFitPositiveSlopeTest) { - std::vector points = { - {0, 0}, {1, 0.5}, {2, 1}, - }; - LineOfBestFit line = LidarObstacleManager::getLineOfBestFit(points); - EXPECT_DOUBLE_EQ(0, line.getXIntercept()); - EXPECT_DOUBLE_EQ(0.5, line.getSlope()); - EXPECT_DOUBLE_EQ(1, line.correlation); -} - -TEST_F(LidarObstacleManagerTest, getLineOfBestFitNegativeSlopeTest) { - std::vector points = { - {0, 1}, {1, 0.5}, {2, 0}, - }; - LineOfBestFit line = LidarObstacleManager::getLineOfBestFit(points); - EXPECT_DOUBLE_EQ(2, line.getXIntercept()); - EXPECT_DOUBLE_EQ(-0.5, line.getSlope()); - EXPECT_DOUBLE_EQ(-1, line.correlation); -} - -TEST_F(LidarObstacleManagerTest, getLineOfBestFitRandomPoints) { - std::vector points = { - {0, 1}, {1, 0.5}, {2, 2}, {3, -4}, {3, -5}, - }; - LineOfBestFit line = LidarObstacleManager::getLineOfBestFit(points); - EXPECT_DOUBLE_EQ(2.2352941176470589, line.getYIntercept()); - EXPECT_DOUBLE_EQ(-1.8529411764705883, line.getSlope()); - EXPECT_DOUBLE_EQ(-0.76208438348419327, line.correlation); -} - -TEST_F(LidarObstacleManagerTest, getLineOfBestFitRandomPoints2) { - std::vector points = { - {0, -1}, {1, -0.5}, {2, 2.3}, {3, -3}, {3, -8}, {4, 9}, {5, 100}}; - LineOfBestFit line = LidarObstacleManager::getLineOfBestFit(points); - EXPECT_DOUBLE_EQ(-22.617741935483881, line.getYIntercept()); - EXPECT_DOUBLE_EQ(14.284677419354843, line.getSlope()); - EXPECT_DOUBLE_EQ(0.64214088713774, line.correlation); -} - -TEST_F(LidarObstacleManagerTest, getLineOfBestFitRandomPoints3) { - std::vector points = { - {12.13, 41.44}, - {7865.653, 6234.54}, - {0, -100}, - {10, 20}, - {9699.454, 10344.334}, - }; - LineOfBestFit line = LidarObstacleManager::getLineOfBestFit(points); - EXPECT_DOUBLE_EQ(0.96926393150820644, line.getSlope()); - EXPECT_DOUBLE_EQ(-101.27209579731976, line.getYIntercept()); - EXPECT_DOUBLE_EQ(0.98463989293624832, line.correlation); -} - -// TODO -// TEST(SlopeInterceptLineTest, testGetXIntercept){} - -// TODO: Test, Ideally stopping should be a SEPARATE class, move it out later -TEST_F(LidarObstacleManagerTest, noObstacleInFront) { - // LidarObstacleManager man = LidarObstacleManager(0.3, 1.3, 1.5, 0.4, 1.0, - // 1.5); - // man.addLaserScan(scan1); - // EXPECT_FALSE(man.collisionDetected()); -} - -// TODO: Test -TEST_F(LidarObstacleManagerTest, obstacleInFront) { - sensor_msgs::LaserScan end_zone_scan; - - ulong num_rays = 300; - end_zone_scan.angle_min = (float) -M_PI / 2; - end_zone_scan.angle_max = (float) M_PI / 2; - end_zone_scan.angle_increment = - (end_zone_scan.angle_max - end_zone_scan.angle_min) / num_rays; - // Set all the ranges to 0 initially - end_zone_scan.ranges = std::vector(num_rays, 0); - end_zone_scan.range_min = 0.02; - end_zone_scan.range_max = 5.6; - - // Add the end zone1 - std::fill(end_zone_scan.ranges.begin(), - end_zone_scan.ranges.begin() + 8, - 0.75); // cone1 - std::fill(end_zone_scan.ranges.begin() + 88, - end_zone_scan.ranges.begin() + 93, - 1.25); // cone2 - std::fill(end_zone_scan.ranges.begin() + 115, - end_zone_scan.ranges.begin() + 118, - 2.13); // cone3 - std::fill(end_zone_scan.ranges.begin() + 126, - end_zone_scan.ranges.begin() + 128, - 3.09); // cone4 - - // Make some ranges outside the min and max of the scan - std::fill(end_zone_scan.ranges.begin() + 150, - end_zone_scan.ranges.begin() + 160, - 45.123); - std::fill( - end_zone_scan.ranges.begin() + 50, end_zone_scan.ranges.begin() + 70, 0.01); - - // LidarObstacleManager man = LidarObstacleManager(0.3, 1.3, 1.5, 0.4, 1.0, - // 0.5); - // man.addLaserScan(scan1); - // EXPECT_TRUE(man.collisionDetected()); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/drag_race_iarrc/test/lidar-obstacle-test.cpp b/src/drag_race_iarrc/test/lidar-obstacle-test.cpp deleted file mode 100644 index fc8f85fc3..000000000 --- a/src/drag_race_iarrc/test/lidar-obstacle-test.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/* - * Created By: Gareth Ellis - * Created On: September 22, 2016 - * Description: Tests for LidarDecision and LidarObstacle - */ - -// Snowbots Includes -#include - -// Library Includes -#include - -// STD Includes -#include - -class LidarObstacleTest : public testing::Test { - protected: - virtual void SetUp() { - readings1 = {{4, 44}, {3, 99}, {6, 2}}; - readings2 = {{-1, 2}, {10, 4}, {-15, 6}}; - - obstacle1 = LidarObstacle(0.1, 10); - obstacle2 = LidarObstacle(0.2, 20); - obstacle3 = LidarObstacle(0.15, 15); - obstacle4 = LidarObstacle(readings1); - obstacle5 = LidarObstacle(readings2); - } - - std::vector readings1, readings2; - LidarObstacle obstacle1, obstacle2, obstacle3, obstacle4, obstacle5; -}; - -// Test all constructors to see if variables are set -// and default correctly -TEST_F(LidarObstacleTest, ContstructorMinWallLengthDefaultTest) { - LidarObstacle empty_constructor; - LidarObstacle min_length_constructor(50); - LidarObstacle angle_distance_constructor(10, 20); - LidarObstacle length_angle_constructor(51, 10, 20); - LidarObstacle vector_constructor({Reading{10, 20}, Reading{30, 40}}); - LidarObstacle length_vector_constructor(52, - {Reading{10, 20}, Reading{30, 40}}); - - int default_min_wall_length = 1; - EXPECT_EQ(default_min_wall_length, empty_constructor.getMinWallLength()); - EXPECT_EQ(50, min_length_constructor.getMinWallLength()); - EXPECT_EQ(default_min_wall_length, - angle_distance_constructor.getMinWallLength()); - EXPECT_EQ(51, length_angle_constructor.getMinWallLength()); - EXPECT_EQ(default_min_wall_length, vector_constructor.getMinWallLength()); - EXPECT_EQ(52, length_vector_constructor.getMinWallLength()); - - // CLion doesn't like these lines, but it compiles fine. - EXPECT_EQ(NONE, empty_constructor.getObstacleType()); - EXPECT_EQ(NONE, min_length_constructor.getObstacleType()); -} - -TEST_F(LidarObstacleTest, ConstructorTest1) { - auto readings = obstacle1.getAllLaserReadings(); - EXPECT_EQ(1, readings.size()); - EXPECT_EQ(10, readings[0].range); - EXPECT_NEAR(0.1, readings[0].angle, 0.000001); -} - -TEST_F(LidarObstacleTest, ConstructorTest2) { - auto readings = obstacle4.getAllLaserReadings(); - // Check that the correct number of readings were input - EXPECT_EQ(3, readings.size()); - // Check that they are in the correct order - // (sorted in ascending order by angle) - EXPECT_EQ(3, readings[0].angle); - EXPECT_EQ(4, readings[1].angle); - EXPECT_EQ(6, readings[2].angle); -} - -TEST_F(LidarObstacleTest, getAvgAngleTest) { - EXPECT_NEAR(0.1, obstacle1.getAvgAngle(), 0.00001); - EXPECT_NEAR(4.333333333, obstacle4.getAvgAngle(), 0.00001); - EXPECT_NEAR(-2, obstacle5.getAvgAngle(), 0.00001); -} - -TEST_F(LidarObstacleTest, mergeInReadingsTest) { - obstacle1.mergeInLidarObstacle(obstacle2); - obstacle1.mergeInLidarObstacle(obstacle3); - auto readings = obstacle1.getAllLaserReadings(); - // Check that the merge obstacle has the correct number of readings - EXPECT_EQ(3, readings.size()); - // Check that they are in the correct order - // (sorted in ascending order by angle) - EXPECT_NEAR(0.1, readings[0].angle, 0.000001); - EXPECT_NEAR(0.15, readings[1].angle, 0.000001); - EXPECT_NEAR(0.2, readings[2].angle, 0.000001); -} - -TEST_F(LidarObstacleTest, determineObstacleTypeTest) { - LidarObstacle empty_obstacle = LidarObstacle(); - LidarObstacle cone_obstacle = LidarObstacle(1, {{0, 1}, {M_PI / 6, 1}}); - LidarObstacle wall_obstacle = LidarObstacle(1, {{0, 1}, {M_PI / 2, 1}}); - - // CLion doesn't like this line, but it compiles fine. - EXPECT_EQ(NONE, empty_obstacle.getObstacleType()); - EXPECT_EQ(CONE, cone_obstacle.getObstacleType()); - EXPECT_EQ(WALL, wall_obstacle.getObstacleType()); - - // Change the NONE to a CONE - empty_obstacle.mergeInLidarObstacle(cone_obstacle); - EXPECT_EQ(CONE, empty_obstacle.getObstacleType()); -} - -TEST_F(LidarObstacleTest, getLengthTest) { - LidarObstacle long_wall({{-M_PI / 4, 1}, {-M_PI / 6, 20}, {M_PI / 4, 1}}); - LidarObstacle cone({{0, 1}, {M_PI / 3, 1}}); - - EXPECT_DOUBLE_EQ(std::sqrt(2), long_wall.getLength()); - EXPECT_DOUBLE_EQ(1, cone.getLength()); -} - -TEST_F(LidarObstacleTest, getReadingsAsPoints) { - LidarObstacle test_obstacle({{-M_PI / 4, 0.5}, {M_PI * 0.7, 0.9}}); - std::vector test_points = { - {0.3535533905932738, -0.35355339059327373}, - {-0.5290067270632258, 0.7281152949374528}}; - std::vector test_obstacle_points = - test_obstacle.getReadingsAsPoints(); - EXPECT_DOUBLE_EQ(test_points[0].x, test_obstacle_points[0].x); - EXPECT_DOUBLE_EQ(test_points[0].y, test_obstacle_points[0].y); - EXPECT_DOUBLE_EQ(test_points[1].x, test_obstacle_points[1].x); - EXPECT_DOUBLE_EQ(test_points[1].y, test_obstacle_points[1].y); -} - -TEST_F(LidarObstacleTest, getCenterTest) { - LidarObstacle test_obstacle = LidarObstacle(0, 1); - Point center = test_obstacle.getCenter(); - EXPECT_DOUBLE_EQ(1.0, center.x); - EXPECT_DOUBLE_EQ(0.0, center.y); - LidarObstacle merge_me = LidarObstacle(M_PI / 2, 1); - test_obstacle.mergeInLidarObstacle(merge_me); - center = test_obstacle.getCenter(); - EXPECT_DOUBLE_EQ(0.5, center.x); - EXPECT_DOUBLE_EQ(0.5, center.y); -} - -TEST(PointTest, distanceBetweenPointsTest) { - Point p1{-0.35312, 341.1231251134}, p2{345345.242, -323423.23}; - - EXPECT_DOUBLE_EQ(473378.21709845297, distanceBetweenPoints(p1, p2)); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/drivers/CMakeLists.txt b/src/drivers/CMakeLists.txt index 5ca575c93..a821686af 100644 --- a/src/drivers/CMakeLists.txt +++ b/src/drivers/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs sb_utils ) -find_library(libserial_LIBRARIES libserial.so.0) +find_library(libserial_LIBRARIES libserial.so.1) catkin_package( INCLUDE_DIRS include diff --git a/src/drivers/include/SteeringDriver.h b/src/drivers/include/SteeringDriver.h index 8c0f7017e..8219f4a7d 100644 --- a/src/drivers/include/SteeringDriver.h +++ b/src/drivers/include/SteeringDriver.h @@ -20,7 +20,7 @@ #include // Other -#include +#include class SteeringDriver { public: diff --git a/src/drivers/src/SteeringDriver.cpp b/src/drivers/src/SteeringDriver.cpp index a9f120b82..784e9914c 100644 --- a/src/drivers/src/SteeringDriver.cpp +++ b/src/drivers/src/SteeringDriver.cpp @@ -61,8 +61,8 @@ SteeringDriver::SteeringDriver(int argc, char** argv, std::string node_name) { // received a message // Open the given serial port arduino.Open(port); - arduino.SetBaudRate(LibSerial::SerialStreamBuf::BAUD_9600); - arduino.SetCharSize(LibSerial::SerialStreamBuf::CHAR_SIZE_8); + arduino.SetBaudRate(LibSerial::BaudRate::BAUD_9600); + arduino.SetCharacterSize(LibSerial::CharacterSize::CHAR_SIZE_8); } void SteeringDriver::twistCallback( diff --git a/src/init_camera/CMakeLists.txt b/src/init_camera/CMakeLists.txt new file mode 100644 index 000000000..6f3fa62b7 --- /dev/null +++ b/src/init_camera/CMakeLists.txt @@ -0,0 +1,214 @@ +cmake_minimum_required(VERSION 3.0.2) +project(init_camera) + +## Compile as C++11, supported in ROS Kinetic and newer + add_compile_options(-std=c++11) + #add_compile_definitions(-std=c++11) +## 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 + cv_bridge + image_transport + roscpp + rospy + sensor_msgs + std_msgs +) +find_package(OpenCV 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 exec_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 exec_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 +# sensor_msgs# 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 exec_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 your 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 init_camera +# CATKIN_DEPENDS cv_bridge image_transport roscpp rospy sensor_msgs std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ./include +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/init_camera.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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide + add_executable(camera_pub src/camera_pubber.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against + target_link_libraries(camera_pub + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ) + +############# +## 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 +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_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 +# catkin_add_gtest(${PROJECT_NAME}-test test/test_init_camera.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/init_camera/launch/camera_pub.launch b/src/init_camera/launch/camera_pub.launch new file mode 100644 index 000000000..f7994885b --- /dev/null +++ b/src/init_camera/launch/camera_pub.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/mapping_igvc/package.xml b/src/init_camera/package.xml similarity index 75% rename from src/mapping_igvc/package.xml rename to src/init_camera/package.xml index 88cc2bd8e..58338ac89 100644 --- a/src/mapping_igvc/package.xml +++ b/src/init_camera/package.xml @@ -1,13 +1,13 @@ - mapping_igvc + init_camera 0.0.0 - UBC Snowbots Mapping Package for IGVC + The init_camera package - gareth + rowan @@ -19,7 +19,7 @@ - + @@ -49,15 +49,23 @@ catkin - + cv_bridge + image_transport roscpp - message_generation + rospy + sensor_msgs std_msgs - + cv_bridge + image_transport roscpp - + rospy + sensor_msgs + std_msgs + cv_bridge + image_transport roscpp - message_runtime + rospy + sensor_msgs std_msgs diff --git a/src/init_camera/src/camera_pubber.cpp b/src/init_camera/src/camera_pubber.cpp new file mode 100644 index 000000000..97ef9323d --- /dev/null +++ b/src/init_camera/src/camera_pubber.cpp @@ -0,0 +1,54 @@ +#include +#include +#include +#include + + +using namespace cv; +using namespace std; + +int main(int argc, char** argv) { + // string inputWindow = "Camera"; + // namedWindow(inputWindow, CV_WINDOW_AUTOSIZE); + + VideoCapture cap(0); // captures the first camera + if (!cap.isOpened()) { + cout << "Camera cannot be opened" << endl; + return -1; + } + + ros::init(argc, argv, "camera_pubber"); + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + image_transport::Publisher pub = it.advertise("cam_1/color/image_raw", 1); + + Mat inputImage; + + ros::Rate loop_rate(30); + + while (nh.ok()) { + bool isRead = cap.read(inputImage); + if (!isRead) { + cout << "Failed to read image from camera" << endl; + break; + } + // imshow(inputWindow, inputImage); + sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", inputImage).toImageMsg(); + pub.publish(msg); + waitKey(1); + ros::spinOnce(); + loop_rate.sleep(); + } + + +} + + + + + + + + + + \ No newline at end of file diff --git a/src/localisation_igvc/CMakeLists.txt b/src/localisation_igvc/CMakeLists.txt deleted file mode 100644 index a1e5bd7b5..000000000 --- a/src/localisation_igvc/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(localisation_igvc) - -# This package is currently just used to house the EKF launch file. -# If we end up adding more in-house localisation things, they should go here diff --git a/src/localisation_igvc/README.md b/src/localisation_igvc/README.md deleted file mode 100644 index df03dafa1..000000000 --- a/src/localisation_igvc/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# localisation -This package currently just acts as a place to hold the launch file we use to launch [robot_pose_ekf](http://wiki.ros.org/robot_pose_ekf), an Extended [Kalman Filter](http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/). However, if we elect to create more in-house localisation stuff, it should go in this package. diff --git a/src/localisation_igvc/launch/ekf.launch b/src/localisation_igvc/launch/ekf.launch deleted file mode 100644 index bcc0c5645..000000000 --- a/src/localisation_igvc/launch/ekf.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/localisation_igvc/launch/gps.launch b/src/localisation_igvc/launch/gps.launch deleted file mode 100644 index 7f33abe04..000000000 --- a/src/localisation_igvc/launch/gps.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/src/localisation_igvc/launch/gps_to_odom.launch b/src/localisation_igvc/launch/gps_to_odom.launch deleted file mode 100644 index c50ca9c6a..000000000 --- a/src/localisation_igvc/launch/gps_to_odom.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - diff --git a/src/localisation_igvc/launch/localisation.launch b/src/localisation_igvc/launch/localisation.launch deleted file mode 100644 index 98dc3cf5f..000000000 --- a/src/localisation_igvc/launch/localisation.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - diff --git a/src/localisation_igvc/launch/sensors.launch b/src/localisation_igvc/launch/sensors.launch deleted file mode 100644 index 0b4a2510c..000000000 --- a/src/localisation_igvc/launch/sensors.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/src/localisation_igvc/package.xml b/src/localisation_igvc/package.xml deleted file mode 100644 index 0e104c6b3..000000000 --- a/src/localisation_igvc/package.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - localisation_igvc - 0.0.0 - Snowbots Localisation Package for IGVC - - - - - gareth - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - - phidgets_drivers - roscpp - gps_common - phidgets_imu - - - - - - - - diff --git a/src/mapping_igvc/CMakeLists.txt b/src/mapping_igvc/CMakeLists.txt deleted file mode 100644 index efc8d009c..000000000 --- a/src/mapping_igvc/CMakeLists.txt +++ /dev/null @@ -1,95 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mapping_igvc) - -# 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 -find_package(catkin REQUIRED COMPONENTS - roscpp - message_generation - std_msgs -) - -################################################ -## 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( - DIRECTORY msg - FILES ConeObstacle.msg LineObstacle.msg Point2D.msg -) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES std_msgs -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -catkin_package( - #INCLUDE_DIRS include - CATKIN_DEPENDS message_runtime std_msgs -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ${catkin_INCLUDE_DIRS} - ./include -) - -## Declare a C++ executable -# TODO - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) -# TODO - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - #gTest - # rostest - # TODO: Maybe? Not sure if we need rostests for the ObstacleManager - -endif() - diff --git a/src/mapping_igvc/msg/ConeObstacle.msg b/src/mapping_igvc/msg/ConeObstacle.msg deleted file mode 100644 index 9bd2594fb..000000000 --- a/src/mapping_igvc/msg/ConeObstacle.msg +++ /dev/null @@ -1,8 +0,0 @@ -# Time of obstacle observation, coordinate frame ID -Header header - -# The center point of the cone -Point2D center - -# The radius of the cone (in meters) -float64 radius diff --git a/src/mapping_igvc/msg/LineObstacle.msg b/src/mapping_igvc/msg/LineObstacle.msg deleted file mode 100644 index 07beb0f90..000000000 --- a/src/mapping_igvc/msg/LineObstacle.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Time of obstacle observation, coordinate frame ID -Header header - -# The coefficients representing a polynomial line -# The degree of the polynomial is determined by the number of coefficients -float64[] coefficients - -# The min and max x values for the polynomial line -# this is what makes it a line segment, rather then a never-ending line -float64 x_max -float64 x_min \ No newline at end of file diff --git a/src/mapping_igvc/msg/Point2D.msg b/src/mapping_igvc/msg/Point2D.msg deleted file mode 100644 index e16a01e78..000000000 --- a/src/mapping_igvc/msg/Point2D.msg +++ /dev/null @@ -1,3 +0,0 @@ -# A 2d point -float64 x -float64 y diff --git a/src/mapping_urc/CATKIN_IGNORE b/src/mapping_urc/CATKIN_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/mapping_urc/CMakeLists.txt b/src/mapping_urc/CMakeLists.txt deleted file mode 100644 index 9d0262a1c..000000000 --- a/src/mapping_urc/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mapping_urc) - -# 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 -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -# Bring in the `multi_resolution_graph` library -add_subdirectory(multi_resolution_graph) - -################################### -## catkin specific configuration ## -################################### -catkin_package() - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ${catkin_INCLUDE_DIRS} - ./include -) - -## Declare a C++ executable -# TODO: Issue #348 - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) -# TODO: Issue #348 - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - #TODO: gTest (Issue #348) - - #TODO: rosTest (Issue #348) - -endif() - diff --git a/src/mapping_urc/package.xml b/src/mapping_urc/package.xml deleted file mode 100644 index e281ac582..000000000 --- a/src/mapping_urc/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - mapping_urc - 0.0.0 - UBC Snowbots Mapping Package for the University Rover Competition - - gareth - - TODO - - catkin - - roscpp - message_generation - std_msgs - - roscpp - - roscpp - message_runtime - std_msgs - - diff --git a/src/marker_qr_detection/launch/arcu_detect.launch b/src/marker_qr_detection/launch/arcu_detect.launch index 2d002ed65..c12d10a63 100644 --- a/src/marker_qr_detection/launch/arcu_detect.launch +++ b/src/marker_qr_detection/launch/arcu_detect.launch @@ -1,7 +1,11 @@ +<<<<<<< HEAD +======= + +>>>>>>> origin/master diff --git a/src/marker_qr_detection/src/DetectMarker.cpp b/src/marker_qr_detection/src/DetectMarker.cpp index 8940c7849..12309c2af 100644 --- a/src/marker_qr_detection/src/DetectMarker.cpp +++ b/src/marker_qr_detection/src/DetectMarker.cpp @@ -29,7 +29,7 @@ DetectMarker::DetectMarker(int argc, char** argv, std::string node_name) { SB_getParam(private_nh, parameter_name2, camera, 1); // Setup Subscriber(s) if (camera == 1) { - topic_to_subscribe_to = "cam_1/color/raw"; + topic_to_subscribe_to = "cam_1/color/image_raw"; } else { topic_to_subscribe_to = "cam_2/color/image_raw"; } 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/pathfinding_igvc/src/path_finder.cpp b/src/move_group_arm/src/move_group.cpp similarity index 51% rename from src/pathfinding_igvc/src/path_finder.cpp rename to src/move_group_arm/src/move_group.cpp index a40695ddd..4b416fb14 100644 --- a/src/pathfinding_igvc/src/path_finder.cpp +++ b/src/move_group_arm/src/move_group.cpp @@ -1,18 +1,17 @@ /* - * Created By: Min Gyo Kim - * Created On: May 5th 2018 - * Description: Spawns path finding node + * Created By: Tate Kolton + * Created On: August 9, 2022 + * Description: Move groups interface for arm */ -#include "ros/ros.h" -#include +#include "../include/moveGroup.h" int main(int argc, char** argv) { // Setup your ROS node - std::string node_name = "path_finder"; + std::string node_name = "move_group"; // Create an instance of your class - PathFinderNode node(argc, argv, node_name); + MoveGroupArm arm(argc, argv, node_name); // Start up ros. This will continue to run until the node is killed ros::spin(); diff --git a/src/pathfinding_igvc/CMakeLists.txt b/src/pathfinding_igvc/CMakeLists.txt deleted file mode 100644 index 3b9e18bfb..000000000 --- a/src/pathfinding_igvc/CMakeLists.txt +++ /dev/null @@ -1,138 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(pathfinding_igvc) - -# 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() - - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS roscpp tf) -find_package(sb_utils REQUIRED) - -add_definitions(-std=c++14) - -catkin_package( - # INCLUDE_DIRS include -) - -########### -## 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++ executable -# add_executable(sample_package_node src/sample_package_node.cpp) -add_executable(path_finding - src/path_finding.cpp - src/PathFinding.cpp - include/PathFinding.h - ) - -add_executable(path_finder - include/AStar.h - include/FrameTransformer.h - include/OccupancyGridAdapter.h - include/OccupancyGridResizer.h - include/PathConstructor.h - include/PathFinder.h - include/PathFinderUtils.h - include/PathFinderNode.h - src/AStar.cpp - src/FrameTransformer.cpp - src/OccupancyGridAdapter.cpp - src/OccupancyGridResizer.cpp - src/PathConstructor.cpp - src/PathFinder.cpp - src/path_finder.cpp - src/PathFinderNode.cpp - ) - -## Specify libraries to link a library or executable target against -# target_link_libraries(sample_package_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(path_finding - ${catkin_LIBRARIES} - ${sb_utils_LIBRARIES} - ) - -target_link_libraries(path_finder - ${catkin_LIBRARIES} - ${sb_utils_LIBRARIES} - ) - - -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - - # Adding gtests to the package - catkin_add_gtest(path-finding-test test/path-finding-test.cpp src/PathFinding.cpp) - target_link_libraries(path-finding-test ${catkin_LIBRARIES}) - - catkin_add_gtest(path-finder-test test/path-finder-test.cpp - src/PathFinder.cpp include/PathFinder.h - src/AStar.cpp include/AStar.h - src/OccupancyGridResizer.cpp include/OccupancyGridResizer.h - src/PathConstructor.cpp include/PathConstructor.h - src/OccupancyGridAdapter.cpp include/OccupancyGridAdapter.h - src/FrameTransformer.cpp include/FrameTransformer.h - ) - target_link_libraries(path-finder-test ${catkin_LIBRARIES}) - - catkin_add_gtest(path-finder-utils-test test/path-finder-utils-test.cpp include/PathFinder.h) - target_link_libraries(path-finder-utils-test ${catkin_LIBRARIES}) - - catkin_add_gtest(astar-test test/astar-test.cpp src/AStar.cpp include/AStar.h) - target_link_libraries(astar-test ${catkin_LIBRARIES}) - - catkin_add_gtest(path-constructor-test test/path-constructor-test.cpp - src/PathConstructor.cpp include/PathConstructor.h - src/OccupancyGridAdapter.cpp include/OccupancyGridAdapter.h - src/FrameTransformer.cpp include/FrameTransformer.h - ) - target_link_libraries(path-constructor-test ${catkin_LIBRARIES}) - - catkin_add_gtest(frame-transformer-test test/frame-transformer-test.cpp - src/FrameTransformer.cpp include/FrameTransformer.h - ) - target_link_libraries(frame-transformer-test ${catkin_LIBRARIES}) - - catkin_add_gtest(occupancy-grid-adapter-test test/occupancy-grid-adapter-test.cpp - src/OccupancyGridAdapter.cpp include/OccupancyGridAdapter.h - src/FrameTransformer.cpp include/FrameTransformer.h - ) - target_link_libraries(occupancy-grid-adapter-test ${catkin_LIBRARIES}) - - catkin_add_gtest(occupancy-grid-resizer-test test/occupancy-grid-resizer-test.cpp - src/OccupancyGridResizer.cpp include/OccupancyGridResizer.h - src/OccupancyGridAdapter.cpp include/OccupancyGridAdapter.h - src/FrameTransformer.cpp include/FrameTransformer.h - ) - target_link_libraries(occupancy-grid-resizer-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(path_finder_rostest test/path_finder_igvc_test.test test/path_finder_rostest.cpp) - target_link_libraries(path_finder_rostest ${catkin_LIBRARIES}) - - add_rostest_gtest(path_finding_rostest test/pathfinding_igvc_test.test test/path_finding_rostest.cpp) - target_link_libraries(path_finding_rostest ${catkin_LIBRARIES}) -endif() - diff --git a/src/pathfinding_igvc/include/AStar.h b/src/pathfinding_igvc/include/AStar.h deleted file mode 100644 index 723b31051..000000000 --- a/src/pathfinding_igvc/include/AStar.h +++ /dev/null @@ -1,193 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 9th 2018 - * Description: Class that implements A* algorithm to find the shortest path - * between two points given an occupancy grid. Basically copies the algorithm - * from - * https://www.geeksforgeeks.org/a-search-algorithm/ - */ - -#ifndef PATHFINDING_IGVC_ASTAR_H -#define PATHFINDING_IGVC_ASTAR_H - -#include -#include -#include - -class AStar { - public: - /* - * Assumes the value at a cell of occupancy grid is either GRID_FREE - * or GRID_OCCUPIED - */ - static const int GRID_FREE = 0; - static const int GRID_OCCUPIED = 100; - - /* - * A representation of a point in grid with its col and row - */ - struct GridPoint { - int col; - int row; - GridPoint(int r = 0, int c = 0) : col(c), row(r){}; - }; - - /** - * Takes an occupancy grid as well as start and goal points, and calculates - * the shortest path from start to goal. - * - * @param occupancy_grid occupancy grid - * @param start GridPoint containing row and column of the starting cell - * @param goal GridPoint containing row and column of the goal cell - * @return points stacked in order, where the top contains the starting - * GridPoint and the bottom contains the goal GridPoint - */ - static std::stack run(nav_msgs::OccupancyGrid occupancy_grid, - GridPoint start, - GridPoint goal); - - private: - /* - * A representation of a point in grid along with its score. - * Its score, f, is equal to g+h, where: - * g is "the movement cost from the starting point" to this point - * h is "the estimated movement cost to move from this point to - * the final destination". - */ - typedef std::pair> GridPointWithScore; - - /* - * A structure that holds the necessary details for a cell - * It stores its parent as well as its score parameters, - * f, g, and h. - */ - struct CellDetail { - // parent of cell - GridPoint parent; - // f = g + h - double f = FLT_MAX, g = FLT_MAX, h = FLT_MAX; - }; - - /* - * parameters for storing the size of the grid, - * obtained from the occupancy grid metadata - */ - int _num_rows; - int _num_cols; - - /* - * parameters for storing the starting and goal - * points in the grid - */ - GridPoint _start; - GridPoint _goal; - - /* - * stores the grid, accessible to all functions - * of this class - */ - std::vector _grid; - - /* - * _cell_details stores the CellDetail for every - * cell in the grid - */ - std::vector> _cell_details; - - /* - * _closed_list stores whether or not a cell has - * already been visited for every cell in the grid - */ - std::vector> _closed_list; - - /* - * _open_list stores which cells to visit - */ - std::set _open_list; - - /** - * Initializes the class with the given occupancy_grid and start and - * goal grid points - * - * The constructor is private because a path would only be found once - * for a given occupancy grid and start and goal points. It's also a - * way to force initialization of the class variables every time we - * run the algorithm. - * - * @param occupancy_grid occupancy grid - * @param start - * @param goal - */ - AStar(nav_msgs::OccupancyGrid occupancy_grid, - GridPoint start, - GridPoint goal); - - /** - * Performs A* search and returns the path in a stack. - * The occupancy grid as well as start and goal points are accessed - * through the class' instance variables. - * - * @return - */ - std::stack search(); - - /** - * Returns whether or not a point is inside the occupancy grid - * - * @param point - * @return true if point is inside the grid, false otherwise - */ - bool isValid(AStar::GridPoint point); - - /** - * Returns whether or not a point on grid is blocked - * - * @param point - * @return true if point on grid is GRID_FREE, false if GRID_OCCUPIED - */ - bool isUnBlocked(AStar::GridPoint point); - - /** - * Returns whether or not a point on grid is the destination - * (i.e. this->_goal) - * - * @param point - * @return true if point == this->_goal, false otherwise - */ - bool isDestination(AStar::GridPoint point); - - /** - * Calculates h value of a point in the grid using euclidean distance. - * h is "the estimated movement cost to move from this point to - * the final destination". - * - * @param point - * @return h value - */ - double calculateHValue(AStar::GridPoint point); - - /** - * Function that returns the path once the destination is found. - * Returns a stack of grid points where the top is the starting - * point and the bottom is the goal. - * - * @return path - */ - std::stack tracePath(); - - /** - * Function that takes in a potential successor of a point (parent). - * A point becomes a successor of the parent point if it is the goal, - * or the point has never been visited before, or the point has a higher - * score (f = g + h) than it was previously visited. - * - * Returns true if successor is the goal, otherwise false. - * - * @param successor potential successor of parent - * @param parent - * @return - */ - bool processSuccessor(GridPoint successor, GridPoint parent); -}; - -#endif // PATHFINDING_IGVC_ASTAR_H diff --git a/src/pathfinding_igvc/include/FrameTransformer.h b/src/pathfinding_igvc/include/FrameTransformer.h deleted file mode 100644 index 9bfc81454..000000000 --- a/src/pathfinding_igvc/include/FrameTransformer.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 13th 2018 - * Description: A class that transforms points between two frames, - * specifically map and grid frame. - */ - -#ifndef PATHFINDING_IGVC_FRAMETRANSFORMER_H -#define PATHFINDING_IGVC_FRAMETRANSFORMER_H - -#include -#include -#include -#include - -class FrameTransformer { - tf::Transform _transformation_to_grid; - tf::Transform _transformation_to_map; - - public: - /** - * Returns a FrameTransformer that transforms points - * between grid frame and map frame - * - * Takes in the rotation/orientation and position of the grid frame relative - * map frame - * - * @param rotation rotation/orientation of the grid frame relative expressed - * map frame - * @param position position of the grid frame expressed in map frame - * @return FrameTransformer - */ - FrameTransformer(tf::Quaternion rotation, tf::Vector3 position); - - /** - * Takes a point represented in map frame and returns its position in grid - * frame - * - * @param point a point represented in map frame - * @return point represented in grid frame - */ - geometry_msgs::Point - transformFromMapToGridFrame(geometry_msgs::Point point); - - /** - * Takes a point represented in grid frame and returns its position in map - * frame - * - * @param point a point represented in grid frame - * @return point represented in map frame - */ - geometry_msgs::Point - transformFromGridToMapFrame(geometry_msgs::Point point); -}; - -#endif // PATHFINDING_IGVC_FRAMETRANSFORMER_H diff --git a/src/pathfinding_igvc/include/OccupancyGridAdapter.h b/src/pathfinding_igvc/include/OccupancyGridAdapter.h deleted file mode 100644 index 2c507a793..000000000 --- a/src/pathfinding_igvc/include/OccupancyGridAdapter.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 13th 2018 - * Description: A class that calculates the location of a point in map frame - * in the occupancy grid, and vice versa. - * This class depends on FrameTransformer. - */ - -#ifndef PATHFINDING_IGVC_OCCUPANCYGRIDADAPTER_H -#define PATHFINDING_IGVC_OCCUPANCYGRIDADAPTER_H - -#include -#include -#include - -class OccupancyGridAdapter { - nav_msgs::MapMetaData _grid_info; - FrameTransformer* _frame_transformer; - - public: - /** - * Takes in occupancy grid meta data and creates a class - * that calculates the location of a point represented in map frame in - * occupancy grid - * - * @param info map meta data of occupancy grid - * @return OccupancyGridAdapter - */ - OccupancyGridAdapter(nav_msgs::MapMetaData info); - - /** - * Takes in a point represented in map frame and returns a struct - * that contains the corresponding row and column of the point in the - * occupancy grid. - * - * @param point a point represented in map frame - * @return AStar::GridPoint containing row and column of the point in grid - */ - AStar::GridPoint convertFromMapToGridPoint(geometry_msgs::Point point); - - /** - * Takes in a struct containing the row and column of a point in the - * occupancy grid and returns its representation in map frame - * - * @param AStar::GridPoint containing row and column of a point in grid - * @return point represented in map frame - */ - geometry_msgs::Point convertFromGridToMapPoint(AStar::GridPoint point); -}; - -#endif // PATHFINDING_IGVC_OCCUPANCYGRIDADAPTER_H diff --git a/src/pathfinding_igvc/include/OccupancyGridResizer.h b/src/pathfinding_igvc/include/OccupancyGridResizer.h deleted file mode 100644 index 9b55d9b63..000000000 --- a/src/pathfinding_igvc/include/OccupancyGridResizer.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: A class that adds space around the grid, i.e. - * an additional row below the grid and above the grid, and - * an additional column to the left and right of the grid. - */ - -#ifndef PATHFINDING_IGVC_OCCUPANCYGRIDRESIZER_H -#define PATHFINDING_IGVC_OCCUPANCYGRIDRESIZER_H - -#include "AStar.h" -#include - -class OccupancyGridResizer { - public: - /** - * Given an occupancy grid, adds space around the grid, i.e. - * an additional row below the grid and above the grid, and - * an additional column to the left and right of the grid. - * - * The grid will also move its origin down by one cell and left by - * one cell. - * - * @param grid the occupancy grid passed by reference - */ - void static addSpaceAroundGrid(nav_msgs::OccupancyGrid& grid); - - private: - /** - * Given an occupancy grid, adds an additional column to the left - * of the grid. The grid's origin is also moved to the left by one cell. - * - * @param grid - */ - void static addSpaceLeft(nav_msgs::OccupancyGrid& grid); - - /** - * Given an occupancy grid, adds an additional column to the right - * of the grid. The grid's origin is unchanged. - * - * @param grid - */ - void static addSpaceRight(nav_msgs::OccupancyGrid& grid); - - /** - * Given an occupancy grid, adds an additional row above - * the grid. The grid's origin is unchanged. - * - * @param grid - */ - void static addSpaceUp(nav_msgs::OccupancyGrid& grid); - - /** - * Given an occupancy grid, adds an additional row below - * the grid. The grid's origin is also moved down by one cell. - * - * @param grid - */ - void static addSpaceDown(nav_msgs::OccupancyGrid& grid); - - /** - * Given an occupancy grid, updates origin by moving the origin - * one cell down and one cell left. - * - * @param grid - */ - void static updateOrigin(nav_msgs::OccupancyGrid& grid); -}; - -#endif // PATHFINDING_IGVC_OCCUPANCYGRIDRESIZER_H diff --git a/src/pathfinding_igvc/include/PathConstructor.h b/src/pathfinding_igvc/include/PathConstructor.h deleted file mode 100644 index 5b7698adb..000000000 --- a/src/pathfinding_igvc/include/PathConstructor.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: A class that takes in the output of AStar, represented as a - * stack of cell locations, and produces the actual path - * containing poses in map frame - */ - -#ifndef PATHFINDING_IGVC_PATHCONSTRUCTOR_H -#define PATHFINDING_IGVC_PATHCONSTRUCTOR_H - -#include -#include - -class PathConstructor { - std::shared_ptr _occupancy_grid_adapter; - - public: - /** - * Takes in an OccupancyGridAdapter and returns a - * PathConstructor. - * The occupancy grid adapter is used to convert the cell - * location of the points to - * an actual point in the map frame. - * - * @param occupancy_grid_adapter - * @return PathConstructor - */ - PathConstructor( - std::shared_ptr occupancy_grid_adapter_ptr); - - /** - * Takes in a stack of GridPoints and returns a path - * - * @param points a stack of GridPoints where the top contains the starting - * GridPoint and the bottom contains the goal GridPoint - * @return path - */ - nav_msgs::Path constructPath(std::stack points); -}; - -#endif // PATHFINDING_IGVC_PATHCONSTRUCTOR_H diff --git a/src/pathfinding_igvc/include/PathFinder.h b/src/pathfinding_igvc/include/PathFinder.h deleted file mode 100644 index 0904b0be3..000000000 --- a/src/pathfinding_igvc/include/PathFinder.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 5th 2018 - * Description: Class that acts as an interface between PathFinderNode and - * AStar. - * Deals with a lot of frame transformations and data type - * conversions between ROS types and native types. - * Does not actually calculate the path itself - depends on AStar - * to get the path. - */ - -#ifndef PATHFINDING_IGVC_PATHFINDER_H -#define PATHFINDING_IGVC_PATHFINDER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class PathFinder { - public: - /** - * Takes a start and goal points in world frame, and returns the shortest - * path - * from start to goal based on information from occupancy grid. - * - * @param start starting point in world frame - * @param goal goal point in world frame - * @param grid occupancy grid - * @return shortest path from start to goal - */ - static nav_msgs::Path calculatePath(geometry_msgs::Point start, - geometry_msgs::Point goal, - nav_msgs::OccupancyGrid grid); - - /** - * Takes in a grid and adds space around the grid if the goal is not - * inside the grid. - * - * Also calculates row and col of starting point and goal point in - * occupancy grid (represented as AStar::GridPoint) into start_on_grid - * and goal_on_grid. - * - * @param grid the occupancy grid - * @param start the start point in map frame - * @param goal the goal point in map frame - * @param start_on_grid transformation of start point into occupancy grid - * @param goal_on_grid transformation of goal point into occupancy grid - */ - static void - processGridAndGetStartAndGoalOnGrid(nav_msgs::OccupancyGrid& grid, - geometry_msgs::Point start, - geometry_msgs::Point goal, - AStar::GridPoint& start_on_grid, - AStar::GridPoint& goal_on_grid); -}; - -#endif // PATHFINDING_IGVC_PATHFINDER_H diff --git a/src/pathfinding_igvc/include/PathFinderNode.h b/src/pathfinding_igvc/include/PathFinderNode.h deleted file mode 100644 index 9c2463eb6..000000000 --- a/src/pathfinding_igvc/include/PathFinderNode.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 23rd 2018 - * Description: Path Finder Node - subscribes to occupancy grid - * (nav_msgs/OccupancyGrid) - * and goal point (geometry_msgs/Point), obtains starting point from tf tree, - * and publishes the shortest path (nav_msgs/Path) from starting point to goal - * point. - */ - -#ifndef PROJECT_PATHFINDERNODE_H -#define PROJECT_PATHFINDERNODE_H - -#include -#include -#include -#include -#include -#include -#include - -class PathFinderNode { - private: - ros::Subscriber grid_subscriber; - ros::Subscriber goal_subscriber; - ros::Publisher publisher; - - tf::TransformListener* _listener; - - std::string _global_frame_name; - std::string _base_frame_name; - - geometry_msgs::Point _goal; - nav_msgs::OccupancyGrid _grid; - - bool _received_goal = false; - bool _receivied_grid = false; - - public: - // constructor - PathFinderNode(int argc, char** argv, std::string node_name); - - private: - /** - * Function that receives the occupancy grid. - * It first stores the grid as a member variable of this class, - * and checks if a goal has been received. If we have a goal, - * then calculates the path and publishes it. - * - * @param grid the occupancy grid - */ - void occupancyGridCallback(const nav_msgs::OccupancyGrid grid); - - /** - * Function that receives the goal point (in map frame) - * It first stores the goal as a member variable of this class, - * and checks if a grid has been received. If we have a grid, - * then calculates the path and publishes it. - * - * @param goal - */ - void goalCallback(const geometry_msgs::Point goal); - - /** - * Function that publishes the path. - * It is responsible for getting the starting position by - * getting the current position of the robot through the tf - * tree, then calling the function that returns the path. - */ - void publishPath(); -}; - -#endif // PROJECT_PATHFINDERNODE_H diff --git a/src/pathfinding_igvc/include/PathFinderUtils.h b/src/pathfinding_igvc/include/PathFinderUtils.h deleted file mode 100644 index 8a88404fe..000000000 --- a/src/pathfinding_igvc/include/PathFinderUtils.h +++ /dev/null @@ -1,130 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 13th 2018 - * Description: A class that contains all generic helper functions for all - * implementations of path finding - */ - -#ifndef PATHFINDING_IGVC_PATHFINDERUTILS_H -#define PATHFINDING_IGVC_PATHFINDERUTILS_H - -#include "AStar.h" -#include -#include -#include -#include -#include - -class PathFinderUtils { - public: - /** - * Takes a geometry_msgs::Point and converts it to type tf::Vector3 - * - * @param point point of type geometry_msgs::Point - * @return vector of type tf::Vector3 - */ - static tf::Vector3 pointToVector(geometry_msgs::Point point) { - return tf::Vector3(point.x, point.y, point.z); - } - - /** - * Takes a tf::Vector3 and converts it to type geometry_msgs::Point - * - * @param vector3 vector of type tf::Vector3 - * @return point of type geometry_msgs::Point - */ - static geometry_msgs::Point vectorToPoint(tf::Vector3 vector3) { - geometry_msgs::Point p; - p.x = vector3.x(); - p.y = vector3.y(); - p.z = vector3.z(); - return p; - } - - /** - * Constructs a 2D geometry_msgs::PoseStamped given a 2D point (only care - * about x and y) - * and z-angle - * - * @param point 2D point (only care about x and y) - * @param angle angle in z-axis - * @return pose stamped - */ - static geometry_msgs::PoseStamped - constructPoseStamped(geometry_msgs::Point point, double angle) { - geometry_msgs::Pose pose; - - pose.position = point; - tf::Quaternion q = getQuaternionFromAngle(angle); - tf::quaternionTFToMsg(q, pose.orientation); - - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose = pose; - - return pose_stamped; - } - - /** - * Returns the angle of the vector connecting two 2D points - * - * @param from the origin point of vector - * @param to the end point (arrow head) of vector - * @return angle or direction of the vector - */ - static double getAngleBetweenPoints(geometry_msgs::Point from, - geometry_msgs::Point to) { - tf::Vector3 vec = pointToVector(to) - pointToVector(from); - return atan2(vec.y(), vec.x()); - } - - /** - * Returns the quaternion that represents an angle in z-axis - * - * @param angle angle in z-axis - * @return quaternion quaternion representation of the angle - */ - static tf::Quaternion getQuaternionFromAngle(double angle) { - tf::Quaternion q; - tf::Matrix3x3 rotationMatrix = tf::Matrix3x3(); - rotationMatrix.setEulerYPR( - angle, 0.0, 0.0); // only set Z rotation since it's 2D - rotationMatrix.getRotation(q); - - return q; - } - - /** - * Returns whether a given grid point is inside the occupancy grid. - * - * @param grid_info the metadata of the occupancy grid - * @param point the point in question - * @return - */ - static bool isPointInsideGrid(nav_msgs::MapMetaData grid_info, - AStar::GridPoint point) { - if (point.col < 0 || point.row < 0) return false; - if (point.col >= grid_info.width) return false; - if (point.row >= grid_info.height) return false; - return true; - } - - /** - * Fits the grid point inside the occupancy grid by updating it to - * be the closest cell to the goal that is still in the grid. - * - * @param grid_info - * @param point - */ - static void fitPointInsideGrid(nav_msgs::MapMetaData grid_info, - AStar::GridPoint& point) { - point.col = point.col < 0 ? 0 : point.col; - point.col = - point.col >= grid_info.width ? grid_info.width - 1 : point.col; - - point.row = point.row < 0 ? 0 : point.row; - point.row = - point.row >= grid_info.height ? grid_info.height - 1 : point.row; - } -}; - -#endif // PATHFINDING_IGVC_PATHFINDERUTILS_H diff --git a/src/pathfinding_igvc/include/PathFinding.h b/src/pathfinding_igvc/include/PathFinding.h deleted file mode 100644 index 8046705e7..000000000 --- a/src/pathfinding_igvc/include/PathFinding.h +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Created By: William Gu - * Created On: Jan 20, 2018 - * Description: A path finding node which converts a given path into a Twist - * message to send to the robot - */ - -#ifndef PATHFINDING_IGVC_PATHFINDING_H -#define PATHFINDING_IGVC_PATHFINDING_H - -#include "geometry_msgs/Point.h" -#include "geometry_msgs/Pose.h" -#include "geometry_msgs/PoseStamped.h" -#include "geometry_msgs/Quaternion.h" -#include "geometry_msgs/Twist.h" -#include "nav_msgs/Path.h" -#include "ros/ros.h" -#include "sb_utils.h" -#include -#include - -class PathFinding { - public: - // The constructor - PathFinding(int argc, char** argv, std::string node_name); - - /** - * Produces a twist message for the robot from the path message and - * current robot coordinates + orientation - * @param path_msg given path message to process - * @param x_pos current robot x cood - * @param y_pos current robot y cood - * @param orientation current robot orientation in global frame - * @param num_poses number of poses to process including initial robot pos - * @return calculated path message - */ - static geometry_msgs::Twist pathToTwist(nav_msgs::Path path_msg, - double x_pos, - double y_pos, - double orientation, - int num_poses, - bool valid_cood); - - /** - * Adds geometric vector values to two empty vectors, based on contents of - * an array of poses - * @param poses : Array of pose messages - * @param x_vectors Empty vector to represent x values of geometric vectors - * @param y_vectors Empty vector to represent y values of geometric vectors - * @param num_poses Number of poses to process (including initial robot - * position), - * must be >=1 but <=size(x_vectors)-1 - * @param x_pos Current robot x position in global system - * @param y_pos Current robot y position in global system - */ - static void - calcVectors(const std::vector& poses, - std::vector& x_vectors, - std::vector& y_vectors, - int num_poses, - double x_pos, - double y_pos); - - /** - * Calculates a weighted value given a std::vector of geometric vectors - * The values near the front of the list are given a higher weight (since - * they are imminent) - * Currently the scaling for the weights are INVERSE - * @param vectors represents geometric vector values for one dimension, i.e. - * x - * values in some geometric vectors - * @param numToSum number of elements to sum, should be less than or equal - * to - * size of vectors - * @return weighted values of given geometric vectors - */ - static float weightedSum(const std::vector& vectors, int num_to_sum); - - private: - /** - * Produces twist msg based on path msg and publishes it - * @param path_ptr - */ - void pathCallBack(const nav_msgs::Path::ConstPtr& path_ptr); - - /** - * Updates the current position and orientation of the robot in the global - * frame, - * and stores in member variables - * @param tf_message - */ - void tfCallBack(const tf2_msgs::TFMessageConstPtr tf_message); - - ros::Subscriber path_subscriber; - ros::Subscriber tf_subscriber; - ros::Publisher twist_publisher; - - std::string base_frame; // The base frame of the robot ("base_link", - // "base_footprint", etc.) - std::string global_frame; // The global frame ("map", "odom", etc.) - int num_poses; // Rosparam: number of poses to process - - /* Robot coordinates in global system */ - double robot_x_pos; - double robot_y_pos; - double robot_orientation; - - bool valid_cood; // Flag is set to true when we receive our first tf message -}; - -#endif // PATHFINDING_IGVC_PATHFINDING_H diff --git a/src/pathfinding_igvc/launch/path_finder.launch b/src/pathfinding_igvc/launch/path_finder.launch deleted file mode 100644 index 559bf7676..000000000 --- a/src/pathfinding_igvc/launch/path_finder.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - "/base_link" - "/map" - - - diff --git a/src/pathfinding_igvc/launch/path_finding.launch b/src/pathfinding_igvc/launch/path_finding.launch deleted file mode 100644 index b715e2dc3..000000000 --- a/src/pathfinding_igvc/launch/path_finding.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - 10 - "BASE_FRAME" - "GLOBAL_FRAME" - - - diff --git a/src/pathfinding_igvc/package.xml b/src/pathfinding_igvc/package.xml deleted file mode 100644 index de842f3b8..000000000 --- a/src/pathfinding_igvc/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - pathfinding_igvc - 0.0.0 - The pathfinding_igvc package - - - william - - - - - - TODO - - - - catkin - roscpp - sb_utils - roscpp - roscpp - - diff --git a/src/pathfinding_igvc/src/AStar.cpp b/src/pathfinding_igvc/src/AStar.cpp deleted file mode 100644 index 7e96c2051..000000000 --- a/src/pathfinding_igvc/src/AStar.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 9th 2018 - * Description: Class that implements A* algorithm to find the shortest path - * between two points given an occupancy grid - */ - -#include -using namespace std; - -AStar::AStar(nav_msgs::OccupancyGrid occupancy_grid, - GridPoint start, - GridPoint goal) { - this->_num_cols = occupancy_grid.info.width; - this->_num_rows = occupancy_grid.info.height; - this->_goal = goal; - this->_start = start; - this->_grid = occupancy_grid.data; - - this->_cell_details = std::vector>( - this->_num_rows, std::vector(this->_num_cols)); - - // Create an open list - this->_open_list = set(); - - // Create a closed list and initialise it to false which means - // that no cell has been included yet - // This closed list is implemented as a boolean 2D array - this->_closed_list = std::vector>( - this->_num_rows, std::vector(this->_num_cols, false)); -} - -std::stack AStar::run(nav_msgs::OccupancyGrid occupancy_grid, - GridPoint start, - GridPoint goal) { - // The constructor is private because a path would only be found once - // for a given occupancy grid and start and goal points. It's also a - // way to force initialization of the class variables every time we - // run the algorithm. - return AStar(occupancy_grid, start, goal).search(); -} - -std::stack AStar::search() { - GridPoint start = this->_start; - - // Initialising the parameters of the starting node - this->_cell_details[start.row][start.col].f = 0.0; - this->_cell_details[start.row][start.col].g = 0.0; - this->_cell_details[start.row][start.col].h = 0.0; - this->_cell_details[start.row][start.col].parent = start; - - this->_open_list.insert( - std::make_pair(0.0, std::make_pair(start.row, start.col))); - - while (!this->_open_list.empty()) { - GridPointWithScore p = *this->_open_list.begin(); - - // Remove this vertex from the open list - this->_open_list.erase(this->_open_list.begin()); - - // Add this vertex to the open list - int row = p.second.first; - int col = p.second.second; - this->_closed_list[row][col] = true; - - GridPoint parent(row, col); - /* - Generating all the 8 successor of this cell - - N.W N N.E - \ | / - \ | / - W----CellDetail----E - / | \ - / | \ - S.W S S.E - - CellDetail-->Popped CellDetail (row, col) - N --> North (row-1, col) - S --> South (row+1, col) - E --> East (row, col+1) - W --> West (row, col-1) - N.E--> North-East (row-1, col+1) - N.W--> North-West (row-1, col-1) - S.E--> South-East (row+1, col+1) - S.W--> South-West (row+1, col-1)*/ - - //----------- 1st Successor (North) ------------ - if (processSuccessor(GridPoint(row - 1, col), parent)) { - return tracePath(); - } - - //----------- 2nd Successor (South) ------------ - if (processSuccessor(GridPoint(row + 1, col), parent)) { - return tracePath(); - } - - //----------- 3rd Successor (East) ------------ - - if (processSuccessor(GridPoint(row, col + 1), parent)) { - return tracePath(); - } - - //----------- 4th Successor (West) ------------ - - if (processSuccessor(GridPoint(row, col - 1), parent)) { - return tracePath(); - } - - //----------- 5th Successor (North-East) ------------ - - if (processSuccessor(GridPoint(row - 1, col + 1), parent)) { - return tracePath(); - } - - //----------- 6th Successor (North-West) ------------ - - if (processSuccessor(GridPoint(row - 1, col - 1), parent)) { - return tracePath(); - } - - //----------- 7th Successor (South-East) ------------ - - if (processSuccessor(GridPoint(row + 1, col + 1), parent)) { - return tracePath(); - } - - //----------- 8th Successor (South-West) ------------ - - if (processSuccessor(GridPoint(row + 1, col - 1), parent)) { - return tracePath(); - } - } - - return std::stack(); -} - -bool AStar::processSuccessor(GridPoint successor, GridPoint parent) { - if (!isValid(successor)) return false; - - // If the destination cell is the same as the - // current successor - if (isDestination(successor)) { - // Set the Parent of the destination cell - this->_cell_details[successor.row][successor.col].parent = parent; - return true; - } - // If the successor is already on the closed - // list or if it is blocked, then ignore it. - // Else do the following - if (!this->_closed_list[successor.row][successor.col] && - isUnBlocked(successor)) { - double g_new, h_new, f_new; - g_new = this->_cell_details[parent.row][parent.col].g + 1.0; - h_new = calculateHValue(successor); - f_new = g_new + h_new; - - // If it isn’t on the open list, add it to - // the open list. Make the current square - // the parent of this square. Record the - // f, g, and h costs of the square cell - // OR - // If it is on the open list already, check - // to see if this path to that square is better, - // using 'f' cost as the measure. - if (this->_cell_details[successor.row][successor.col].f == FLT_MAX || - this->_cell_details[successor.row][successor.col].f > f_new) { - this->_open_list.insert(std::make_pair( - f_new, std::make_pair(successor.row, successor.col))); - - // Update the details of this cell - this->_cell_details[successor.row][successor.col].f = f_new; - this->_cell_details[successor.row][successor.col].g = g_new; - this->_cell_details[successor.row][successor.col].h = h_new; - this->_cell_details[successor.row][successor.col].parent = parent; - } - } - - return false; -} - -// A Utility Function to trace the path from the source -// to destination -std::stack AStar::tracePath() { - int row = this->_goal.row; - int col = this->_goal.col; - - std::stack path; - - while (!(this->_cell_details[row][col].parent.row == row && - this->_cell_details[row][col].parent.col == col)) { - path.push(GridPoint(row, col)); - - int temp_row = this->_cell_details[row][col].parent.row; - int temp_col = this->_cell_details[row][col].parent.col; - - row = temp_row; - col = temp_col; - } - - path.push(GridPoint(row, col)); - - return path; -} - -bool AStar::isUnBlocked(AStar::GridPoint point) { - return this->_grid[point.row * this->_num_cols + point.col] == GRID_FREE; -} - -bool AStar::isDestination(AStar::GridPoint point) { - return this->_goal.row == point.row && this->_goal.col == point.col; -} - -// A Utility Function to calculate the 'h' heuristics. -double AStar::calculateHValue(AStar::GridPoint point) { - // Return using the euclidean distance formula - return sqrt(pow(point.row - this->_goal.row, 2) + - pow(point.col - this->_goal.col, 2)); -} - -bool AStar::isValid(AStar::GridPoint point) { - // Returns true if row number and column number - // is in range - return (point.row >= 0) && (point.row < this->_num_rows) && - (point.col >= 0) && (point.col < this->_num_cols); -} diff --git a/src/pathfinding_igvc/src/FrameTransformer.cpp b/src/pathfinding_igvc/src/FrameTransformer.cpp deleted file mode 100644 index 17392734d..000000000 --- a/src/pathfinding_igvc/src/FrameTransformer.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 13th 2018 - * Description: Implementation of frame transformer - */ - -#include - -FrameTransformer::FrameTransformer(tf::Quaternion rotation, - tf::Vector3 position) { - this->_transformation_to_map = tf::Transform(rotation, position); - this->_transformation_to_grid = this->_transformation_to_map.inverse(); -} - -geometry_msgs::Point -FrameTransformer::transformFromMapToGridFrame(geometry_msgs::Point point) { - tf::Vector3 map_point = PathFinderUtils::pointToVector(point); - tf::Vector3 grid_point = this->_transformation_to_grid * map_point; - return PathFinderUtils::vectorToPoint(grid_point); -} - -geometry_msgs::Point -FrameTransformer::transformFromGridToMapFrame(geometry_msgs::Point point) { - tf::Vector3 grid_point = PathFinderUtils::pointToVector(point); - tf::Vector3 map_point = this->_transformation_to_map * grid_point; - return PathFinderUtils::vectorToPoint(map_point); -} diff --git a/src/pathfinding_igvc/src/OccupancyGridAdapter.cpp b/src/pathfinding_igvc/src/OccupancyGridAdapter.cpp deleted file mode 100644 index 0c75dc158..000000000 --- a/src/pathfinding_igvc/src/OccupancyGridAdapter.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 13th 2018 - * Description: Implementation of occupancy grid adapter - */ - -#include - -OccupancyGridAdapter::OccupancyGridAdapter(nav_msgs::MapMetaData info) { - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - tf::Vector3 origin_position = - PathFinderUtils::pointToVector(info.origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::Quaternion origin_quaternion; - tf::quaternionMsgToTF(info.origin.orientation, origin_quaternion); - - this->_frame_transformer = - new FrameTransformer(origin_quaternion, origin_position); - this->_grid_info = info; -} - -AStar::GridPoint -OccupancyGridAdapter::convertFromMapToGridPoint(geometry_msgs::Point point) { - geometry_msgs::Point point_in_grid_frame = - this->_frame_transformer->transformFromMapToGridFrame(point); - - // cell (0,0) is in the bottom left of the grid - int col = point_in_grid_frame.x / this->_grid_info.resolution; - int row = point_in_grid_frame.y / this->_grid_info.resolution; - - col = col < 0 ? col - 1 : col; - row = row < 0 ? row - 1 : row; - - return AStar::GridPoint(row, col); -} - -geometry_msgs::Point -OccupancyGridAdapter::convertFromGridToMapPoint(AStar::GridPoint grid_point) { - geometry_msgs::Point point; - point.x = grid_point.col * this->_grid_info.resolution; - point.y = grid_point.row * this->_grid_info.resolution; - - return this->_frame_transformer->transformFromGridToMapFrame(point); -} diff --git a/src/pathfinding_igvc/src/OccupancyGridResizer.cpp b/src/pathfinding_igvc/src/OccupancyGridResizer.cpp deleted file mode 100644 index 7026d3dde..000000000 --- a/src/pathfinding_igvc/src/OccupancyGridResizer.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Implementation of occupancy grid resizer - */ - -#include - -void OccupancyGridResizer::addSpaceAroundGrid(nav_msgs::OccupancyGrid& grid) { - addSpaceLeft(grid); - addSpaceRight(grid); - addSpaceUp(grid); - addSpaceDown(grid); - - updateOrigin(grid); -} - -void OccupancyGridResizer::addSpaceLeft(nav_msgs::OccupancyGrid& grid) { - auto it = grid.data.begin(); - - for (unsigned int row = 0; row < grid.info.height; row++) { - it = grid.data.insert(it, 1, AStar::GRID_FREE); - it += grid.info.width + 1; - } - - grid.info.width++; -} - -void OccupancyGridResizer::addSpaceRight(nav_msgs::OccupancyGrid& grid) { - auto it = grid.data.begin() + grid.info.width; - - for (unsigned int row = 0; row < grid.info.height; row++) { - it = grid.data.insert(it, 1, AStar::GRID_FREE); - it += grid.info.width + 1; - } - - grid.info.width++; -} - -void OccupancyGridResizer::addSpaceUp(nav_msgs::OccupancyGrid& grid) { - grid.data.insert(grid.data.end(), grid.info.width, AStar::GRID_FREE); - - grid.info.height++; -} - -void OccupancyGridResizer::addSpaceDown(nav_msgs::OccupancyGrid& grid) { - grid.data.insert(grid.data.begin(), grid.info.width, AStar::GRID_FREE); - - grid.info.height++; -} - -void OccupancyGridResizer::updateOrigin(nav_msgs::OccupancyGrid& grid) { - AStar::GridPoint new_origin(-1, -1); - grid.info.origin.position = - OccupancyGridAdapter(grid.info).convertFromGridToMapPoint(new_origin); -} diff --git a/src/pathfinding_igvc/src/PathConstructor.cpp b/src/pathfinding_igvc/src/PathConstructor.cpp deleted file mode 100644 index b11963eed..000000000 --- a/src/pathfinding_igvc/src/PathConstructor.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Implementation of path constructor - */ - -#include - -PathConstructor::PathConstructor( -std::shared_ptr occupancy_grid_adapter_ptr) { - this->_occupancy_grid_adapter = occupancy_grid_adapter_ptr; -} - -nav_msgs::Path -PathConstructor::constructPath(std::stack grid_points) { - nav_msgs::Path path; - - while (!grid_points.empty()) { - AStar::GridPoint current_grid_point = grid_points.top(); - geometry_msgs::Point current_map_point = - this->_occupancy_grid_adapter->convertFromGridToMapPoint( - current_grid_point); - grid_points.pop(); - - double angle = 0.0; - if (!grid_points.empty()) { - AStar::GridPoint next_grid_point = grid_points.top(); - geometry_msgs::Point next_map_point = - this->_occupancy_grid_adapter->convertFromGridToMapPoint( - next_grid_point); - angle = PathFinderUtils::getAngleBetweenPoints(current_map_point, - next_map_point); - } - - geometry_msgs::PoseStamped pose_stamped = - PathFinderUtils::constructPoseStamped(current_map_point, angle); - path.poses.push_back(pose_stamped); - } - - return path; -} diff --git a/src/pathfinding_igvc/src/PathFinder.cpp b/src/pathfinding_igvc/src/PathFinder.cpp deleted file mode 100644 index e8b1ff761..000000000 --- a/src/pathfinding_igvc/src/PathFinder.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 5th 2018 - * Description: Class that acts as an interface between PathFinderNode and - * AStar. - * Deals with a lot of frame transformations and data type - * conversions between ROS types and native types. - * Does not actually calculate the path itself - depends on AStar - * to get the path. - */ - -#include - -nav_msgs::Path PathFinder::calculatePath(geometry_msgs::Point start, - geometry_msgs::Point goal, - nav_msgs::OccupancyGrid grid) { - AStar::GridPoint goal_on_grid; - AStar::GridPoint start_on_grid; - processGridAndGetStartAndGoalOnGrid( - grid, start, goal, start_on_grid, goal_on_grid); - - std::stack points = - AStar::run(grid, start_on_grid, goal_on_grid); - std::shared_ptr occupancy_grid_adapter_ptr( - new OccupancyGridAdapter(grid.info)); - return PathConstructor(occupancy_grid_adapter_ptr).constructPath(points); -} - -void PathFinder::processGridAndGetStartAndGoalOnGrid( -nav_msgs::OccupancyGrid& grid, -geometry_msgs::Point start, -geometry_msgs::Point goal, -AStar::GridPoint& start_on_grid, -AStar::GridPoint& goal_on_grid) { - AStar::GridPoint initial_goal_on_grid = - OccupancyGridAdapter(grid.info).convertFromMapToGridPoint(goal); - bool grid_needs_resizing = - !PathFinderUtils::isPointInsideGrid(grid.info, initial_goal_on_grid); - if (grid_needs_resizing) { OccupancyGridResizer::addSpaceAroundGrid(grid); } - - OccupancyGridAdapter occupancy_grid_adapter = - OccupancyGridAdapter(grid.info); - start_on_grid = occupancy_grid_adapter.convertFromMapToGridPoint(start); - goal_on_grid = occupancy_grid_adapter.convertFromMapToGridPoint(goal); - - bool point_needs_fitting = - !PathFinderUtils::isPointInsideGrid(grid.info, goal_on_grid); - if (point_needs_fitting) { - PathFinderUtils::fitPointInsideGrid(grid.info, goal_on_grid); - } -} diff --git a/src/pathfinding_igvc/src/PathFinderNode.cpp b/src/pathfinding_igvc/src/PathFinderNode.cpp deleted file mode 100644 index e13167c96..000000000 --- a/src/pathfinding_igvc/src/PathFinderNode.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 23rd 2018 - * Description: Implementation of Path Finder Node - * Once the node has received both occupancy grid and the goal - * point, - * it retrieves the position of the robot through the tf tree, and - * calls PathFinder's static methods to calculate the path. - * Then it publishes the path. - */ - -#include - -PathFinderNode::PathFinderNode(int argc, char** argv, std::string node_name) { - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - SB_getParam(private_nh, - std::string("global_frame_name"), - this->_global_frame_name, - std::string("/map")); - SB_getParam(private_nh, - std::string("base_frame_name"), - this->_base_frame_name, - std::string("/base_link")); - - std::string grid_subscriber_topic = "/occupancy_grid"; - int refresh_rate = 10; - this->grid_subscriber = nh.subscribe(grid_subscriber_topic, - refresh_rate, - &PathFinderNode::occupancyGridCallback, - this); - - std::string goal_subscriber_topic = "/goal"; - this->goal_subscriber = nh.subscribe( - goal_subscriber_topic, refresh_rate, &PathFinderNode::goalCallback, this); - - std::string topic_to_publish_to = "path"; - uint32_t queue_size = 1; - this->publisher = - private_nh.advertise(topic_to_publish_to, queue_size); - - this->_listener = new tf::TransformListener(); -} - -void PathFinderNode::occupancyGridCallback(const nav_msgs::OccupancyGrid grid) { - this->_grid = grid; - this->_receivied_grid = true; - if (this->_received_goal) { publishPath(); } -} - -void PathFinderNode::goalCallback(const geometry_msgs::Point goal) { - this->_goal = goal; - this->_received_goal = true; - if (this->_receivied_grid) { publishPath(); } -} - -void PathFinderNode::publishPath() { - tf::StampedTransform transform; - - try { - this->_listener->lookupTransform(this->_global_frame_name, - this->_base_frame_name, - ros::Time::now(), - transform); - } catch (tf::TransformException ex) { - // If we can't lookup the tf, then don't publish path - ROS_WARN_STREAM( - "Could not lookup tf between " << this->_global_frame_name << " and " - << this->_base_frame_name); - ROS_WARN_STREAM(ex.what()); - return; - } - - geometry_msgs::Point start; - start.x = transform.getOrigin().x(); - start.y = transform.getOrigin().y(); - - nav_msgs::Path path = - PathFinder::calculatePath(start, this->_goal, this->_grid); - - this->publisher.publish(path); -} diff --git a/src/pathfinding_igvc/src/PathFinding.cpp b/src/pathfinding_igvc/src/PathFinding.cpp deleted file mode 100644 index 09a1bfcdd..000000000 --- a/src/pathfinding_igvc/src/PathFinding.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/* - * Created By: William Gu - * Created On: Jan 20, 2018 - * Description: A path finding node which converts a given path into a Twist - * message to send to the robot - */ - -#include - -PathFinding::PathFinding(int argc, char** argv, std::string node_name) { - // Setup NodeHandles - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - uint32_t queue_size = 1; - - /*No received coordinates from tf on launch so not valid */ - valid_cood = false; - - std::string path_subscribe_topic = "/path"; // Setup subscriber to path - path_subscriber = nh.subscribe( - path_subscribe_topic, queue_size, &PathFinding::pathCallBack, this); - - std::string tf_subscribe_topic = "/tf"; // Setup subscriber to tf - tf_subscriber = nh.subscribe( - tf_subscribe_topic, queue_size, &PathFinding::tfCallBack, this); - - std::string twist_publish_topic = - private_nh.resolveName("/cmd_vel"); // setup Publisher to twist - twist_publisher = - nh.advertise(twist_publish_topic, queue_size); - - // Get Params - SB_getParam( - private_nh, "base_frame", base_frame, (std::string) "base_link"); - SB_getParam( - private_nh, "global_frame", global_frame, (std::string) "odom_combined"); - SB_getParam(private_nh, "num_poses", num_poses, 10); -} - -void PathFinding::pathCallBack(const nav_msgs::Path::ConstPtr& path_ptr) { - nav_msgs::Path path_msg = - *path_ptr; // Take required information from received message - geometry_msgs::Twist twist_msg = pathToTwist(path_msg, - robot_x_pos, - robot_y_pos, - robot_orientation, - num_poses, - valid_cood); - twist_publisher.publish(twist_msg); -} - -void PathFinding::tfCallBack(const tf2_msgs::TFMessageConstPtr tf_message) { - // Check if the tf_message contains the transform we're looking for - for (geometry_msgs::TransformStamped tf_stamped : tf_message->transforms) { - if (tf_stamped.header.frame_id == global_frame && - tf_stamped.child_frame_id == base_frame) { - double x_pos = tf_stamped.transform.translation.x; - double y_pos = tf_stamped.transform.translation.y; - - double quatx = tf_stamped.transform.rotation.x; - double quaty = tf_stamped.transform.rotation.y; - double quatz = tf_stamped.transform.rotation.z; - double quatw = tf_stamped.transform.rotation.w; - tf::Quaternion quat(quatx, quaty, quatz, quatw); - tf::Matrix3x3 rot_matrix(quat); - double roll, pitch, yaw; - rot_matrix.getRPY(roll, pitch, yaw); - - // Set member variables - robot_x_pos = x_pos; - robot_y_pos = y_pos; - robot_orientation = - yaw; // Orientation = rotation about z axis (yaw) - valid_cood = true; - } - } -} - -geometry_msgs::Twist PathFinding::pathToTwist(nav_msgs::Path path_msg, - double x_pos, - double y_pos, - double orientation, - int num_poses, - bool valid_cood) { - geometry_msgs::Twist twist_msg; // Initialize velocity message - - if (!valid_cood) { // No TF received yet so don't move - twist_msg.linear.x = 0; - twist_msg.angular.z = 0; - return twist_msg; - } - - std::vector inc_poses = path_msg.poses; - - std::vector x_vectors; // holds x values for vectors - std::vector y_vectors; // holds y values for vectors - calcVectors(inc_poses, - x_vectors, - y_vectors, - num_poses, - x_pos, - y_pos); // x_vectors and y_vectors now updated - - float x_sum = weightedSum( - x_vectors, - num_poses - - 1); //-1 because number of vectors is one less than number of poses - float y_sum = weightedSum(y_vectors, num_poses - 1); - - float desired_angle = atan(y_sum / x_sum); - - float turn_rate = fmod(desired_angle - orientation, - 2 * M_PI); // Keep turn_rate between -2pi and 2pi - - // If pi < turn < 2pi or -2pi < turn < -pi, turn in other direction instead - if (turn_rate > M_PI) - turn_rate -= 2 * M_PI; - else if (turn_rate < -M_PI) - turn_rate += 2 * M_PI; - - float speed = - 1.0 - fabs(fmod(turn_rate, 2 * M_PI) / - (2 * M_PI)); // Could multiply this by some factor to scale speed - - twist_msg.linear.x = speed; - twist_msg.angular.z = turn_rate; - return twist_msg; -} - -void PathFinding::calcVectors( -const std::vector& poses, -std::vector& x_vectors, -std::vector& y_vectors, -int num_poses, -double x_pos, -double y_pos) { - x_vectors.push_back(poses[0].pose.position.x - x_pos); - y_vectors.push_back(poses[0].pose.position.y - y_pos); - for (int i = 1; i < num_poses; i++) { - x_vectors.push_back(poses[i].pose.position.x - - poses[i - 1].pose.position.x); - y_vectors.push_back(poses[i].pose.position.y - - poses[i - 1].pose.position.y); - } -} - -float PathFinding::weightedSum(const std::vector& vectors, - int num_to_sum) { - float weighted_sum = 0; - for (int i = 0; i < num_to_sum; i++) { - weighted_sum += vectors[i] / (i + 1); // 1/x scaling - } - return weighted_sum; -} diff --git a/src/pathfinding_igvc/src/path_finding.cpp b/src/pathfinding_igvc/src/path_finding.cpp deleted file mode 100644 index 576a4a597..000000000 --- a/src/pathfinding_igvc/src/path_finding.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* - * Created By: William Gu - * Created On: Jan 20, 2018 - * Description: A path finding node which converts a given path into a Twist - * message to send to the robot - */ - -#include - -int main(int argc, char** argv) { - // Setup your ROS node - std::string node_name = "path_finding"; - - // Create an instance of your class - PathFinding pf_node(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/pathfinding_igvc/test/PathFinderTestUtils.h b/src/pathfinding_igvc/test/PathFinderTestUtils.h deleted file mode 100644 index 08e20ebeb..000000000 --- a/src/pathfinding_igvc/test/PathFinderTestUtils.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 12th 2018 - * Description: Class that provides helper functions for path-finder-test - */ - -#ifndef PATHFINDING_IGVC_PATHFINDERTESTUTILS_H -#define PATHFINDING_IGVC_PATHFINDERTESTUTILS_H - -#include -#include -#include - -class PathFinderTestUtils { - public: - /** - * Constructs a 2D pose based on x and y position and z-angle - * - * @param x x component of position - * @param y y component of position - * @param angle angle in z-axis - * @return geometry_msgs::Pose containing position and orientation - */ - static geometry_msgs::Pose constructPose(double x, double y, double angle) { - geometry_msgs::Pose origin; - - // set position of the origin - geometry_msgs::Point position; - position.x = x; - position.y = y; - position.z = 0.0; - origin.position = position; - - // set orientation of the origin - tf::Quaternion q; - tf::Matrix3x3 rotationMatrix = tf::Matrix3x3(); - rotationMatrix.setEulerYPR( - angle, 0.0, 0.0); // only set Z rotation since it's 2D - rotationMatrix.getRotation(q); - tf::quaternionTFToMsg(q, origin.orientation); - - return origin; - } -}; - -#endif // PATHFINDING_IGVC_PATHFINDERTESTUTILS_H diff --git a/src/pathfinding_igvc/test/astar-test.cpp b/src/pathfinding_igvc/test/astar-test.cpp deleted file mode 100644 index b27e948af..000000000 --- a/src/pathfinding_igvc/test/astar-test.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 20th 2018 - * Description: Tests for AStar class - */ - -#include "PathFinderTestUtils.h" -#include -#include - -signed char _ = AStar::GRID_FREE; -signed char X = AStar::GRID_OCCUPIED; - -TEST(AStar, FullPathTest) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(0.0, 0.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData mapMetaData; - mapMetaData.resolution = 1.0; - mapMetaData.width = 10; - mapMetaData.height = 9; - // add origin to mapMetaData - mapMetaData.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - - // set mapMetaData - grid.info = mapMetaData; - grid.data = {_, _, _, X, X, X, _, X, X, _, _, X, _, _, _, _, X, _, - _, _, _, X, X, X, X, _, X, X, X, _, _, X, _, _, _, _, - X, _, X, X, _, _, _, X, _, _, _, X, _, X, X, X, _, X, - _, X, X, X, X, _, _, _, _, X, _, _, X, _, X, _, _, _, - _, X, _, _, _, X, _, _, _, X, _, _, _, _, X, _, _, _}; - - AStar::GridPoint start(0.0, 0.0); - AStar::GridPoint goal(8.0, 0.0); - - std::stack path = AStar::run(grid, start, goal); - - std::vector expected_x = {0, 0, 0, 0, 1, 2, 1, 0, 0}; - std::vector expected_y = {0, 1, 2, 3, 4, 5, 6, 7, 8}; - - int i = 0; - while (!path.empty()) { - AStar::GridPoint point = path.top(); - path.pop(); - - EXPECT_EQ(expected_x[i], point.col); - EXPECT_EQ(expected_y[i], point.row); - - i++; - } -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/frame-transformer-test.cpp b/src/pathfinding_igvc/test/frame-transformer-test.cpp deleted file mode 100644 index 50cfcac13..000000000 --- a/src/pathfinding_igvc/test/frame-transformer-test.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Unit tests for frame transformer - */ - -#include "PathFinderTestUtils.h" -#include -#include -#include - -TEST(FrameTransformer, TestGetAngleBetweenPoints) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(5.0, 5.0, 0.0); - - tf::Quaternion rotation; - tf::Vector3 position; - - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - position = PathFinderUtils::pointToVector(origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::quaternionMsgToTF(origin.orientation, rotation); - - geometry_msgs::Point point; - point.x = 5.0; - point.y = 6.0; - point.z = 0.0; - - geometry_msgs::Point point_on_grid = - FrameTransformer(rotation, position).transformFromMapToGridFrame(point); - - EXPECT_FLOAT_EQ(point_on_grid.x, point.x - origin.position.x); - EXPECT_FLOAT_EQ(point_on_grid.y, point.y - origin.position.y); -} - -TEST(FrameTransformer, TestChangeOfFrameWith90Rotation) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(5.0, 5.0, M_PI / 2); - - tf::Quaternion rotation; - tf::Vector3 position; - - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - position = PathFinderUtils::pointToVector(origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::quaternionMsgToTF(origin.orientation, rotation); - - geometry_msgs::Point point; - point.x = 2.0; - point.y = 6.0; - point.z = 0.0; - - geometry_msgs::Point point_on_grid = - FrameTransformer(rotation, position).transformFromMapToGridFrame(point); - - EXPECT_FLOAT_EQ(point_on_grid.x, 1.0); - EXPECT_FLOAT_EQ(point_on_grid.y, 3.0); -} - -TEST(FrameTransformer, TestChangeOfFrameWith30Rotation) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, M_PI / 6); - - tf::Quaternion rotation; - tf::Vector3 position; - - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - position = PathFinderUtils::pointToVector(origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::quaternionMsgToTF(origin.orientation, rotation); - - geometry_msgs::Point point; - point.x = 3.0 + sqrt(3.0); - point.y = 3.0 + 1.0; - point.z = 0.0; - - geometry_msgs::Point point_on_grid = - FrameTransformer(rotation, position).transformFromMapToGridFrame(point); - - EXPECT_NEAR(point_on_grid.x, 2.0, 0.01); - EXPECT_NEAR(point_on_grid.y, 0.0, 0.01); -} - -TEST(FrameTransformer, TestChangeOfFrameWith30RotationToMap) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, M_PI / 6); - - tf::Quaternion rotation; - tf::Vector3 position; - - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - position = PathFinderUtils::pointToVector(origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::quaternionMsgToTF(origin.orientation, rotation); - - geometry_msgs::Point point_on_grid; - point_on_grid.x = 2.0; - point_on_grid.y = 0.0; - - geometry_msgs::Point point_on_map = - FrameTransformer(rotation, position) - .transformFromGridToMapFrame(point_on_grid); - - EXPECT_NEAR(point_on_map.x, 3.0 + sqrt(3.0), 0.01); - EXPECT_NEAR(point_on_map.y, 3.0 + 1.0, 0.01); -} - -TEST(FrameTransformer, TestChangeOfFrameWith45RotationToMap) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, M_PI / 4); - - tf::Quaternion rotation; - tf::Vector3 position; - - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - position = PathFinderUtils::pointToVector(origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::quaternionMsgToTF(origin.orientation, rotation); - - geometry_msgs::Point point_on_grid; - point_on_grid.x = -2.0; - point_on_grid.y = -2.0; - - geometry_msgs::Point point_on_map = - FrameTransformer(rotation, position) - .transformFromGridToMapFrame(point_on_grid); - - EXPECT_FLOAT_EQ(3.0 - 2 * cos(M_PI / 4) + 2 * sin(M_PI / 4), - point_on_map.x); - EXPECT_FLOAT_EQ(3.0 - 2 * sin(M_PI / 4) - 2 * cos(M_PI / 4), - point_on_map.y); -} - -TEST(FrameTransformer, TestChangeOfFrameWith45RotationToMapWithBothXAndY) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, M_PI / 6); - - tf::Quaternion rotation; - tf::Vector3 position; - - // setup transformation matrix from map to grid frame - // convert geometry_msgs::Point to tf::Vector3 - position = PathFinderUtils::pointToVector(origin.position); - // convert geometry_msgs::Quaternion to tf::Quaternion - tf::quaternionMsgToTF(origin.orientation, rotation); - - geometry_msgs::Point point_on_grid; - point_on_grid.x = -2.0; - point_on_grid.y = -2.0; - - geometry_msgs::Point point_on_map = - FrameTransformer(rotation, position) - .transformFromGridToMapFrame(point_on_grid); - - EXPECT_FLOAT_EQ(3.0 - 2 * cos(M_PI / 6) + 2 * sin(M_PI / 6), - point_on_map.x); - EXPECT_FLOAT_EQ(3.0 - 2 * sin(M_PI / 6) - 2 * cos(M_PI / 6), - point_on_map.y); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/occupancy-grid-adapter-test.cpp b/src/pathfinding_igvc/test/occupancy-grid-adapter-test.cpp deleted file mode 100644 index 7d28d2050..000000000 --- a/src/pathfinding_igvc/test/occupancy-grid-adapter-test.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Unit tests for occupancy grid adapter - */ - -#include "PathFinderTestUtils.h" -#include -#include - -TEST(OccupancyGridAdapter, TestIndexOfPointInGrid) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 2; - // add origin to mapMetaData - map_meta_data.origin = origin; - - /* OccupancyGridAdapter */ - OccupancyGridAdapter adapter = OccupancyGridAdapter(map_meta_data); - - geometry_msgs::Point point; - point.x = 6.2; - point.y = 3.1; - point.z = 0.0; - - AStar::GridPoint grid_point = adapter.convertFromMapToGridPoint(point); - - EXPECT_EQ(grid_point.col, 1); - EXPECT_EQ(grid_point.row, 0); -} - -TEST(OccupancyGridAdapter, TestConvertFromMapToGridPoint) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 2; - // add origin to mapMetaData - map_meta_data.origin = origin; - - /* OccupancyGridAdapter */ - OccupancyGridAdapter adapter = OccupancyGridAdapter(map_meta_data); - AStar::GridPoint point(-99, -99); - - geometry_msgs::Point map_point = adapter.convertFromGridToMapPoint(point); - - EXPECT_FLOAT_EQ(map_point.x, 3.0 - 99 * 2); - EXPECT_FLOAT_EQ(map_point.y, 3.0 - 99 * 2); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/occupancy-grid-resizer-test.cpp b/src/pathfinding_igvc/test/occupancy-grid-resizer-test.cpp deleted file mode 100644 index 998a902da..000000000 --- a/src/pathfinding_igvc/test/occupancy-grid-resizer-test.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Unit tests for occupancy grid resizer - */ - -#include "PathFinderTestUtils.h" -#include -#include -#include - -TEST(OccupancyGridResizer, TestAddSpaceAroundGrid) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* map_meta_data of OccupancyGrid */ - // initialize map_meta_data - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 3; - // add origin to map_meta_data - map_meta_data.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - // set map_meta_data - grid.info = map_meta_data; - grid.data = std::vector(6, AStar::GRID_OCCUPIED); - - OccupancyGridResizer::addSpaceAroundGrid(grid); - - EXPECT_EQ(map_meta_data.width + 2, grid.info.width); - EXPECT_EQ(map_meta_data.height + 2, grid.info.height); - EXPECT_FLOAT_EQ(map_meta_data.origin.position.x - map_meta_data.resolution, - grid.info.origin.position.x); - EXPECT_FLOAT_EQ(map_meta_data.origin.position.y - map_meta_data.resolution, - grid.info.origin.position.y); - - std::vector expected_data = { - AStar::GRID_FREE, AStar::GRID_FREE, AStar::GRID_FREE, - AStar::GRID_FREE, AStar::GRID_FREE, AStar::GRID_OCCUPIED, - AStar::GRID_OCCUPIED, AStar::GRID_FREE, AStar::GRID_FREE, - AStar::GRID_OCCUPIED, AStar::GRID_OCCUPIED, AStar::GRID_FREE, - AStar::GRID_FREE, AStar::GRID_OCCUPIED, AStar::GRID_OCCUPIED, - AStar::GRID_FREE, AStar::GRID_FREE, AStar::GRID_FREE, - AStar::GRID_FREE, AStar::GRID_FREE, - }; - - EXPECT_EQ(expected_data, grid.data); -} - -TEST(OccupancyGridResizer, TestAddSpaceAroundGridWithAngle) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, M_PI / 4); - - /* map_meta_data of OccupancyGrid */ - // initialize map_meta_data - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 3; - // add origin to map_meta_data - map_meta_data.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - // set map_meta_data - grid.info = map_meta_data; - grid.data = std::vector(6, AStar::GRID_OCCUPIED); - - OccupancyGridResizer::addSpaceAroundGrid(grid); - - EXPECT_EQ(map_meta_data.width + 2, grid.info.width); - EXPECT_EQ(map_meta_data.height + 2, grid.info.height); - EXPECT_FLOAT_EQ(3.0 - 2 * cos(M_PI / 4) + 2 * sin(M_PI / 4), - grid.info.origin.position.x); - EXPECT_FLOAT_EQ(3.0 - 2 * sin(M_PI / 4) - 2 * cos(M_PI / 4), - grid.info.origin.position.y); - - std::vector expected_data = { - AStar::GRID_FREE, AStar::GRID_FREE, AStar::GRID_FREE, - AStar::GRID_FREE, AStar::GRID_FREE, AStar::GRID_OCCUPIED, - AStar::GRID_OCCUPIED, AStar::GRID_FREE, AStar::GRID_FREE, - AStar::GRID_OCCUPIED, AStar::GRID_OCCUPIED, AStar::GRID_FREE, - AStar::GRID_FREE, AStar::GRID_OCCUPIED, AStar::GRID_OCCUPIED, - AStar::GRID_FREE, AStar::GRID_FREE, AStar::GRID_FREE, - AStar::GRID_FREE, AStar::GRID_FREE, - }; - - EXPECT_EQ(expected_data, grid.data); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/path-constructor-test.cpp b/src/pathfinding_igvc/test/path-constructor-test.cpp deleted file mode 100644 index 70acd1d42..000000000 --- a/src/pathfinding_igvc/test/path-constructor-test.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Unit tests for path constructor - */ - -#include "PathFinderTestUtils.h" -#include -#include - -// TODO: Uncomment and fix, see issue #339 -TEST(PathConstructor, TestConstructPath) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 2; - // add origin to mapMetaData - map_meta_data.origin = origin; - - /* OccupancyGridAdapter */ - std::shared_ptr occupancy_grid_adapter_ptr( - new OccupancyGridAdapter(map_meta_data)); - - /* first point in path*/ - AStar::GridPoint point1(-99 - sqrt(3), -99 - 1); - - /* second point in path*/ - AStar::GridPoint point2(-99, -99); - - /* add points to path */ - std::stack points; - points.push(point1); - points.push(point2); - - PathConstructor path_constructor(occupancy_grid_adapter_ptr); - nav_msgs::Path path = path_constructor.constructPath(points); - - tf::Quaternion q1; - tf::quaternionMsgToTF(path.poses[0].pose.orientation, q1); - - /* we lose resolution by converting a point into a grid, so allow more - error - */ - EXPECT_NEAR(q1.getAngle(), M_PI + M_PI / 6, 0.5); - EXPECT_FLOAT_EQ( - path.poses[0].pose.position.x, - occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point2).x); - EXPECT_FLOAT_EQ( - path.poses[0].pose.position.y, - occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point2).y); - EXPECT_FLOAT_EQ( - path.poses[1].pose.position.x, - occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point1).x); - EXPECT_FLOAT_EQ( - path.poses[1].pose.position.y, - occupancy_grid_adapter_ptr->convertFromGridToMapPoint(point1).y); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/path-finder-test.cpp b/src/pathfinding_igvc/test/path-finder-test.cpp deleted file mode 100644 index 3d0fbeff5..000000000 --- a/src/pathfinding_igvc/test/path-finder-test.cpp +++ /dev/null @@ -1,281 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 19th 2018 - * Description: Unit Tests for Path Finder - */ - -#include "PathFinderTestUtils.h" -#include -#include - -signed char _ = AStar::GRID_FREE; -signed char X = AStar::GRID_OCCUPIED; - -// TODO: Uncomment and fix, see issue #339 -TEST(PathFinder, TestPathWithNoObstacle) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData mapMetaData; - mapMetaData.resolution = 2.0; - mapMetaData.width = 2; - mapMetaData.height = 2; - // add origin to mapMetaData - mapMetaData.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - - // set mapMetaData - grid.info = mapMetaData; - grid.data = std::vector(4, _); - - geometry_msgs::Point start; - start.x = 3.0; - start.y = 3.0; - geometry_msgs::Point goal; - goal.x = 5.0; - goal.y = 5.0; - - nav_msgs::Path path = PathFinder::calculatePath(start, goal, grid); - - ASSERT_EQ(path.poses.size(), 2); - - EXPECT_FLOAT_EQ(path.poses[0].pose.position.x, 3.0); - EXPECT_FLOAT_EQ(path.poses[0].pose.position.y, 3.0); - - EXPECT_FLOAT_EQ(path.poses[1].pose.position.x, 5.0); - EXPECT_FLOAT_EQ(path.poses[1].pose.position.y, 5.0); -} - -// TODO: Uncomment and fix, see issue #339 -TEST(PathFinder, TestFullPath) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(0.0, 0.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData mapMetaData; - mapMetaData.resolution = 1.0; - mapMetaData.width = 10; - mapMetaData.height = 9; - // add origin to mapMetaData - mapMetaData.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - - // set mapMetaData - grid.info = mapMetaData; - grid.data = {_, _, _, X, X, X, _, X, X, _, _, X, _, _, _, _, X, _, - _, _, _, X, X, X, X, _, X, X, X, _, _, X, _, _, _, _, - X, _, X, X, _, _, _, X, _, _, _, X, _, X, X, X, _, X, - _, X, X, X, X, _, _, _, _, X, _, _, X, _, X, _, _, _, - _, X, _, _, _, X, _, _, _, X, _, _, _, _, X, _, _, _}; - - geometry_msgs::Point start; - start.x = 0.0; - start.y = 0.0; - geometry_msgs::Point goal; - goal.x = 0.0; - goal.y = 8.0; - - nav_msgs::Path path = PathFinder::calculatePath(start, goal, grid); - - ASSERT_EQ(path.poses.size(), 9); - - std::vector expected_x = { - 0.0, 0.0, 0.0, 0.0, 1.0, 2.0, 1.0, 0.0, 0.0}; - std::vector expected_y = { - 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; - - for (int i = 0; i < path.poses.size(); i++) { - EXPECT_FLOAT_EQ(expected_x[i], path.poses[i].pose.position.x); - EXPECT_FLOAT_EQ(expected_y[i], path.poses[i].pose.position.y); - } -} - -// TODO: Uncomment and fix, see issue #339 -TEST(PathFinder, PathFindingWhenGoalNotInGrid) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* map_meta_data of OccupancyGrid */ - // initialize map_meta_data - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 3; - // add origin to map_meta_data - map_meta_data.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - // set map_meta_data - grid.info = map_meta_data; - grid.data = { - X, X, X, X, _, _, - }; - - /* Starting point in map frame */ - geometry_msgs::Point start; - start.x = 3.0; - start.y = 7.0; - - /* Goal point in map frame */ - geometry_msgs::Point goal; - goal.x = 3.0; - goal.y = -9999; - - nav_msgs::Path path = PathFinder::calculatePath(start, goal, grid); - - EXPECT_EQ(path.poses.size(), 4); - - std::vector expected_x = {3.0, 1.0, 1.0, 3.0}; - std::vector expected_y = {7.0, 5.0, 3.0, 1.0}; - for (int i = 0; i < path.poses.size(); i++) { - EXPECT_FLOAT_EQ(expected_x[i], path.poses[i].pose.position.x); - EXPECT_FLOAT_EQ(expected_y[i], path.poses[i].pose.position.y); - } -} - -TEST(PathFinder, ProcessGridAndGetStartAndGoalOnGrid) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* map_meta_data of OccupancyGrid */ - // initialize map_meta_data - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 3; - // add origin to map_meta_data - map_meta_data.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - // set map_meta_data - grid.info = map_meta_data; - grid.data = std::vector(6, X); - - /* Starting point in map frame */ - geometry_msgs::Point start; - start.x = 4.8; - start.y = 7.9; - - /* Goal point in map frame */ - geometry_msgs::Point goal; - goal.x = 9999; - goal.y = 6.3; - - /* Declaration of starting and goal point in grid frame */ - AStar::GridPoint start_on_grid; - AStar::GridPoint goal_on_grid; - - /* Function being tested */ - PathFinder::processGridAndGetStartAndGoalOnGrid( - grid, start, goal, start_on_grid, goal_on_grid); - - /* Verify that grid has been resized */ - EXPECT_EQ(map_meta_data.width + 2, grid.info.width); - EXPECT_EQ(map_meta_data.height + 2, grid.info.height); - EXPECT_FLOAT_EQ(map_meta_data.origin.position.x - map_meta_data.resolution, - grid.info.origin.position.x); - EXPECT_FLOAT_EQ(map_meta_data.origin.position.y - map_meta_data.resolution, - grid.info.origin.position.y); - - std::vector expected_data = { - _, _, _, _, _, X, X, _, _, X, X, _, _, X, X, _, _, _, _, _, - }; - - EXPECT_EQ(expected_data, grid.data); - - /* Verify that starting point has been correctly transformed into the - * resized grid frame */ - EXPECT_EQ(1, start_on_grid.col); - EXPECT_EQ(3, start_on_grid.row); - - /* Verify that goal point has been correctly transformed into the resized - * grid frame and fit into the grid */ - EXPECT_EQ(3, goal_on_grid.col); - EXPECT_EQ(2, goal_on_grid.row); -} - -TEST(PathFinder, ProcessGridAndGetStartAndGoalOnGridWithAngle) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, M_PI / 6); - - /* map_meta_data of OccupancyGrid */ - // initialize map_meta_data - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 2; - map_meta_data.height = 3; - // add origin to map_meta_data - map_meta_data.origin = origin; - - /* OccupancyGrid */ - // initialize occupancy grid - nav_msgs::OccupancyGrid grid; - // set map_meta_data - grid.info = map_meta_data; - grid.data = std::vector(6, X); - - /* Starting point in map frame */ - geometry_msgs::Point start; - start.x = 4.8; - start.y = 7.9; - - /* Goal point in map frame */ - geometry_msgs::Point goal; - goal.x = 9999; - goal.y = 6.3; - - /* Declaration of starting and goal point in grid frame */ - AStar::GridPoint start_on_grid; - AStar::GridPoint goal_on_grid; - - /* Function being tested */ - PathFinder::processGridAndGetStartAndGoalOnGrid( - grid, start, goal, start_on_grid, goal_on_grid); - - /* Verify that grid has been resized */ - EXPECT_EQ(map_meta_data.width + 2, grid.info.width); - EXPECT_EQ(map_meta_data.height + 2, grid.info.height); - - float expected_origin_x = map_meta_data.origin.position.x - - map_meta_data.resolution * cos(M_PI / 6) + - map_meta_data.resolution * sin(M_PI / 6); - float expected_origin_y = map_meta_data.origin.position.y - - map_meta_data.resolution * sin(M_PI / 6) - - map_meta_data.resolution * cos(M_PI / 6); - EXPECT_FLOAT_EQ(expected_origin_x, grid.info.origin.position.x); - EXPECT_FLOAT_EQ(expected_origin_y, grid.info.origin.position.y); - - std::vector expected_data = { - _, _, _, _, _, X, X, _, _, X, X, _, _, X, X, _, _, _, _, _, - }; - - EXPECT_EQ(expected_data, grid.data); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/path-finder-utils-test.cpp b/src/pathfinding_igvc/test/path-finder-utils-test.cpp deleted file mode 100644 index cbc48a99b..000000000 --- a/src/pathfinding_igvc/test/path-finder-utils-test.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 14th 2018 - * Description: Unit tests for path finder utils class - */ - -#include "PathFinderTestUtils.h" -#include -#include - -TEST(PathFinderUtils, TestGetAngleBetweenPoints) { - geometry_msgs::Point from; - from.x = -99.0; - from.y = -99.0; - from.z = 0.0; - - geometry_msgs::Point to; - to.x = from.x - sqrt(3); - to.y = from.y - 1.0; - to.z = 0.0; - - double angle = PathFinderUtils::getAngleBetweenPoints(from, to); - tf::Quaternion q = PathFinderUtils::getQuaternionFromAngle(angle); - EXPECT_FLOAT_EQ(M_PI + M_PI / 6, q.getAngle()); -} - -TEST(PathFinderUtils, TestFitPointInsideGridInNegativeRow) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 4; - map_meta_data.height = 5; - // add origin to mapMetaData - map_meta_data.origin = origin; - - AStar::GridPoint grid_point = AStar::GridPoint(-100, 1); - - PathFinderUtils::fitPointInsideGrid(map_meta_data, grid_point); - - EXPECT_EQ(grid_point.col, 1); - EXPECT_EQ(grid_point.row, 0); -} - -TEST(PathFinderUtils, TestFitPointInsideGridInPositiveRow) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 4; - map_meta_data.height = 5; - // add origin to mapMetaData - map_meta_data.origin = origin; - - AStar::GridPoint grid_point = AStar::GridPoint(99999, 3); - - PathFinderUtils::fitPointInsideGrid(map_meta_data, grid_point); - - EXPECT_EQ(grid_point.col, 3); - EXPECT_EQ(grid_point.row, map_meta_data.height - 1); -} - -TEST(PathFinderUtils, TestFitPointInsideGridInNegativeCol) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 4; - map_meta_data.height = 5; - // add origin to mapMetaData - map_meta_data.origin = origin; - - AStar::GridPoint grid_point = AStar::GridPoint(2, -99999); - - PathFinderUtils::fitPointInsideGrid(map_meta_data, grid_point); - - EXPECT_EQ(grid_point.col, 0); - EXPECT_EQ(grid_point.row, 2); -} - -TEST(PathFinderUtils, TestFitPointInsideGridInPositiveCol) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 4; - map_meta_data.height = 5; - // add origin to mapMetaData - map_meta_data.origin = origin; - - AStar::GridPoint grid_point = AStar::GridPoint(4, 8888); - - PathFinderUtils::fitPointInsideGrid(map_meta_data, grid_point); - - EXPECT_EQ(grid_point.col, map_meta_data.width - 1); - EXPECT_EQ(grid_point.row, 4); -} - -TEST(PathFinderUtils, TestFitPointInsideGridInNegativeColNegativeRow) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 4; - map_meta_data.height = 5; - // add origin to mapMetaData - map_meta_data.origin = origin; - - AStar::GridPoint grid_point = AStar::GridPoint(-77777, -77777); - - PathFinderUtils::fitPointInsideGrid(map_meta_data, grid_point); - - EXPECT_EQ(grid_point.col, 0); - EXPECT_EQ(grid_point.row, 0); -} - -TEST(PathFinderUtils, TestFitPointInsideGridInPositiveXPositiveY) { - /* origin of OccupancyGrid */ - // initialize origin of occupancy grid - geometry_msgs::Pose origin = - PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); - - /* mapMetaData of OccupancyGrid */ - // initialize mapMetaData - nav_msgs::MapMetaData map_meta_data; - map_meta_data.resolution = 2.0; - map_meta_data.width = 4; - map_meta_data.height = 5; - // add origin to mapMetaData - map_meta_data.origin = origin; - - AStar::GridPoint grid_point = AStar::GridPoint(77777, 77777); - - PathFinderUtils::fitPointInsideGrid(map_meta_data, grid_point); - - EXPECT_EQ(grid_point.col, map_meta_data.width - 1); - EXPECT_EQ(grid_point.row, map_meta_data.height - 1); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/path-finding-test.cpp b/src/pathfinding_igvc/test/path-finding-test.cpp deleted file mode 100644 index db53cb626..000000000 --- a/src/pathfinding_igvc/test/path-finding-test.cpp +++ /dev/null @@ -1,200 +0,0 @@ -#include -#include - -TEST(PathFinding, testWeightedSum1) { - std::vector testVec; - testVec.push_back(3); - testVec.push_back(3); - testVec.push_back(3); - - EXPECT_EQ(5.5, PathFinding::weightedSum(testVec, 3)); -} - -TEST(PathFinding, testWeightedSum2) { - std::vector testVec; - testVec.push_back(1); - testVec.push_back(2); - testVec.push_back(3); - - EXPECT_EQ(1.0, PathFinding::weightedSum(testVec, 1)); -} - -TEST(PathFinding, testWeightedSumNegative) { - std::vector testVec; - testVec.push_back(1); - testVec.push_back(-2); - testVec.push_back(-3); - - EXPECT_EQ(-1.0, PathFinding::weightedSum(testVec, 3)); -} - -// Case where robot starting orientation is forward and must travel a straight -// path diagonal (along y=x line) -TEST(PathFinding, testStraightPathToTwist) { - double xPos = 0; - double yPos = 0; - double orientation = 0; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose.position.x = i; - pose_stamped.pose.position.y = i; - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(0.875, twist_msg.linear.x, 0.01); - EXPECT_NEAR(0.785, twist_msg.angular.z, 0.01); -} - -// Case where robot starting orientation is perpendicular to a straight path -TEST(PathFinding, testPerpenPathToTwist) { - double xPos = 0; - double yPos = 0; - double orientation = M_PI / 2.0; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose.position.x = i; - pose_stamped.pose.position.y = 0; - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(0.750, twist_msg.linear.x, 0.01); - EXPECT_NEAR(-1.57, twist_msg.angular.z, 0.01); -} - -// Case where robot starting orientation is opposite to a straight path -TEST(PathFinding, testOppPathToTwist) { - double xPos = 0; - double yPos = 0; - double orientation = M_PI; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose.position.x = i; - pose_stamped.pose.position.y = 0; - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(0.5, twist_msg.linear.x, 0.01); - EXPECT_NEAR(M_PI, twist_msg.angular.z, 0.01); -} - -// Case where robot is far from beginning of path and facing wrong direction -TEST(PathFinding, testFarStartPos) { - double xPos = -100; - double yPos = -100; - double orientation = -M_PI / 2; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose.position.x = i; - pose_stamped.pose.position.y = i; - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(0.63, twist_msg.linear.x, 0.01); - EXPECT_NEAR(2.36, twist_msg.angular.z, 0.01); -} - -// Case where path consists of 2 sharp turns, one 90 deg left and one 90 deg -// right later -TEST(PathFinding, testSharpTurns) { - double xPos = 0; - double yPos = 0; - double orientation = 0; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - if (i <= 5) { - pose_stamped.pose.position.x = 0; - pose_stamped.pose.position.y = i; - } else { - pose_stamped.pose.position.x = i - 5; - pose_stamped.pose.position.y = 5; - } - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(0.787, twist_msg.linear.x, 0.01); - EXPECT_NEAR(1.336, twist_msg.angular.z, 0.01); -} - -// Case where path consists of erratic direction and magnitude changes -TEST(PathFinding, testErraticPath) { - double xPos = 0; - double yPos = 0; - double orientation = 0; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - if (i % 2 == 0) { - pose_stamped.pose.position.x = 2 * i; - pose_stamped.pose.position.y = -2 * i; - } else { - pose_stamped.pose.position.x = i; - pose_stamped.pose.position.y = 2 * i; - } - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(0.903, twist_msg.linear.x, 0.01); - EXPECT_NEAR(0.606, twist_msg.angular.z, 0.01); -} - -// Case where position/orientation of robot is ahead of the path -TEST(PathFinding, testPathLag) { - double xPos = 2.5; - double yPos = 0; - double orientation = 0; - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose.position.x = i; - pose_stamped.pose.position.y = 0; - poses.push_back(pose_stamped); - } - path_msg.poses = poses; - - geometry_msgs::Twist twist_msg = - PathFinding::pathToTwist(path_msg, xPos, yPos, orientation, 10, true); - EXPECT_NEAR(1.0, twist_msg.linear.x, 0.01); - EXPECT_NEAR(0, twist_msg.angular.z, 0.01); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/pathfinding_igvc/test/path_finder_igvc_test.test b/src/pathfinding_igvc/test/path_finder_igvc_test.test deleted file mode 100644 index 4e6d339d9..000000000 --- a/src/pathfinding_igvc/test/path_finder_igvc_test.test +++ /dev/null @@ -1,10 +0,0 @@ - - - - - "/base_link" - "/map" - - - - diff --git a/src/pathfinding_igvc/test/path_finder_rostest.cpp b/src/pathfinding_igvc/test/path_finder_rostest.cpp deleted file mode 100644 index a6bb21d46..000000000 --- a/src/pathfinding_igvc/test/path_finder_rostest.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: May 24th 2018 - * Description: Ros tests for Path Finder Node - */ - -#include "PathFinderTestUtils.h" -#include -#include -#include -#include -#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 PathFinderRosTest : public testing::Test { - protected: - virtual void SetUp() { - test_goal_publisher = nh_.advertise("/goal", 1); - test_grid_publisher = - nh_.advertise("/occupancy_grid", 1); - test_subscriber = nh_.subscribe( - "/path_finder/path", 1, &PathFinderRosTest::callback, this); - - // Let the publishers and subscribers set itself up timely - ros::Rate loop_rate(1); - loop_rate.sleep(); - } - - ros::NodeHandle nh_; - nav_msgs::Path path; - ros::Publisher test_goal_publisher; - ros::Publisher test_grid_publisher; - ros::Subscriber test_subscriber; - - public: - void callback(const nav_msgs::Path& incoming_path) { path = incoming_path; } -}; - -signed char _ = AStar::GRID_FREE; -signed char X = AStar::GRID_OCCUPIED; - -// TODO: Uncomment and fix, see issue #339 -// TEST_F(PathFinderRosTest, TestPathFinder) { -// /* origin of OccupancyGrid */ -// // initialize origin of occupancy grid -// geometry_msgs::Pose origin = -// PathFinderTestUtils::constructPose(3.0, 7.0, 0.0); -// -// /* mapMetaData of OccupancyGrid */ -// // initialize mapMetaData -// nav_msgs::MapMetaData mapMetaData; -// mapMetaData.resolution = 1.0; -// mapMetaData.width = 10; -// mapMetaData.height = 9; -// // add origin to mapMetaData -// mapMetaData.origin = origin; -// -// /* OccupancyGrid */ -// // initialize occupancy grid -// nav_msgs::OccupancyGrid grid; -// -// // set mapMetaData -// grid.info = mapMetaData; -// grid.data = {_, _, _, X, X, X, _, X, X, _, _, X, _, _, _, _, X, _, -// _, _, _, X, X, X, X, _, X, X, X, _, _, X, _, _, _, _, -// X, _, X, X, _, _, _, X, _, _, _, X, _, X, X, X, _, X, -// _, X, X, X, X, _, _, _, _, X, _, _, X, _, X, _, _, _, -// _, X, _, _, _, X, _, _, _, X, _, _, _, _, X, _, _, _}; -// -// test_grid_publisher.publish(grid); -// -// geometry_msgs::Point goal; -// goal.x = 3.0; -// goal.y = 15.0; -// -// test_goal_publisher.publish(goal); -// -// ros::Rate loop_rate(1); -// -// // Wait for the message to get passed around -// 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(); -// -// ASSERT_EQ(path.poses.size(), 9); -// -// std::vector expected_x_in_base_link = { -// 0.0, 0.0, 0.0, 0.0, 1.0, 2.0, 1.0, 0.0, 0.0}; -// std::vector expected_y_in_base_link = { -// 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; -// -// for (int i = 0; i < path.poses.size(); i++) { -// EXPECT_FLOAT_EQ(expected_x_in_base_link[i] + origin.position.x, -// path.poses[i].pose.position.x); -// EXPECT_FLOAT_EQ(expected_y_in_base_link[i] + origin.position.y, -// path.poses[i].pose.position.y); -// } -//} - -// TODO: Uncomment and fix, see issue #339 -// TEST_F(PathFinderRosTest, PathFindingWhenGoalNotInGrid) { -// /* origin of OccupancyGrid */ -// // initialize origin of occupancy grid -// geometry_msgs::Pose origin = -// PathFinderTestUtils::constructPose(3.0, 3.0, 0.0); -// -// /* map_meta_data of OccupancyGrid */ -// // initialize map_meta_data -// nav_msgs::MapMetaData map_meta_data; -// map_meta_data.resolution = 2.0; -// map_meta_data.width = 2; -// map_meta_data.height = 3; -// // add origin to map_meta_data -// map_meta_data.origin = origin; -// -// /* OccupancyGrid */ -// // initialize occupancy grid -// nav_msgs::OccupancyGrid grid; -// // set map_meta_data -// grid.info = map_meta_data; -// grid.data = { -// X, X, X, X, _, _, -// }; -// -// test_grid_publisher.publish(grid); -// -// /* Goal point in map frame */ -// geometry_msgs::Point goal; -// goal.x = 3.0; -// goal.y = -9999; -// -// test_goal_publisher.publish(goal); -// -// ros::Rate loop_rate(1); -// -// // Wait for the message to get passed around -// 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(path.poses.size(), 4); -// -// std::vector expected_x = {3.0, 1.0, 1.0, 3.0}; -// std::vector expected_y = {7.0, 5.0, 3.0, 1.0}; -// for (int i = 0; i < path.poses.size(); i++) { -// EXPECT_FLOAT_EQ(expected_x[i], path.poses[i].pose.position.x); -// EXPECT_FLOAT_EQ(expected_y[i], path.poses[i].pose.position.y); -// } -//} - -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, "path_finder_rostest"); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/pathfinding_igvc/test/path_finding_rostest.cpp b/src/pathfinding_igvc/test/path_finding_rostest.cpp deleted file mode 100644 index 3ea6b8510..000000000 --- a/src/pathfinding_igvc/test/path_finding_rostest.cpp +++ /dev/null @@ -1,264 +0,0 @@ -#include -#include - -class PathFindingTest : public testing::Test { - protected: - virtual void SetUp() { - test_publisher_path = - nh_.advertise("/path", 1); // Path publisher - test_publisher_tf = - nh_.advertise("/tf", 1); // TF publisher - test_subscriber = - nh_.subscribe("/cmd_vel", 1, &PathFindingTest::callback, this); - - // Let the publishers and subscribers set itself up timely - ros::Rate loop_rate(1); - loop_rate.sleep(); - } - - ros::NodeHandle nh_; - ros::Publisher test_publisher_path; - ros::Publisher test_publisher_tf; - ros::Subscriber test_subscriber; - double speed; - double turn_rate; - - public: - void callback(const geometry_msgs::Twist::ConstPtr vel) { - speed = vel->linear.x; - turn_rate = vel->angular.z; - } -}; - -TEST_F(PathFindingTest, testStraightPathFinding) { - tf2_msgs::TFMessage tf_msg; - std::vector all_transforms; - geometry_msgs::TransformStamped transform_stamped; - - transform_stamped.header.frame_id = "GLOBAL_FRAME"; - transform_stamped.child_frame_id = "BASE_FRAME"; - transform_stamped.transform.translation.x = 0; - transform_stamped.transform.translation.y = 0; - transform_stamped.transform.rotation.x = 0; - transform_stamped.transform.rotation.y = 0; - transform_stamped.transform.rotation.z = 0; - transform_stamped.transform.rotation.w = 1; - all_transforms.push_back(transform_stamped); - tf_msg.transforms = all_transforms; - test_publisher_tf.publish(tf_msg); - // Wait for the message to get passed around - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - // Construct a path message to send to the test node - // Path is currently a straight line - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose; - pose.pose.position.x = i; - pose.pose.position.y = i; - poses.push_back(pose); - } - path_msg.poses = poses; - - test_publisher_path.publish(path_msg); - - // Wait for the message to get passed around - loop_rate.sleep(); - - ros::spinOnce(); - - EXPECT_NEAR(0.88, speed, 0.1); - EXPECT_NEAR(M_PI / 4, turn_rate, 0.1); -} - -TEST_F(PathFindingTest, testCurvedPathFinding) { - tf2_msgs::TFMessage tf_msg; - std::vector all_transforms; - geometry_msgs::TransformStamped transform_stamped; - - transform_stamped.header.frame_id = "GLOBAL_FRAME"; - transform_stamped.child_frame_id = "BASE_FRAME"; - transform_stamped.transform.translation.x = 0; - transform_stamped.transform.translation.y = 0; - transform_stamped.transform.rotation.x = 0; - transform_stamped.transform.rotation.y = 0; - transform_stamped.transform.rotation.z = 0; - transform_stamped.transform.rotation.w = 1; - all_transforms.push_back(transform_stamped); - tf_msg.transforms = all_transforms; - test_publisher_tf.publish(tf_msg); - // Wait for the message to get passed around - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - // Construct a path message to send to the test node - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose; - pose.pose.position.x = i; - if (i <= 5) { - pose.pose.position.y = i; - } else { - pose.pose.position.y = 10 - i; - } - poses.push_back(pose); - } - path_msg.poses = poses; - - test_publisher_path.publish(path_msg); - - // Wait for the message to get passed around - loop_rate.sleep(); - - ros::spinOnce(); - - EXPECT_NEAR(0.89, speed, 0.1); - EXPECT_NEAR(0.55, turn_rate, 0.1); -} - -TEST_F(PathFindingTest, testSharpUTurn) { - tf2_msgs::TFMessage tf_msg; - std::vector all_transforms; - geometry_msgs::TransformStamped transform_stamped; - - transform_stamped.header.frame_id = "GLOBAL_FRAME"; - transform_stamped.child_frame_id = "BASE_FRAME"; - transform_stamped.transform.translation.x = 0; - transform_stamped.transform.translation.y = 0; - transform_stamped.transform.rotation.x = 0; - transform_stamped.transform.rotation.y = 0; - transform_stamped.transform.rotation.z = 0; - transform_stamped.transform.rotation.w = 1; - all_transforms.push_back(transform_stamped); - tf_msg.transforms = all_transforms; - test_publisher_tf.publish(tf_msg); - // Wait for the message to get passed around - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - // Construct a path message to send to the test node - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose; - pose.pose.position.y = i; - if (i <= 3) { - pose.pose.position.x = i; - } else { - pose.pose.position.x = 6 - i; - } - poses.push_back(pose); - } - path_msg.poses = poses; - - test_publisher_path.publish(path_msg); - - // Wait for the message to get passed around - loop_rate.sleep(); - - ros::spinOnce(); - - EXPECT_NEAR(0.80, speed, 0.1); - EXPECT_NEAR(1.28, turn_rate, 0.1); -} - -TEST_F(PathFindingTest, testReceiveTF1) { - // Send custom tf to update robot position to pos x=0, y=100, facing in pos - // x direction - // Then send a path msg (straight line) with the robot's new position in - // mind - tf2_msgs::TFMessage tf_msg; - std::vector all_transforms; - geometry_msgs::TransformStamped transform_stamped; - - transform_stamped.header.frame_id = "GLOBAL_FRAME"; - transform_stamped.child_frame_id = "BASE_FRAME"; - transform_stamped.transform.translation.x = 0; - transform_stamped.transform.translation.y = 100; - transform_stamped.transform.rotation.x = 0; - transform_stamped.transform.rotation.y = 0; - transform_stamped.transform.rotation.z = 0; - transform_stamped.transform.rotation.w = 1; - all_transforms.push_back(transform_stamped); - tf_msg.transforms = all_transforms; - test_publisher_tf.publish(tf_msg); - // Wait for the message to get passed around - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose; - pose.pose.position.y = 100; - pose.pose.position.x = i; - poses.push_back(pose); - } - path_msg.poses = poses; - - test_publisher_path.publish(path_msg); - - // Wait for the message to get passed around - loop_rate.sleep(); - ros::spinOnce(); - - EXPECT_NEAR(1, speed, 0.1); - EXPECT_NEAR(0, turn_rate, 0.1); -} - -TEST_F(PathFindingTest, testReceiveTF2) { - // Send custom tf to update robot position to pos x=50, y=50, facing in pos - // y direction - // Then send a path msg (straight line) with the robot's new position in - // mind - tf2_msgs::TFMessage tf_msg; - std::vector all_transforms; - geometry_msgs::TransformStamped transform_stamped; - - transform_stamped.header.frame_id = "GLOBAL_FRAME"; - transform_stamped.child_frame_id = "BASE_FRAME"; - transform_stamped.transform.translation.x = 50; - transform_stamped.transform.translation.y = 50; - transform_stamped.transform.rotation.x = 0; - transform_stamped.transform.rotation.y = 0; - transform_stamped.transform.rotation.z = 0.707; - transform_stamped.transform.rotation.w = 0.707; - all_transforms.push_back(transform_stamped); - tf_msg.transforms = all_transforms; - test_publisher_tf.publish(tf_msg); - // Wait for the message to get passed around - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - nav_msgs::Path path_msg; - std::vector poses; - for (int i = 1; i < 10; i++) { - geometry_msgs::PoseStamped pose; - pose.pose.position.y = 50 + i; - pose.pose.position.x = 50; - poses.push_back(pose); - } - path_msg.poses = poses; - - test_publisher_path.publish(path_msg); - - // Wait for the message to get passed around - loop_rate.sleep(); - ros::spinOnce(); - - EXPECT_NEAR(1, speed, 0.1); - EXPECT_NEAR(0, turn_rate, 0.1); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "path_finding_rostest"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/pathfinding_igvc/test/pathfinding_igvc_test.test b/src/pathfinding_igvc/test/pathfinding_igvc_test.test deleted file mode 100644 index d9d658339..000000000 --- a/src/pathfinding_igvc/test/pathfinding_igvc_test.test +++ /dev/null @@ -1,10 +0,0 @@ - - - - - 10 - "BASE_FRAME" - "GLOBAL_FRAME" - - - diff --git a/src/procontroller_snowbots/include/ProController.h b/src/procontroller_snowbots/include/ProController.h index 50a6cb425..8a7cb4006 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,13 +58,64 @@ 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 b845f2ebf..f6cb159a4 100755 --- a/src/procontroller_snowbots/src/ProController.cpp +++ b/src/procontroller_snowbots/src/ProController.cpp @@ -10,6 +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"; + 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)) { @@ -23,7 +26,13 @@ ProController::ProController(int argc, char** argv, string node_name) { } 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(); @@ -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); } } } @@ -133,8 +161,12 @@ void ProController::printControllerDebug(int type, int code, int value) { 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 * speed; - msg.angular.z = z_new * speed; + 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,19 +189,28 @@ 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) { + + if (inDeadzone(value)) { + if(state == Mode::wheels) { x = 0; - } else { + } + } + + 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); @@ -179,103 +220,142 @@ void ProController::leftJoystickX(int value) { // Updates x, which is then published by publish___XZ in readInputs() void ProController::leftJoystickY(int value) { - if (value > 115 && value < 135) { - z = 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); 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,58 +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; } } -// Increase or reduce rover speed void ProController::arrowsUorD(int value) { if (value == 1) { ROS_INFO("Up button pressed"); - speed = speed < 100 ? speed + 5 : speed; - ROS_INFO("Speed increased to %d%% of max output", speed); + 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"); - speed = speed > 5 ? speed - 5 : speed; - ROS_INFO("Speed increased to %d%% of max output", speed); + armOutVal = arrowD; + if (state == Mode::wheels) { + speed = speed > increment ? speed - increment : speed; + ROS_INFO("Speed decreased to %d%% of max output", speed); + } } } @@ -353,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/reactive_system_urc/CMakeLists.txt b/src/reactive_system_urc/CMakeLists.txt deleted file mode 100644 index a3012ea0d..000000000 --- a/src/reactive_system_urc/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(reactive_system_urc) - -# 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 -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - mapping_msgs_urc - sb_utils -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ./include - ./test - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ executable -add_executable(reactive_system_node - src/reactive_system_node.cpp - src/ReactiveSystemNode.cpp - src/ReactiveSystemPath.cpp - include/ReactiveSystemNode.h - include/ReactiveSystemPath.h) - - -## Specify libraries to link a library or executable target against -# target_link_libraries(sample_package_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(reactive_system_node - ${catkin_LIBRARIES} -) - - -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - - catkin_add_gtest(reactive-system-test test/reactive-system-test.cpp src/ReactiveSystemPath.cpp test/RiskAreaBuilder.h) - target_link_libraries(reactive-system-test ${catkin_LIBRARIES}) - -endif() diff --git a/src/reactive_system_urc/include/ReactiveSystemNode.h b/src/reactive_system_urc/include/ReactiveSystemNode.h deleted file mode 100644 index 78dcad9d5..000000000 --- a/src/reactive_system_urc/include/ReactiveSystemNode.h +++ /dev/null @@ -1,62 +0,0 @@ -/** - * Created by William Gu on Sept 29 2018 - * Class declaration for Reactive System Node - * This node is a wrapper for ReactiveSystemPath logic, which subscribes to - * twist, current GPS goal, and risk area - * nodes, and publishes a path message to head towards the goal while avoiding - * risky areas - */ - -#ifndef REACTIVE_SYSTEM_NODE_H -#define REACTIVE_SYSTEM_NODE_H - -#include "ReactiveSystemPath.h" -#include -#include -#include -#include -#include -#include - -class ReactiveSystemNode { - public: - ReactiveSystemNode(int argc, char** argv, std::string node_name); - - private: - ros::Subscriber risk_subscriber; - ros::Subscriber goal_subscriber; - ros::Publisher path_publisher; - - /** - * Callback function for receiving an array of risks - * Publishes an optimal twist command for the robot, avoiding risky areas - * while tending towards the goal - * @param ptr - */ - void riskCallBack(const mapping_msgs_urc::RiskAreaArray::ConstPtr& ptr); - - /** - * Callback for updating goal position - * @param ptr - */ - void goalCallBack(const sb_geom_msgs::Point2D::ConstPtr& ptr); - - /* ros params */ - // size of unit increments in a trajectory calculation - float traj_time_inc; - // number of total increments used in trajectory calculation - int traj_num_incs; - // current linear vel of robot (needs to be set initally) - float linear_vel; - // max turning velocity - float max_angular_vel; - // number of possible turning velocities to consider - int num_angular_vel; - // squared "radius" of the robot - float risk_dist_tol_sq; - - // current goal position to head towards - sb_geom_msgs::Point2D goal_pos; -}; - -#endif // REACTIVE_SYSTEM_NODE diff --git a/src/reactive_system_urc/include/ReactiveSystemPath.h b/src/reactive_system_urc/include/ReactiveSystemPath.h deleted file mode 100644 index 2890ccb43..000000000 --- a/src/reactive_system_urc/include/ReactiveSystemPath.h +++ /dev/null @@ -1,111 +0,0 @@ -/** - * Created by William Gu on Nov 3 2018 - * Declarations for Reactive System Pathfinder - * This node is responsible for moving the robot towards a given GPS goal while - * avoiding risky areas - */ - -#ifndef PROJECT_ReactiveSystemTwist_H -#define PROJECT_ReactiveSystemTwist_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define MAX_RISK 1 - -class ReactiveSystemPath { - public: - /** - * Entry point for pathfinding algorithm, produces a path message by - * generating multiple possible trajectories in a given range, assigning - * each a risk score, - * and choosing the trajectory with the lowest risk score - * @param risk_areas: risk area ros msg produced by risk assessment system - * @param goal_pos: current goal position - * @param traj_time_inc: size of unit increments in a trajectory - * calculation - * @param traj_num_incs: number of unit increments in a trajectory - * calculation - * @param linear_vel: current linear velocity of robot - * @param max_angular_vel: max turning velocity of robot - * @param num_angular_vel: number of turning velocities to test (each - * corresponds to different trajectory) - * @param risk_dist_tol_sq: squared "radius" of robot - * @return path of trajectory that minimizes trajectory score i.e. overall - * risk - */ - static nav_msgs::Path getPath(mapping_msgs_urc::RiskAreaArray risk_areas, - sb_geom_msgs::Point2D goal_pos, - float traj_time_inc, - int traj_num_incs, - float linear_vel, - float max_angular_vel, - int num_angular_vel, - float risk_dist_tol_sq); - - /** - * Calculates the arc trajectory given some twist command (linear and - * angular velocity), form of trajectory is point locations of the - * trajectory - * given at discrete time steps , and coordinates are in the frame of robot - * position - * @param linear_vel: linear x velocity of trajectory - * @param angular_vel: angular z velocity of trajectory - * @param time_inc: time increment step in seconds, dictates the distance - * between trajectory points (should not need to be too small) - * @param num_incs: number of point positions in trajectory (should not be - * too high but >= 1) - * @return list of point positions - */ - static std::vector getArcTrajectory( - float linear_vel, float angular_vel, float time_inc, int num_incs); - - /** - * Calculate the arc point in a given arc trajectory - * @ param center: center point to rotate origin about - * @ param angle: amount to rotate point by, in radians - * @return point position in arc - */ - static sb_geom_msgs::Point2D getArcPoint(sb_geom_msgs::Point2D center, - float angle); - - /** - * Calculate the risk score for a given trajectory, where higher scores are - * riskier - * @param trajectory: a proposed trajectory for the robot - * @param goal_pos: the current goal heading for the robot (in robot frame) - * @param risk_areas - * @param dist_tol_sq: the maximum distance for any given risk area to be - * within some trajectory point (should change based on robot dimensions), - * SQUARED - * @return a risk score for the given trajectory - */ - static float - getTrajectoryScore(std::vector trajectory, - sb_geom_msgs::Point2D goal_pos, - mapping_msgs_urc::RiskAreaArray risk_areas, - float dist_tol_sq); - - /** - * Helper for calcTrajectoryScore, calculates if two points are within some - * distance of another and returns true if so - * @param dist_tol_sq: distance tolerance, squared - * @return true if points are within dist_tol_sq - */ - static bool isWithinDistance(sb_geom_msgs::Point2D p1, - sb_geom_msgs::Point2D p2, - float dist_tol_sq); -}; - -#endif // PROJECT_ReactiveSystemTwist_H diff --git a/src/reactive_system_urc/package.xml b/src/reactive_system_urc/package.xml deleted file mode 100644 index fed7df3d0..000000000 --- a/src/reactive_system_urc/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - reactive_system_urc - 0.0.0 - The reactive_system_urc package - - - - - william - - - - - TODO - - - - - catkin - roscpp - sb_utils - - roscpp - sb_utils - roscpp - sb_utils - mapping_msgs_urc - - diff --git a/src/reactive_system_urc/src/ReactiveSystemNode.cpp b/src/reactive_system_urc/src/ReactiveSystemNode.cpp deleted file mode 100644 index 65c39730d..000000000 --- a/src/reactive_system_urc/src/ReactiveSystemNode.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/** - * Created by William Gu on Sept 29 2018 - * Implementation for Reactive System Node - * This node is a wrapper for ReactiveSystemPath logic, which subscribes to - * twist, current GPS goal, and risk area - * nodes, and publishes a path message to head towards the goal while avoiding - * risky areas - */ - -#include - -ReactiveSystemNode::ReactiveSystemNode(int argc, - char** argv, - std::string node_name) { - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - uint32_t queue_size = 1; - - /* Get ros params */ - - std::string traj_time_inc_param = "traj_time_inc"; - float default_traj_time_inc = 0.01; - SB_getParam( - private_nh, traj_time_inc_param, traj_time_inc, default_traj_time_inc); - - std::string traj_num_incs_param = "traj_num_incs"; - int default_traj_num_incs = 5; - SB_getParam( - private_nh, traj_num_incs_param, traj_num_incs, default_traj_num_incs); - - std::string linear_vel_param = "linear_vel"; - float default_linear_vel = 5; - SB_getParam(private_nh, linear_vel_param, linear_vel, default_linear_vel); - - std::string max_angular_vel_param = "max_angular_vel"; - float default_max_angular_vel = 5; - SB_getParam(private_nh, - max_angular_vel_param, - max_angular_vel, - default_max_angular_vel); - - std::string num_angular_vel_param = "num_angular_vel"; - int default_num_angular_vel = 10; - SB_getParam(private_nh, - num_angular_vel_param, - num_angular_vel, - default_num_angular_vel); - - std::string risk_dist_tol_sq_param = "risk_dist_tol_sq"; - float default_risk_dist_tol_sq = 10; - SB_getParam(private_nh, - risk_dist_tol_sq_param, - risk_dist_tol_sq, - default_risk_dist_tol_sq); - - // Initial goal is at current position - sb_geom_msgs::Point2D initial_goal; - initial_goal.x = 0; - initial_goal.y = 0; - goal_pos = initial_goal; - - /* Setup subscribers and publishers */ - - std::string risk_topic = "risk_areas"; - risk_subscriber = nh.subscribe( - risk_topic, queue_size, &ReactiveSystemNode::riskCallBack, this); - - std::string goal_topic = "goal"; - goal_subscriber = nh.subscribe( - goal_topic, queue_size, &ReactiveSystemNode::goalCallBack, this); - - std::string twist_topic = "path"; - path_publisher = - private_nh.advertise(twist_topic, queue_size); -} - -void ReactiveSystemNode::riskCallBack( -const mapping_msgs_urc::RiskAreaArray::ConstPtr& ptr) { - mapping_msgs_urc::RiskAreaArray risk_areas = *ptr; - - nav_msgs::Path path_msg = ReactiveSystemPath::getPath(risk_areas, - goal_pos, - traj_time_inc, - traj_num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - path_publisher.publish(path_msg); -} - -void ReactiveSystemNode::goalCallBack( -const sb_geom_msgs::Point2D::ConstPtr& ptr) { - sb_geom_msgs::Point2D goal = *ptr; - goal_pos = goal; // update current goal -} diff --git a/src/reactive_system_urc/src/ReactiveSystemPath.cpp b/src/reactive_system_urc/src/ReactiveSystemPath.cpp deleted file mode 100644 index 5362686d1..000000000 --- a/src/reactive_system_urc/src/ReactiveSystemPath.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/** - * Created by William Gu on Nov 3 2018 - * Implementation for Reactive System Pathfinder - */ - -#include - -nav_msgs::Path -ReactiveSystemPath::getPath(mapping_msgs_urc::RiskAreaArray risk_areas, - sb_geom_msgs::Point2D goal_pos, - float traj_time_inc, - int traj_num_incs, - float linear_vel, - float max_angular_vel, - int num_angular_vel, - float risk_dist_tol_sq) { - nav_msgs::Path path; - - float min_traj_score = std::numeric_limits::max(); - std::vector best_traj; - - // Find lowest trajectory score in the range of [-max_angular_vel, - // max_angular_vel], - // out of a total of "num_angular_vel" trajectories - for (int i = -num_angular_vel; i <= num_angular_vel; i++) { - float angular_vel = i * (max_angular_vel / num_angular_vel); - std::vector trajectory = - getArcTrajectory(linear_vel, angular_vel, traj_time_inc, traj_num_incs); - float traj_score = - getTrajectoryScore(trajectory, goal_pos, risk_areas, risk_dist_tol_sq); - - if (traj_score < min_traj_score) { - min_traj_score = traj_score; - best_traj = trajectory; - } - } - - // Convert best trajectory to a path - for (int i = 0; i < best_traj.size(); i++) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.pose.position.x = best_traj[i].x; - pose_stamped.pose.position.y = best_traj[i].y; - path.poses.push_back(pose_stamped); - } - - return path; -} - -std::vector ReactiveSystemPath::getArcTrajectory( -float linear_vel, float angular_vel, float time_inc, int num_incs) { - std::vector arc_trajectory; - - if (abs(angular_vel) < 0.01) { // essentially a straight trajectory - for (int i = 1; i <= num_incs; i++) { - sb_geom_msgs::Point2D arc_point; - arc_point.x = time_inc * i * linear_vel; - arc_point.y = 0; - arc_trajectory.push_back(arc_point); - } - } else { // use arc formula to obtain trajectory - sb_geom_msgs::Point2D center; - center.x = 0; - center.y = linear_vel / angular_vel; - - for (int i = 1; i <= num_incs; i++) { - float angle = time_inc * i * angular_vel; - sb_geom_msgs::Point2D arc_point = - ReactiveSystemPath::getArcPoint(center, angle); - arc_trajectory.push_back(arc_point); - } - } - - return arc_trajectory; -} - -sb_geom_msgs::Point2D -ReactiveSystemPath::getArcPoint(sb_geom_msgs::Point2D center, float angle) { - // Calculate the arc point in a given arc trajectory by rotating (0,0) about - // the center point, by angle - sb_geom_msgs::Point2D arc_point; - - arc_point.x = center.y * sin(angle); - arc_point.y = center.y - center.y * cos(angle); - - return arc_point; -} - -float ReactiveSystemPath::getTrajectoryScore( -std::vector trajectory, -sb_geom_msgs::Point2D goal_pos, -mapping_msgs_urc::RiskAreaArray risk_areas, -float dist_tol_sq) { - float risk_score = 0; - - // Give each point in the trajectory a riskiness based on proximity to risk - // areas and immediate proximity to current position - // Add all the risks for each point for a risk sum for the trajectory - for (int i = 0; i < trajectory.size(); i++) { - sb_geom_msgs::Point2D traj_point = trajectory[i]; - // risk score for a given trajectory point, set to - // -1 since actual risks should be >= 0 - float traj_point_risk = -1; - for (mapping_msgs_urc::RiskArea area : - risk_areas.areas) { // Check all points for all risk polygons - for (sb_geom_msgs::Point2D risk_point : area.area.points) { - if (isWithinDistance(risk_point, traj_point, dist_tol_sq)) { - if (area.score.data > traj_point_risk) // Keep track of the - // highest risk for this - // trajectory point - traj_point_risk = area.score.data; - break; - } - } - } - if (traj_point_risk < - 0) // unknown risk, so just assign it the maximum risk - traj_point_risk = MAX_RISK; - - // Multiplier applied to points based on proximity to current position - // (closer = higher multiplier) - traj_point_risk *= (trajectory.size() - i); - risk_score += traj_point_risk; - } - - // Apply an additive value to the total risk score for a given trajectory, - // based on how close it heads towards the goal (closer = lower multiplier) - float goal_ang = atan2(goal_pos.y, goal_pos.x); - float traj_ang = - atan2(trajectory[trajectory.size() / 2].y, - trajectory[trajectory.size() / 2].x); // approximate trajectory angle - // from the line b/w last point - // in the trajectory and origin - float ang_diff = abs(goal_ang - traj_ang); - float goal_adder = ang_diff * MAX_RISK; // apply a multiplier to the amount - // that goal difference effects risk - // level - risk_score += goal_adder; - - return risk_score; -} - -bool ReactiveSystemPath::isWithinDistance(sb_geom_msgs::Point2D p1, - sb_geom_msgs::Point2D p2, - float dist_tol_sq) { - return (pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) < dist_tol_sq); -} diff --git a/src/reactive_system_urc/src/reactive_system_node.cpp b/src/reactive_system_urc/src/reactive_system_node.cpp deleted file mode 100644 index 0f7bcb0ac..000000000 --- a/src/reactive_system_urc/src/reactive_system_node.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/** - * Created by William Gu on Sept 29 2018 - * Runs the reactive system node - */ - -#include - -int main(int argc, char** argv) { - // Set up ROS node - std::string node_name = "reactive_system_node"; - - // Create an instance of the class - ReactiveSystemNode node(argc, argv, node_name); - - // Start up - ros::spin(); - - // Once node stops, return 0 - return 0; -} diff --git a/src/reactive_system_urc/test/RiskAreaBuilder.h b/src/reactive_system_urc/test/RiskAreaBuilder.h deleted file mode 100644 index 1ce319239..000000000 --- a/src/reactive_system_urc/test/RiskAreaBuilder.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - * Created By: William Gu - * Created On: Jan 19 2019 - * Description: Helper class for unit testing, to create fake risk area ROS - * messages - */ - -#ifndef PROJECT_RISKAREABUILDER_H -#define PROJECT_RISKAREABUILDER_H - -#include -#include - -namespace RiskAreaBuilder { -class RiskAreaBuilder { - public: - /** - * Initially constructs a large rectangular area (corner to corner) of risk - * areas with 0 risk scores assigned - * @param corner1: should be upper left corner (so corner1.x < corner2.x, - * and corner1.y < corner2.y) - * @param corner2: should be bottom right corner - * @param risk_length, the length for a single risk area - */ - RiskAreaBuilder(sb_geom_msgs::Point2D corner1, - sb_geom_msgs::Point2D corner2, - float risk_length) { - mapping_msgs_urc::RiskAreaArray risk_areas; - - for (float i = corner1.x; i <= corner2.x; i += risk_length) { - for (float j = corner1.y; j <= corner2.x; j += risk_length) { - mapping_msgs_urc::RiskArea area; - sb_geom_msgs::Point2D p1; - sb_geom_msgs::Point2D p2; - sb_geom_msgs::Point2D p3; - sb_geom_msgs::Point2D p4; - p1.x = i; - p1.y = j; - p2.x = i; - p2.y = j + risk_length; - p3.x = i + risk_length; - p3.y = j; - p4.x = i + risk_length; - p4.y = j + risk_length; - area.area.points.push_back(p1); - area.area.points.push_back(p2); - area.area.points.push_back(p3); - area.area.points.push_back(p4); - area.score.data = 0; // lowest possible risk - risk_areas.areas.push_back(area); - } - } - risk_areas_ = risk_areas; - } - - /** - * Adds a risk zone to the current risk area map - * @param risk_center: center of risk - * @param radius: how close risk areas must be to center to get assigned - * risk - * @param score: score to assign each risk area near risk center - */ - void - addRiskZone(sb_geom_msgs::Point2D risk_center, float radius, float score) { - for (int i = 0; i < risk_areas_.areas.size(); i++) { - if (isWithinRadius(risk_areas_.areas[i], risk_center, radius)) { - risk_areas_.areas[i].score.data = score; - } - } - } - - mapping_msgs_urc::RiskAreaArray getRiskArray() { return risk_areas_; } - - private: - mapping_msgs_urc::RiskAreaArray risk_areas_; - - bool isWithinRadius(mapping_msgs_urc::RiskArea area, - sb_geom_msgs::Point2D risk_center, - float radius) { - for (sb_geom_msgs::Point2D point : area.area.points) { - if (pow(point.x - risk_center.x, 2) + - pow(point.y - risk_center.y, 2) < - pow(radius, 2)) { - return true; - } - } - return false; - } -}; -} // namespace RiskAreaBuilder - -#endif // PROJECT_RISKAREABUILDER_H diff --git a/src/reactive_system_urc/test/reactive-system-test.cpp b/src/reactive_system_urc/test/reactive-system-test.cpp deleted file mode 100644 index 09da9838b..000000000 --- a/src/reactive_system_urc/test/reactive-system-test.cpp +++ /dev/null @@ -1,581 +0,0 @@ -/* - * Created By: William Gu - * Created On: Jan 19 2019 - * Description: G-unit tests for Reactive System - */ - -#include -#include -#include - -static void printPoints(std::vector points) { - for (int i = 0; i < points.size(); i++) { - std::cout << points[i].x << "," << points[i].y << std::endl; - } -} - -/* - * Get the average angle given some path - */ -static float getAvgAngle(nav_msgs::Path path) { - float sum_x = 0; - float sum_y = 0; - - for (int i = 0; i < path.poses.size(); i++) { - sum_x += path.poses[i].pose.position.x; - sum_y += path.poses[i].pose.position.y; - } - - return atan2(sum_y, sum_x); -} - -TEST(ReactiveSystem, straightTraj) { - float linear_vel = 1; - float angular_vel = 0; - float time_inc = 0.5; - int num_incs = 3; - - std::vector traj = - ReactiveSystemPath::getArcTrajectory( - linear_vel, angular_vel, time_inc, num_incs); - - EXPECT_FLOAT_EQ(traj[0].x, 0.5); - EXPECT_FLOAT_EQ(traj[0].y, 0); - EXPECT_FLOAT_EQ(traj[1].x, 1); - EXPECT_FLOAT_EQ(traj[1].y, 0); - EXPECT_FLOAT_EQ(traj[2].x, 1.5); - EXPECT_FLOAT_EQ(traj[2].y, 0); -} - -TEST(ReactiveSystem, curvedTraj) { - float linear_vel = 1; - float angular_vel = 1; - float time_inc = 0.2; - int num_incs = 5; - - std::vector traj = - ReactiveSystemPath::getArcTrajectory( - linear_vel, angular_vel, time_inc, num_incs); - - EXPECT_NEAR(traj[0].x, 0.199, 0.001); - EXPECT_NEAR(traj[0].y, 0.020, 0.001); - EXPECT_NEAR(traj[1].x, 0.389, 0.001); - EXPECT_NEAR(traj[1].y, 0.079, 0.001); - EXPECT_NEAR(traj[2].x, 0.565, 0.001); - EXPECT_NEAR(traj[2].y, 0.175, 0.001); - EXPECT_NEAR(traj[3].x, 0.717, 0.001); - EXPECT_NEAR(traj[3].y, 0.303, 0.001); - EXPECT_NEAR(traj[4].x, 0.841, 0.001); - EXPECT_NEAR(traj[4].y, 0.460, 0.001); -} - -TEST(ReactiveSystem, curvedTrajOpposite) { - float linear_vel = 1; - float angular_vel = -1; - float time_inc = 0.2; - int num_incs = 5; - - std::vector traj = - ReactiveSystemPath::getArcTrajectory( - linear_vel, angular_vel, time_inc, num_incs); - - EXPECT_NEAR(traj[0].x, 0.199, 0.001); - EXPECT_NEAR(traj[0].y, -0.020, 0.001); - EXPECT_NEAR(traj[1].x, 0.389, 0.001); - EXPECT_NEAR(traj[1].y, -0.079, 0.001); - EXPECT_NEAR(traj[2].x, 0.565, 0.001); - EXPECT_NEAR(traj[2].y, -0.175, 0.001); - EXPECT_NEAR(traj[3].x, 0.717, 0.001); - EXPECT_NEAR(traj[3].y, -0.303, 0.001); - EXPECT_NEAR(traj[4].x, 0.841, 0.001); - EXPECT_NEAR(traj[4].y, -0.460, 0.001); -} - -TEST(ReactiveSystem, riskAreaBuilder) { - float risk_score = MAX_RISK; - float risk_radius = 1; - sb_geom_msgs::Point2D risk_center; - risk_center.x = 2; - risk_center.y = 0; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone( - risk_center, risk_radius, risk_score); // rad = 1, score = 5 - - mapping_msgs_urc::RiskAreaArray risks = builder.getRiskArray(); - - // For testing that riskareabuilder is correct - for (mapping_msgs_urc::RiskArea area : risks.areas) { - if (area.score.data == risk_score) { // confirm that risk scores were - // added in correct positions - EXPECT_NEAR(pow(area.area.points[0].x - risk_center.x, 2) + - pow(area.area.points[0].y - risk_center.y, 2), - 0, - pow(risk_radius, 2) + 0.15); - } - } -} - -// Test the given twist, for a scenario where an object is directly between our -// position and the goal -TEST(ReactiveSystem, objectInDirectPath) { - // object parameters - float risk_score = MAX_RISK; - float risk_radius = 1; - sb_geom_msgs::Point2D risk_center; - risk_center.x = 2; - risk_center.y = 0; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone(risk_center, risk_radius, risk_score); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 3; - goal.y = 0; - - float linear_vel = 1; - float max_angular_vel = 1.5; - int num_angular_vel = 7; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_GE(fabs(path_ang), - fabs(atan2(risk_center.y + risk_radius, risk_center.x))); -} - -// Test the given twist, for a scenario where 3 objects are directly between our -// position and the goal (covering a greater "width) -TEST(ReactiveSystem, threeObjectsInDirectPath) { - // object parameters - float risk_score = MAX_RISK; - float risk_radius = 1; - - sb_geom_msgs::Point2D risk_center1; - risk_center1.x = 2; - risk_center1.y = 0; - - sb_geom_msgs::Point2D risk_center2; - risk_center2.x = 2; - risk_center2.y = 2; - - sb_geom_msgs::Point2D risk_center3; - risk_center3.x = 2; - risk_center3.y = -2; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone(risk_center1, risk_radius, risk_score); - builder.addRiskZone(risk_center2, risk_radius, risk_score); - builder.addRiskZone(risk_center3, risk_radius, risk_score); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 3; - goal.y = 0; - - float linear_vel = 1; - float max_angular_vel = 2; - int num_angular_vel = 10; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_GE(fabs(path_ang), - fabs(atan2(risk_center2.y + risk_radius, risk_center2.x))); -} - -// Test the given twist, for a scenario where no objects are in the way at all -TEST(ReactiveSystem, noObjectsInDirectPath) { - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 3; - goal.y = 0; - - float linear_vel = 1; - float max_angular_vel = 2; - int num_angular_vel = 10; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_EQ(path_ang, 0); -} - -// Test the given twist, for a scenario where there are two objects but a gap -// exists in the center -TEST(ReactiveSystem, twoObjectsOnSides) { - // object parameters - float risk_score = MAX_RISK; - float risk_radius = 1; - - sb_geom_msgs::Point2D risk_center1; - risk_center1.x = 2; - risk_center1.y = -2; - - sb_geom_msgs::Point2D risk_center2; - risk_center2.x = 2; - risk_center2.y = 2; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone(risk_center1, risk_radius, risk_score); - builder.addRiskZone(risk_center2, risk_radius, risk_score); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 3; - goal.y = 0; - - float linear_vel = 1; - float max_angular_vel = 2; - int num_angular_vel = 10; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_EQ(path_ang, 0); -} - -// Test the given twist, for a scenario where there are two objects but a space -// exists to the "right" -TEST(ReactiveSystem, openingToRight) { - // object parameters - float risk_score = MAX_RISK; - float risk_radius = 1; - - sb_geom_msgs::Point2D risk_center1; - risk_center1.x = 2; - risk_center1.y = 0; - - sb_geom_msgs::Point2D risk_center2; - risk_center2.x = 2; - risk_center2.y = 2; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone(risk_center1, risk_radius, risk_score); - builder.addRiskZone(risk_center2, risk_radius, risk_score); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 3; - goal.y = 0; - - float linear_vel = 1; - float max_angular_vel = 2; - int num_angular_vel = 10; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_LE(path_ang, atan2(risk_center1.y - risk_radius, risk_center1.x)); -} - -// Test the given twist, for a scenario where there are three objects but a gap -// exists between two objects to the "right" -TEST(ReactiveSystem, gapToRight) { - // object parameters - float risk_score = MAX_RISK; - float risk_radius = 0.5; - - sb_geom_msgs::Point2D risk_center1; - risk_center1.x = 3; - risk_center1.y = 0; - - sb_geom_msgs::Point2D risk_center2; - risk_center2.x = 3; - risk_center2.y = 1; - - sb_geom_msgs::Point2D risk_center3; - risk_center3.x = 3; - risk_center3.y = -2; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone(risk_center1, risk_radius, risk_score); - builder.addRiskZone(risk_center2, risk_radius, risk_score); - builder.addRiskZone(risk_center3, risk_radius, risk_score); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 5; - goal.y = 0; - - float linear_vel = 1; - float max_angular_vel = 2; - int num_angular_vel = 10; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_LE(path_ang, atan2(risk_center1.y - risk_radius, risk_center1.x)); - EXPECT_GE(path_ang, atan2(risk_center3.y + risk_radius, risk_center3.x)); -} - -// Test the given twist, for a scenario where no objects are in the way at all, -// but goal is not directly straight -TEST(ReactiveSystem, goalToLeft) { - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 4; - goal.y = 2; - - float linear_vel = 1; - float max_angular_vel = 2; - int num_angular_vel = 10; - float time_inc = 0.2; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_NEAR(path_ang, atan2(goal.y, goal.x), 0.5); -} - -// Test the given twist, for a scenario where there are three objects but a gap -// exists between two objects to the "right", BUT goal is to the left behind the -// objects -TEST(ReactiveSystem, goalToLeftAndGapToRight) { - // object parameters - float risk_score = MAX_RISK; - float risk_radius = 0.5; - - sb_geom_msgs::Point2D risk_center1; - risk_center1.x = 3; - risk_center1.y = 0; - - sb_geom_msgs::Point2D risk_center2; - risk_center2.x = 3; - risk_center2.y = 1; - - sb_geom_msgs::Point2D risk_center3; - risk_center3.x = 3; - risk_center3.y = -2; - - sb_geom_msgs::Point2D risk_center4; - risk_center4.x = 2; - risk_center4.y = 1; - - sb_geom_msgs::Point2D risk_center5; - risk_center5.x = 2.5; - risk_center5.y = 3; - - sb_geom_msgs::Point2D risk_center6; - risk_center6.x = 2; - risk_center6.y = 4; - - sb_geom_msgs::Point2D risk_center7; - risk_center6.x = 1; - risk_center6.y = 1; - - sb_geom_msgs::Point2D corner1; - corner1.x = 0; - corner1.y = -2; - - sb_geom_msgs::Point2D corner2; - corner2.x = 4; - corner2.y = 2; - - float risk_length = 0.05; - RiskAreaBuilder::RiskAreaBuilder builder(corner1, corner2, risk_length); - - builder.addRiskZone(risk_center1, risk_radius, risk_score); - builder.addRiskZone(risk_center2, risk_radius, risk_score); - builder.addRiskZone(risk_center3, risk_radius, risk_score); - builder.addRiskZone(risk_center4, risk_radius, risk_score); - builder.addRiskZone(risk_center5, risk_radius, risk_score); - builder.addRiskZone(risk_center6, risk_radius, risk_score); - - mapping_msgs_urc::RiskAreaArray risk_areas = builder.getRiskArray(); - - sb_geom_msgs::Point2D goal; - goal.x = 5; - goal.y = 2; - - float linear_vel = 1; - float max_angular_vel = 1.5; - int num_angular_vel = 10; - float time_inc = 0.3; - int num_incs = 10; - - float risk_dist_tol_sq = 0.4; - - nav_msgs::Path path = ReactiveSystemPath::getPath(risk_areas, - goal, - time_inc, - num_incs, - linear_vel, - max_angular_vel, - num_angular_vel, - risk_dist_tol_sq); - - float path_ang = getAvgAngle(path); - EXPECT_NEAR(path_ang, atan2(-1, 3), 0.5); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/sb_gazebo/scripts/teleop_twist_keyboard b/src/sb_gazebo/scripts/teleop_twist_keyboard old mode 100755 new mode 100644 diff --git a/src/sb_laserscan_processing/CMakeLists.txt b/src/sb_laserscan_processing/CMakeLists.txt deleted file mode 100644 index 022435b36..000000000 --- a/src/sb_laserscan_processing/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sb_laserscan_processing) - -# 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 -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - mapping_igvc - sb_utils -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp -) - - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ./include - ./test - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ executable -add_executable(cone_extractor_node - src/cone_extractor_node.cpp - src/ConeExtractorNode.cpp - include/ConeExtractorNode.h - src/ConeIdentification.cpp - include/ConeIdentification.h - ) - - -## Specify libraries to link a library or executable target against -# target_link_libraries(sample_package_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(cone_extractor_node - ${catkin_LIBRARIES} - ) - - -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - - # Adding gtests to the package - #catkin_add_gtest(laserscan-cone-manager-test test/laserscan-cone-manager-test.cpp src/LaserscanConeManager.cpp) - #target_link_libraries(laserscan-cone-manager-test ${catkin_LIBRARIES}) - - catkin_add_gtest(cone-identification-test test/cone-identification-test.cpp src/ConeIdentification.cpp test/LaserscanBuilder.h) - target_link_libraries(cone-identification-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(laserscan_cone_manager_rostest test/sb_laserscan_processing_test.test test/laserscan_cone_manager_rostest.cpp) - #target_link_libraries(laserscan_cone_manager_rostest ${catkin_LIBRARIES}) -endif() diff --git a/src/sb_laserscan_processing/include/ConeExtractorNode.h b/src/sb_laserscan_processing/include/ConeExtractorNode.h deleted file mode 100644 index f22d02668..000000000 --- a/src/sb_laserscan_processing/include/ConeExtractorNode.h +++ /dev/null @@ -1,52 +0,0 @@ -/** - * Created by William Gu on 24/03/18. - * Class declaration for a Cone Extractor Node that identifies cones from a - * laser msg - */ - -#ifndef LASERSCAN_CONE_MANAGER_H -#define LASERSCAN_CONE_MANAGER_H - -#include -#include -#include -#include -#include -#include -#include - -class ConeExtractorNode { - public: - ConeExtractorNode(int argc, char** argv, std::string node_name); - - private: - ros::Subscriber laser_subscriber; - ros::Publisher cone_publisher; - ros::Publisher rviz_publisher; - - /** - * Callback function for receiving laser scan msgs. Publishes the cones - * found in the laserscan to - * the correct topic one by one. Note that the coordinates for cones are in - * the base-link (robot frame) - * @param ptr - */ - void laserCallBack(const sensor_msgs::LaserScan::ConstPtr& ptr); - - /** - * Generate a visualization marker given a cone - * @param cone - * @return a visualization marker - */ - void publishMarkers(std::vector cones); - - double cone_dist_tol; // Distance tolerance between cones in cluster - double cone_rad_exp; // Expected cone radius - double cone_rad_tol; // Tolerance for cone radius (max diff between - // calculated and expected values) - int min_points_in_cone; // Index difference between points used in edge - // cluster splitting algorithm - double ang_threshold; // Max angle needed to split edge clusters -}; - -#endif // LASERSCAN_CONE_MANAGER_H diff --git a/src/sb_laserscan_processing/include/ConeIdentification.h b/src/sb_laserscan_processing/include/ConeIdentification.h deleted file mode 100644 index 8fcc992ce..000000000 --- a/src/sb_laserscan_processing/include/ConeIdentification.h +++ /dev/null @@ -1,135 +0,0 @@ -/** - * Created by William Gu on 24/03/18. - * Class declaration for Cone Identification, which contains static methods for - * identifying cones in a laser message - */ -#ifndef CONEIDENTIFICATION_H -#define CONEIDENTIFICATION_H - -#include -#include -#include -#include -#include -#include - -class ConeIdentification { - public: - /** - * Identifies the cones in a given laserscan message. Note that all cones - * will have radius of the expected radius, and only cones that - * have a calculated radius within radius_tol of expected radius will be - * added to the resulting identified cones. - * @param laser_msg laserscan message to analyze - * @param dist_tol max distance between points to be considered that are - * part of one cone - * @param radius_exp expected radius of cone - * @param radius_tol maximum difference between calculated cone radius and - * expected cone radius - * @param min_points_in_cone the minimum number of points that can represent - * a valid cone - * @param ang_tol the max angle for a split to be considered (in radians) - * @return a vector of cone obstacle messages identified - */ - static std::vector - identifyCones(const sensor_msgs::LaserScan& laser_msg, - double dist_tol, - double radius_exp, - double radius_tol, - int min_points_in_cone, - double ang_threshold); - - /** - * Identifies valid cones that can be made from points in edge_points and - * adds it to the identified_cones input vector - * Note that multiple cones may be added in this function call - * @param identified_cones cones identified so far - * @param edge_points edge points (in a cluster) to analyze - * @param radius_exp expected radius of cone - * @param radius_tol maximum difference between calculated and expected cone - * radius - * @param min_points_in_cone the minimum number of points that can represent - * a valid cone - * @param ang_threshold the max angle for a split to be considered (in - * radians) - * @param frame_id frame id used to generate edge_points (typically same as - * the laserscan msg) - */ - static void addConesInEdgeCluster( - std::vector& identified_cones, - std::vector& edge_points, - double radius_exp, - double radius_tol, - int min_points_in_cone, - double ang_threshold, - std::string frame_id); - - /** - * Splits a cluster of edge points such that points forming multiple cones - * extremely close to one another will be returned - * as vectors of points where each vector represents one cone - * @param edge_points - * @param line_point_dist the number of points to take before and after each - * edge point to form a line and test angle - * @param ang_tol the max angle for a split to be considered (in radians) - * @return a vector consisting of split edge point vectors - */ - static std::vector> - splitEdge(const std::vector& edge_points, - int line_point_dist, - double ang_threshold); - - /** - * Converts a cluster of edge points to a cone obstacle with a predicted - * radius - * @param edge_points should have size >= 3 and be in the order they appear - * in the object - * @return a cone formed by edge points - */ - static mapping_igvc::ConeObstacle - edgeToCone(const std::vector& edge_points); - - /** - * Converts a laserscan reading to a point - * @param dist distance reading, should be in valid min-max range of laser - * scan - * @param ang angle reading, should be in valid min-max angle of laser scan - * @return point in 2d - */ - static mapping_igvc::Point2D laserToPoint(double dist, double ang); - - /** - * Gets the distance between 2 points - * @param p1 point 1 - * @param p2 point 2 - * @return distance between points - */ - static double getDist(const mapping_igvc::Point2D& p1, - const mapping_igvc::Point2D& p2); - - /** - * Get the mean x coordinate of points in edge_points - * @param edge_points - * @return mean x coordinate - */ - static double - getMeanX(const std::vector& edge_points); - - /** - * Get the mean y coordinate of points in edge_points - * @param edge_points - * @return mean y coordinate - */ - static double - getMeanY(const std::vector& edge_points); - - /** - * Get the slope of the regression line formed by points in edge_points - * @param edge_points - * @return slope of line - */ - static double - getRegressionSlope(const std::vector& edge_points); -}; - -#endif // CONEIDENTIFICATION_H diff --git a/src/sb_laserscan_processing/launch/cone_extractor.launch b/src/sb_laserscan_processing/launch/cone_extractor.launch deleted file mode 100644 index 1acc3492e..000000000 --- a/src/sb_laserscan_processing/launch/cone_extractor.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/sb_laserscan_processing/package.xml b/src/sb_laserscan_processing/package.xml deleted file mode 100644 index eafc7bb4d..000000000 --- a/src/sb_laserscan_processing/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - sb_laserscan_processing - 0.0.0 - The sb_laserscan_processing package - - - - - william - - - - - TODO - - - - - catkin - mapping_igvc - roscpp - sb_utils - mapping_igvc - roscpp - sb_utils - mapping_igvc - roscpp - sb_utils - - diff --git a/src/sb_laserscan_processing/src/ConeExtractorNode.cpp b/src/sb_laserscan_processing/src/ConeExtractorNode.cpp deleted file mode 100644 index 01fd45fcb..000000000 --- a/src/sb_laserscan_processing/src/ConeExtractorNode.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/** - * Created by William Gu on 24/03/18 - * Implementation for a Cone Extractor Node that identifies cones from a laser - * msg - */ - -#include - -ConeExtractorNode::ConeExtractorNode(int argc, - char** argv, - std::string node_name) { - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - uint32_t queue_size = 1; - - /* Get ros params */ - std::string cone_dist_tol_param = "cone_dist_tol"; - double default_cone_dist_tol = 0.01; - SB_getParam( - private_nh, cone_dist_tol_param, cone_dist_tol, default_cone_dist_tol); - - std::string cone_rad_exp_param = "cone_rad_exp"; - double default_cone_rad_exp = 0.3; - SB_getParam( - private_nh, cone_rad_exp_param, cone_rad_exp, default_cone_rad_exp); - - std::string cone_rad_tol_param = "cone_rad_tol"; - double default_cone_rad_tol = 0.1; // Change later - SB_getParam( - private_nh, cone_rad_tol_param, cone_rad_tol, default_cone_rad_tol); - - std::string min_points_in_cone_param = "min_points_in_cone"; - int default_min_points_in_cone = 5; // Change later - SB_getParam(private_nh, - min_points_in_cone_param, - min_points_in_cone, - default_min_points_in_cone); - - std::string ang_threshold_param = "ang_threshold"; - double default_ang_threshold = 2.3; // Change later - SB_getParam( - private_nh, ang_threshold_param, ang_threshold, default_ang_threshold); - - std::string subscribe_topic = - "/robot/laser/scan"; // Setup subscriber to laserscan (Placeholder) - laser_subscriber = nh.subscribe( - subscribe_topic, queue_size, &ConeExtractorNode::laserCallBack, this); - - std::string output_cone_topic = "output_cone_obstacle"; // Placeholder - cone_publisher = private_nh.advertise( - output_cone_topic, queue_size); - - std::string marker_topic = "markers"; // Placeholder - rviz_publisher = - private_nh.advertise(marker_topic, queue_size); -} - -void ConeExtractorNode::laserCallBack( -const sensor_msgs::LaserScan::ConstPtr& ptr) { - sensor_msgs::LaserScan laser_msg = *ptr; - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - cone_dist_tol, - cone_rad_exp, - cone_rad_tol, - min_points_in_cone, - ang_threshold); - for (int i = 0; i < cones.size(); i++) { // Publish cones individually - cone_publisher.publish(cones[i]); - } - publishMarkers(cones); -} - -void ConeExtractorNode::publishMarkers( -std::vector cones) { - if (cones.empty()) return; - - visualization_msgs::Marker marker; - - marker.id = 0; - marker.header.frame_id = cones[0].header.frame_id; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.orientation.w = 1.0; - - marker.type = visualization_msgs::Marker::POINTS; - - // POINTS markers use x and y scale for width/height respectively - marker.scale.x = 0.1; - marker.scale.y = 0.1; - - // Points are green - marker.color.g = 1.0f; - marker.color.a = 1.0; - - for (int i = 0; i < cones.size(); i++) { - geometry_msgs::Point coneCenter; - coneCenter.x = cones[i].center.x; - coneCenter.y = cones[i].center.y; - marker.points.push_back(coneCenter); - } - - rviz_publisher.publish(marker); -} \ No newline at end of file diff --git a/src/sb_laserscan_processing/src/ConeIdentification.cpp b/src/sb_laserscan_processing/src/ConeIdentification.cpp deleted file mode 100644 index 5d37035a0..000000000 --- a/src/sb_laserscan_processing/src/ConeIdentification.cpp +++ /dev/null @@ -1,298 +0,0 @@ -/** - * Created by William Gu on 24/03/18 - * Implementation of Cone Identification methods for identifying cones from a - * laser msg - */ - -#include - -std::vector -ConeIdentification::identifyCones(const sensor_msgs::LaserScan& laser_msg, - double dist_tol, - double radius_exp, - double radius_tol, - int min_points_in_cone, - double ang_threshold) { - std::vector identified_cones; - std::vector - edge_points; // Represents points in a potential cluster - std::string frame_id = laser_msg.header.frame_id; - - int numIndices = - (laser_msg.angle_max - laser_msg.angle_min) / laser_msg.angle_increment; - for (int i = 0; i < numIndices; i++) { - if (laser_msg.ranges[i] > laser_msg.range_max || - laser_msg.ranges[i] < - laser_msg - .range_min) { // Check if curr laserscan point is in invalid range - addConesInEdgeCluster(identified_cones, - edge_points, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold, - frame_id); - edge_points.clear(); - } else { // Laserscan point in range - - mapping_igvc::Point2D point = - laserToPoint(laser_msg.ranges[i], - laser_msg.angle_min + - i * laser_msg.angle_increment); // Convert to x-y point - - if (!edge_points.empty() && - getDist(edge_points.back(), point) > - dist_tol) { // If out of dist tolerance, analyze points so far, - // clear, then add new point - addConesInEdgeCluster(identified_cones, - edge_points, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold, - frame_id); - edge_points.clear(); - edge_points.push_back(point); - } else if (i == numIndices - 1) { // If out of points, analyze edge - // points so far to create cone, - // and clear edge_points - edge_points.push_back(point); - addConesInEdgeCluster(identified_cones, - edge_points, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold, - frame_id); - edge_points.clear(); - } else { - edge_points.push_back(point); - } - } - } - return identified_cones; -} - -void ConeIdentification::addConesInEdgeCluster( -std::vector& identified_cones, -std::vector& edge_points, -double radius_exp, -double radius_tol, -int min_points_in_cone, -double ang_threshold, -std::string frame_id) { - if (edge_points.size() >= - min_points_in_cone * 2 + 1) { // Needs minimum number of points to split - // a potential multi-cone cluster - - std::vector> split_edges = splitEdge( - edge_points, min_points_in_cone, ang_threshold); // Split edge points if - // multiple cones - // detected (Temp - // values right now as - // params) - - for (int i = 0; i < split_edges.size(); i++) { - if (split_edges[i].size() >= min_points_in_cone) { - mapping_igvc::ConeObstacle potential_cone = - edgeToCone(split_edges[i]); - - if (fabs(potential_cone.radius - radius_exp) <= - radius_tol) { // Within expected radius, valid cone - - potential_cone.radius = radius_exp; - potential_cone.header.frame_id = frame_id; - potential_cone.header.stamp = ros::Time::now(); - identified_cones.push_back(potential_cone); - } - } - } - } else if (edge_points.size() >= - min_points_in_cone) { // Directly make the cone if not enough - // points to split (cone still needs at - // least 3 edge points though) - mapping_igvc::ConeObstacle potential_cone = edgeToCone(edge_points); - - if (fabs(potential_cone.radius - radius_exp) <= - radius_tol) { // Within expected radius, valid cone - potential_cone.radius = radius_exp; - potential_cone.header.frame_id = frame_id; - potential_cone.header.stamp = ros::Time::now(); - identified_cones.push_back(potential_cone); - } - } -} - -mapping_igvc::Point2D ConeIdentification::laserToPoint(double dist, - double ang) { - mapping_igvc::Point2D point = mapping_igvc::Point2D(); - point.x = dist * cos(ang); - point.y = dist * sin(ang); - return point; -} - -double ConeIdentification::getDist(const mapping_igvc::Point2D& p1, - const mapping_igvc::Point2D& p2) { - return sqrt(pow((p1.x - p2.x), 2) + pow((p1.y - p2.y), 2)); -} - -std::vector> ConeIdentification::splitEdge( -const std::vector& edge_points, -int min_points_in_cone, -double ang_threshold) { - std::vector> split_edges; - - std::vector angles; // index 0 corresponds to index - // min_points_in_cone of edge_points vector - for (int i = min_points_in_cone; - i < edge_points.size() - min_points_in_cone; - i++) { - // Calculate the angle between the two lines we form - double AB_x = edge_points[i].x - edge_points[i - min_points_in_cone].x; - double AB_y = edge_points[i].y - edge_points[i - min_points_in_cone].y; - double CB_x = edge_points[i].x - edge_points[i + min_points_in_cone].x; - double CB_y = edge_points[i].y - edge_points[i + min_points_in_cone].y; - double dot = AB_x * CB_x + AB_y * CB_y; - double cross = AB_x * CB_y - AB_y * CB_x; - double ang = -atan2(cross, dot); - if (ang < 0) ang += 2 * M_PI; - - angles.push_back(ang); - } - - // Find local mins, then check if they are below threshold - if true, mark - // as index to split at - std::vector splitIndices; - for (int i = 1; i < angles.size() - 1; i++) { - bool isLocalMin = true; - for (int j = i - min_points_in_cone; j <= i + min_points_in_cone; j++) { - if (angles[j] < angles[i]) { - isLocalMin = false; - break; - } - } - if (angles[i] < ang_threshold && isLocalMin) - splitIndices.push_back(i + min_points_in_cone); - } - - // Split edges based on qualified local mins - size_t lastIndex = 0; - for (int i = 0; i < splitIndices.size(); i++) { - std::vector split( - edge_points.begin() + lastIndex, - edge_points.begin() + splitIndices[i] + 1); - split_edges.push_back(split); - lastIndex = splitIndices[i]; - } - std::vector lastSplit( - edge_points.begin() + lastIndex, - edge_points.end()); // Add last cluster of edges - split_edges.push_back(lastSplit); - - return split_edges; -} - -mapping_igvc::ConeObstacle ConeIdentification::edgeToCone( -const std::vector& edge_points) { - mapping_igvc::ConeObstacle cone = mapping_igvc::ConeObstacle(); - - int i, iter, IterMAX = 99; - - double Xi, Yi, Zi; - double Mz, Mxy, Mxx, Myy, Mxz, Myz, Mzz, Cov_xy, Var_z; - double A0, A1, A2, A22; - double Dy, xnew, x, ynew, y; - double DET, Xcenter, Ycenter; - - // Compute x- and y- sample means (via a function in the class "data") - double meanX = getMeanX(edge_points); - double meanY = getMeanY(edge_points); - - // computing moments - Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.; - for (i = 0; i < edge_points.size(); i++) { - Xi = edge_points[i].x - meanX; // centered x-coordinates - Yi = edge_points[i].y - meanY; // centered y-coordinates - Zi = Xi * Xi + Yi * Yi; - - Mxy += Xi * Yi; - Mxx += Xi * Xi; - Myy += Yi * Yi; - Mxz += Xi * Zi; - Myz += Yi * Zi; - Mzz += Zi * Zi; - } - Mxx /= edge_points.size(); - Myy /= edge_points.size(); - Mxy /= edge_points.size(); - Mxz /= edge_points.size(); - Myz /= edge_points.size(); - Mzz /= edge_points.size(); - - // computing the coefficients of the characteristic polynomial - Mz = Mxx + Myy; - Cov_xy = Mxx * Myy - Mxy * Mxy; - Var_z = Mzz - Mz * Mz; - - A2 = 4 * Cov_xy - 3 * Mz * Mz - Mzz; - A1 = Var_z * Mz + 4 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz; - A0 = Mxz * (Mxz * Myy - Myz * Mxy) + Myz * (Myz * Mxx - Mxz * Mxy) - - Var_z * Cov_xy; - A22 = A2 + A2; - - // find the root of the characteristic polynomial using Newton's method - // starting at x=0 - // (it is guaranteed to converge to the right root) - for (x = 0., y = A0, iter = 0; iter < IterMAX; - iter++) // usually, 4-6 iterations are enough - { - Dy = A1 + x * (A22 + 16. * x * x); - xnew = x - y / Dy; - if ((xnew == x) || (!std::isfinite(xnew))) break; - ynew = A0 + xnew * (A1 + xnew * (A2 + 4 * xnew * xnew)); - if (abs(ynew) >= abs(y)) break; - x = xnew; - y = ynew; - } - - // compute parameters of the fitting circle - DET = x * x - x * Mz + Cov_xy; - Xcenter = (Mxz * (Myy - x) - Myz * Mxy) / DET / 2; - Ycenter = (Myz * (Mxx - x) - Mxz * Mxy) / DET / 2; - - // assemble the output - cone.center.x = Xcenter + meanX; // estimated x position - cone.center.y = Ycenter + meanY; // estimated y position - cone.radius = sqrt(Xcenter * Xcenter + Ycenter * Ycenter + Mz - x - - x); // estimated radius - - return cone; -} - -double ConeIdentification::getMeanX( -const std::vector& edge_points) { - double tot_x = 0; - for (int i = 0; i < edge_points.size(); i++) { tot_x += edge_points[i].x; } - return tot_x / edge_points.size(); -} - -double ConeIdentification::getMeanY( -const std::vector& edge_points) { - double tot_y = 0; - for (int i = 0; i < edge_points.size(); i++) { tot_y += edge_points[i].y; } - return tot_y / edge_points.size(); -} - -double ConeIdentification::getRegressionSlope( -const std::vector& edge_points) { - double meanX = getMeanX(edge_points); - double meanY = getMeanY(edge_points); - double sumNum = 0; - double sumDenom = 0; - - for (int i = 0; i < edge_points.size(); i++) { - sumNum += (edge_points[i].x - meanX) * (edge_points[i].y - meanY); - sumDenom += pow(edge_points[i].x - meanX, 2); - } - return sumNum / sumDenom; -} \ No newline at end of file diff --git a/src/sb_laserscan_processing/src/cone_extractor_node.cpp b/src/sb_laserscan_processing/src/cone_extractor_node.cpp deleted file mode 100644 index 240daddf3..000000000 --- a/src/sb_laserscan_processing/src/cone_extractor_node.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/* - * Created by William Gu on 24/03/18 - * Creates a cone extractor node (for identifying cones from a laser message) - */ - -#include - -int main(int argc, char** argv) { - // Set up ROS node - std::string node_name = "cone_extractor_node"; - - // Create an instance of the class - ConeExtractorNode node(argc, argv, node_name); - - // Start up - ros::spin(); - - // Once node stops, return 0 - return 0; -} \ No newline at end of file diff --git a/src/sb_laserscan_processing/test/LaserscanBuilder.h b/src/sb_laserscan_processing/test/LaserscanBuilder.h deleted file mode 100644 index 85e6b473c..000000000 --- a/src/sb_laserscan_processing/test/LaserscanBuilder.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Created by William Gu on 29/04/18. - * Helper class for populating laser scan messages given x and y coordinates - */ - -#ifndef PROJECT_LASERSCANBUILDER_H -#define PROJECT_LASERSCANBUILDER_H - -#include -#include -#include -#include -#include -#include -#include - -namespace LaserscanBuilder { -class LaserscanBuilder { - public: - LaserscanBuilder() { - point_tol = 0.01; - num_rays = 1000; - laser_msg.range_min = 0.1; - laser_msg.range_max = 5.0; - laser_msg.angle_min = (float) -M_PI / 2; - laser_msg.angle_max = (float) M_PI / 2; - laser_msg.angle_increment = - (laser_msg.angle_max - laser_msg.angle_min) / num_rays; - // Set all the ranges to 10 initially (out of range) - laser_msg.ranges = std::vector(num_rays, 10.0); - } - - /** - * Adds a cone to the laserscan messasge (only the edge points closest to - * the origin of the scan is considered) - * @param x coordinate of cone (center) - * @param y coordinate of cone (center) - * @param radius of cone - */ - void addCone(double x, double y, double radius) { - mapping_igvc::Point2D - coneCenter; // The point of the center of the cone to add - coneCenter.x = x; - coneCenter.y = y; - - for (int i = 0; i < num_rays; i++) { - double ang = laser_msg.angle_min + i * laser_msg.angle_increment; - - // If we can't find a close enough point, don't add any - for (double range = laser_msg.range_min; - range <= laser_msg.range_max; - range += 0.01) { - mapping_igvc::Point2D point = - ConeIdentification::laserToPoint(range, ang); - if (fabs(ConeIdentification::getDist(coneCenter, point) - - radius) <= point_tol) { - if (range < laser_msg.ranges[i]) { // Only add if not - // currently occupied by - // a closer range - laser_msg.ranges[i] = range; - // std::cout<<"("< -#include - -// Test 3 points->cone in an edge (forming a semi circle) - points are evenly -// distributed -TEST(ConeIdentification, edgeToCone3Points) { - std::vector edge_points; - mapping_igvc::Point2D p1; - p1.x = -2; - p1.y = 2; - mapping_igvc::Point2D p2; - p2.x = 0; - p2.y = 0; - mapping_igvc::Point2D p3; - p3.x = 2; - p3.y = 2; - edge_points.push_back(p1); - edge_points.push_back(p2); - edge_points.push_back(p3); - - mapping_igvc::ConeObstacle cone = - ConeIdentification::edgeToCone(edge_points); - EXPECT_FLOAT_EQ(cone.center.x, 0); - EXPECT_FLOAT_EQ(cone.center.y, 2); - EXPECT_FLOAT_EQ(cone.radius, 2); -} - -// Test 6 points->cone in an edge (forming a semi circle) - points are -// symmetrical about y axis -TEST(ConeIdentification, edgeToCone6Points) { - std::vector edge_points; - mapping_igvc::Point2D p1; - p1.x = -2; - p1.y = 2; - mapping_igvc::Point2D p2; - p2.x = -1.73; - p2.y = 1; - mapping_igvc::Point2D p3; - p3.x = -1; - p3.y = 0.268; - mapping_igvc::Point2D p4; - p4.x = 1; - p4.y = 0.268; - mapping_igvc::Point2D p5; - p5.x = 1.73; - p5.y = 1; - mapping_igvc::Point2D p6; - p6.x = 2; - p6.y = 2; - edge_points.push_back(p1); - edge_points.push_back(p2); - edge_points.push_back(p3); - edge_points.push_back(p4); - edge_points.push_back(p5); - edge_points.push_back(p6); - - mapping_igvc::ConeObstacle cone = - ConeIdentification::edgeToCone(edge_points); - EXPECT_NEAR(cone.center.x, 0, 0.01); - EXPECT_NEAR(cone.center.y, 2, 0.01); - EXPECT_NEAR(cone.radius, 2, 0.01); -} - -// Test 4 points->cone in an edge (forming a quarter circle/arc) - points are -// evenly distributed -TEST(ConeIdentification, edgeToConeArc) { - std::vector edge_points; - mapping_igvc::Point2D p1; - p1.x = 0; - p1.y = 0; - mapping_igvc::Point2D p2; - p2.x = 1; - p2.y = 0.268; - mapping_igvc::Point2D p3; - p3.x = 1.73; - p3.y = 1; - mapping_igvc::Point2D p4; - p4.x = 2; - p4.y = 2; - edge_points.push_back(p1); - edge_points.push_back(p2); - edge_points.push_back(p3); - edge_points.push_back(p4); - - mapping_igvc::ConeObstacle cone = - ConeIdentification::edgeToCone(edge_points); - EXPECT_NEAR(cone.center.x, 0, 0.01); - EXPECT_NEAR(cone.center.y, 2, 0.01); - EXPECT_NEAR(cone.radius, 2, 0.01); -} - -// Test 3 points->cone forming very short arc -TEST(ConeIdentification, shortArc) { - std::vector edge_points; - mapping_igvc::Point2D p1; - p1.x = 0; - p1.y = 0; - mapping_igvc::Point2D p2; - p2.x = 0.07; - p2.y = 0.001; - mapping_igvc::Point2D p3; - p3.x = 0.13; - p3.y = 0.004; - edge_points.push_back(p1); - edge_points.push_back(p2); - edge_points.push_back(p3); - - mapping_igvc::ConeObstacle cone = - ConeIdentification::edgeToCone(edge_points); - EXPECT_NEAR(cone.center.x, 0, 0.2); - EXPECT_NEAR(cone.center.y, 2, 0.2); - EXPECT_NEAR(cone.radius, 2, 0.2); -} - -// Test unevenly distributed points->cone in an arc -TEST(ConeIdentification, unevenArc) { - std::vector edge_points; - mapping_igvc::Point2D p1; - p1.x = -2; - p1.y = 2; - mapping_igvc::Point2D p2; - p2.x = -1.99; - p2.y = 1.8; - mapping_igvc::Point2D p3; - p3.x = -1.936; - p3.y = 1.498; - mapping_igvc::Point2D p4; - p4.x = -1.845; - p4.y = 1.228; - mapping_igvc::Point2D p5; - p5.x = 0; - p5.y = 0; - mapping_igvc::Point2D p6; - p6.x = 1.014; - p6.y = 0.276; - edge_points.push_back(p1); - edge_points.push_back(p2); - edge_points.push_back(p3); - edge_points.push_back(p4); - edge_points.push_back(p5); - edge_points.push_back(p6); - - mapping_igvc::ConeObstacle cone = - ConeIdentification::edgeToCone(edge_points); - EXPECT_NEAR(cone.center.x, 0, 0.01); - EXPECT_NEAR(cone.center.y, 2, 0.01); - EXPECT_NEAR(cone.radius, 2, 0.01); -} - -// Test inaccurate points->cone (all points off slightly) -TEST(ConeIdentification, arcWithNoise) { - std::vector edge_points; - mapping_igvc::Point2D p1; - p1.x = -2.02; - p1.y = 2.01; - mapping_igvc::Point2D p2; - p2.x = -1.74; - p2.y = 0.98; - mapping_igvc::Point2D p3; - p3.x = -0.99; - p3.y = 0.275; - mapping_igvc::Point2D p4; - p4.x = 1.01; - p4.y = 0.268; - mapping_igvc::Point2D p5; - p5.x = 1.75; - p5.y = 0.97; - mapping_igvc::Point2D p6; - p6.x = 2.04; - p6.y = 2.01; - edge_points.push_back(p1); - edge_points.push_back(p2); - edge_points.push_back(p3); - edge_points.push_back(p4); - edge_points.push_back(p5); - edge_points.push_back(p6); - - mapping_igvc::ConeObstacle cone = - ConeIdentification::edgeToCone(edge_points); - EXPECT_NEAR(cone.center.x, 0, 0.1); - EXPECT_NEAR(cone.center.y, 2, 0.1); - EXPECT_NEAR(cone.radius, 2, 0.1); -} - -// Test identifyCones with no valid cones in image (there is one valid cone but -// out of range) -TEST(ConeIdentification, coneOutRange) { - float dist_tol = 0.01; - float radius_exp = 1.0; - float radius_tol = 0.05; - int min_points_in_cone = 5; - double ang_threshold = 2.3; // For splitting - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(7, 0, 1); // x y radius - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_TRUE(cones.empty()); -} - -// Test identifyCones with no valid cones in image (there is one object with -// radius too large, of 1.5m instead of 1m) -TEST(ConeIdentification, coneTooLarge) { - float dist_tol = 0.01; - float radius_exp = 1.0; - float radius_tol = 0.05; - int min_points_in_cone = 5; - double ang_threshold = 2.3; - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(3, 0, 1.5); // x y radius - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_TRUE(cones.empty()); -} - -// Test onevalidcone partially out of range -TEST(ConeIdentification, oneValidCone) { - float dist_tol = 0.1; - float radius_exp = 1.5; - float radius_tol = 0.05; - int min_points_in_cone = 3; - double ang_threshold = 2.3; - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(0, 3, 1.5); // x y radius - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - std::cout << cones.size() << std::endl; - - EXPECT_NEAR(cones[0].radius, 1.5, 0.01); - EXPECT_NEAR(cones[0].center.x, 0, 0.05); - EXPECT_NEAR(cones[0].center.y, 3.0, 0.05); -} - -// Test identifyCones with 2 cones NOT in a cluster (they are seperate from each -// other) -TEST(ConeIdentification, twoValidCones) { - float dist_tol = 0.1; - float radius_exp = 1.0; - float radius_tol = 0.05; - int min_points_in_cone = 3; - double ang_threshold = 2.3; - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(3, 3, 1); // x y radius - builder.addCone(3, -3, 1); - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_NEAR(cones[0].radius, 1.0, 0.01); - EXPECT_NEAR(cones[0].center.x, 3.0, 0.01); - EXPECT_NEAR(cones[0].center.y, -3.0, 0.01); - - EXPECT_NEAR(cones[1].radius, 1.0, 0.01); - EXPECT_NEAR(cones[1].center.x, 3.0, 0.01); - EXPECT_NEAR(cones[1].center.y, 3.0, 0.01); -} - -// Test identifyCones with 4 cones NOT in a cluster (they are seperate from each -// other) -TEST(ConeIdentification, fourValidCones) { - float dist_tol = 0.1; - float radius_exp = 0.5; - float radius_tol = 0.1; - int min_points_in_cone = 5; - double ang_threshold = 2.3; - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - - builder.addCone(3, 1, 0.5); // x y radius - builder.addCone(3, -1, 0.5); - builder.addCone(0, 3, 0.5); - builder.addCone(0, -3, 0.5); // ISSUE WITH THIS (Appears to be bug with - // detecting cones along the negative y line) - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_NEAR(cones[0].radius, 0.5, 0.01); - EXPECT_NEAR(cones[0].center.x, 0, 0.1); - EXPECT_NEAR(cones[0].center.y, -3.0, 0.1); - - EXPECT_NEAR(cones[1].radius, 0.5, 0.01); - EXPECT_NEAR(cones[1].center.x, 3.0, 0.1); - EXPECT_NEAR(cones[1].center.y, -1.0, 0.1); - - EXPECT_NEAR(cones[2].radius, 0.5, 0.01); - EXPECT_NEAR(cones[2].center.x, 3.0, 0.1); - EXPECT_NEAR(cones[2].center.y, 1.0, 0.1); - - EXPECT_NEAR(cones[3].radius, 0.5, 0.01); - EXPECT_NEAR(cones[3].center.x, 0.0, 0.1); - EXPECT_NEAR(cones[3].center.y, 3.0, 0.1); -} - -// Test identifyCones with 2 cones in a cluster (their laserscan points overlap) -TEST(ConeIdentification, twoOverlappingCones) { - float dist_tol = 1; - float radius_exp = 1.0; - float radius_tol = 0.05; - int min_points_in_cone = 5; - double ang_threshold = 2.3; // Low as possible - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(3, 0.5, 1); // x y radius - builder.addCone(3, -0.5, 1); - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_NEAR(cones[0].radius, 1.0, 0.01); - EXPECT_NEAR(cones[0].center.x, 3.0, 0.015); - EXPECT_NEAR(cones[0].center.y, -0.5, 0.015); - - EXPECT_NEAR(cones[1].radius, 1.0, 0.01); - EXPECT_NEAR(cones[1].center.x, 3.0, 0.015); - EXPECT_NEAR(cones[1].center.y, 0.5, 0.015); -} - -// Test identifyCones with 3 cones in a cluster (their laserscan points overlap) -TEST(ConeIdentification, threeOverlappingCones) { - float dist_tol = 0.1; - float radius_exp = 1.0; - float radius_tol = 0.15; - int min_points_in_cone = 5; - double ang_threshold = 2.3; - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(3.5, 0, 1); // x y radius - builder.addCone(2, -1.3, 1); // x y radius - builder.addCone(2, 1.3, 1); - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_NEAR(cones[0].radius, 1.0, 0.01); - EXPECT_NEAR(cones[0].center.x, 2.0, 0.03); - EXPECT_NEAR(cones[0].center.y, -1.3, 0.03); - - EXPECT_NEAR(cones[1].radius, 1.0, 0.01); - EXPECT_NEAR(cones[1].center.x, 3.5, 0.05); - EXPECT_NEAR(cones[1].center.y, 0, 0.03); - - EXPECT_NEAR(cones[2].radius, 1.0, 0.01); - EXPECT_NEAR(cones[2].center.x, 2.0, 0.03); - EXPECT_NEAR(cones[2].center.y, 1.3, 0.03); -} - -// Test identifyCones with 2 cones in a cluster (their laserscan points -// "connect", horizontal relative to the robot) -TEST(ConeIdentification, twoConnectedConesHorizontal) { - float dist_tol = 0.1; - float radius_exp = 1.0; - float radius_tol = 0.05; - int min_points_in_cone = 5; - double ang_threshold = 2.3; // Low as possible - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(3, 1, 1); // x y radius - builder.addCone(3, -1, 1); - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_NEAR(cones[0].radius, 1.0, 0.01); - EXPECT_NEAR(cones[0].center.x, 3.0, 0.03); - EXPECT_NEAR(cones[0].center.y, -1, 0.03); - - EXPECT_NEAR(cones[1].radius, 1.0, 0.01); - EXPECT_NEAR(cones[1].center.x, 3.0, 0.03); - EXPECT_NEAR(cones[1].center.y, 1, 0.03); -} - -// Test identifyCones with 2 cones in a diagonal orientation (though not -// physically connected) -TEST(ConeIdentification, twoConesDiagonal) { - float dist_tol = 0.1; - float radius_exp = 1.0; - float radius_tol = 0.2; - int min_points_in_cone = 5; - double ang_threshold = 2.3; // Low as possible - - sensor_msgs::LaserScan laser_msg; - LaserscanBuilder::LaserscanBuilder builder; - builder.addCone(2, 1, 1); // x y radius - builder.addCone(3, -1, 1); - laser_msg = builder.getLaserscan(); - - std::vector cones = - ConeIdentification::identifyCones(laser_msg, - dist_tol, - radius_exp, - radius_tol, - min_points_in_cone, - ang_threshold); - - EXPECT_NEAR(cones[0].radius, 1.0, 0.01); - EXPECT_NEAR(cones[0].center.x, 3.0, 0.03); - EXPECT_NEAR(cones[0].center.y, -1, 0.03); - - EXPECT_NEAR(cones[1].radius, 1.0, 0.01); - EXPECT_NEAR(cones[1].center.x, 2.0, 0.08); // large-ish error here - EXPECT_NEAR(cones[1].center.y, 1, 0.05); -} - -int main(int argc, char** argv) { - ros::Time::init(); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} 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/sb_pointcloud_processing/CMakeLists.txt b/src/sb_pointcloud_processing/CMakeLists.txt deleted file mode 100644 index 18f2134a7..000000000 --- a/src/sb_pointcloud_processing/CMakeLists.txt +++ /dev/null @@ -1,170 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sb_pointcloud_processing) - -# 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_package(OpenMP REQUIRED) -if(OPENMP_FOUND) - message("OPENMP FOUND") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") -endif() - -## 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 - pcl_conversions - sb_utils - pcl_ros - pcl_msgs - sensor_msgs - mapping_igvc - geometry_msgs - tf2 - tf2_ros - tf2_msgs - tf2_sensor_msgs - ) -find_package(PCL 1.3 REQUIRED COMPONENTS - common - io - ) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES sb_pointcloud_processing - CATKIN_DEPENDS nodelet roscpp std_msgs pcl_ros -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ./include - ./test - ${catkin_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(line_extractor_node - include/LineExtractorNode.h - include/DBSCAN.h - include/Regression.h - src/line_extractor_node.cpp - src/LineExtractorNode.cpp - src/DBSCAN.cpp - src/Regression.cpp - include/Regression.h -) - -target_link_libraries(line_extractor_node - ${catkin_LIBRARIES} - ${PCL_COMMON_LIBRARIES} - ${PCL_IO_LIBRARIES} -) - -add_dependencies(line_extractor_node - ${mapping_igvc_EXPORTED_TARGETS} -) - -add_library(sb_pointcloud_processing - include/rgb_to_hsv.h - src/rgb_to_hsv.cpp - include/ColourspaceConverter.h - src/ColourspaceConverter.cpp -) - -target_link_libraries(sb_pointcloud_processing - ${catkin_LIBRARIES} - ${sb_utils_LIBRARIES} - ${PCL_LIBRARIES} -) - - -install( - TARGETS - sb_pointcloud_processing - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -add_executable(test_pcl_generator_node - src/test_pcl_generator_node.cpp - test/TestUtils.h -) - -target_link_libraries(test_pcl_generator_node - ${catkin_LIBRARIES} - ${PCL_COMMON_LIBRARIES} - ${PCL_IO_LIBRARIES} -) - - -add_executable(pcl_transform src/pcl_transform.cpp) - -target_link_libraries(pcl_transform - ${catkin_LIBRARIES} - ${sb_utils_LIBRARIES} -) - -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - - # Adding gtests to the package - catkin_add_gtest(DBSCAN-test - test/DBSCAN-test.cpp - test/TestUtils.h - src/DBSCAN.cpp - ) - target_link_libraries(DBSCAN-test ${catkin_LIBRARIES}) - - catkin_add_gtest(Regression-test - test/Regression-class-test.cpp - test/TestUtils.h - src/Regression.cpp - ) - target_link_libraries(Regression-test ${catkin_LIBRARIES}) - - catkin_add_gtest(colourspace-converter-test test/colourspace-converter-test.cpp include/ColourspaceConverter.h src/ColourspaceConverter.cpp) - target_link_libraries(colourspace-converter-test ${PCL_LIBRARIES}) - - - # Adding rostest to the package - find_package(rostest REQUIRED) - - add_rostest_gtest(line_extractor_rostest - test/line_extractor_test.test - test/line_extractor_rostest.cpp - test/TestUtils.h - src/DBSCAN.cpp - src/Regression.cpp - ) - target_link_libraries(line_extractor_rostest ${catkin_LIBRARIES}) - add_dependencies(line_extractor_rostest - ${mapping_igvc_EXPORTED_TARGETS} - ) - - add_rostest_gtest(pointcloud_nodelet_filters_rostest test/pointcloud_nodelet_filters.test test/pointcloud_nodelet_filters_rostest.cpp) - target_link_libraries(pointcloud_nodelet_filters_rostest ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -endif() diff --git a/src/sb_pointcloud_processing/include/ColourspaceConverter.h b/src/sb_pointcloud_processing/include/ColourspaceConverter.h deleted file mode 100644 index c72bb9a8a..000000000 --- a/src/sb_pointcloud_processing/include/ColourspaceConverter.h +++ /dev/null @@ -1,46 +0,0 @@ -/** - * Created by: Valerian Ratu - * Created on: 2018/01/13 - * Description: A class which converts RGB pointclouds to the HSV colourspace. - * Used in the rgb_to_hsv nodelet. - */ - -#ifndef SB_POINTCLOUD_PROCESSING_COLOURSPACE_CONVERTER_H -#define SB_POINTCLOUD_PROCESSING_COLOURSPACE_CONVERTER_H - -#include -#include -#include - -class ColourspaceConverter { - public: - ColourspaceConverter(); - - /** - * Converts the pointcloud given by setInputCloud to an - * HSV colourspace - * @param output memory allocated to store hte output cloud - */ - void convert(pcl::PointCloud& output); - - /** - * Sets the input cloud to be processed - * @param input - */ - void setInputCloud(pcl::PointCloud::Ptr input); - - /** - * Converts an RGB point to the HSV colourspace - * Implementation taken from PCL - * @see pcl/point_types/conversion.h - * @param in a PointXYZRGB - * @param out the corresponding point in HSV colourspace - */ - void PointXYZRGBAtoXYZHSV(const pcl::PointXYZRGB& in, - pcl::PointXYZHSV& out); - - private: - pcl::PointCloud::Ptr cloud_; -}; - -#endif // SB_POINTCLOUD_PROCESSING_COLOURSPACE_CONVERTER_H diff --git a/src/sb_pointcloud_processing/include/DBSCAN.h b/src/sb_pointcloud_processing/include/DBSCAN.h deleted file mode 100644 index 93c79ffe4..000000000 --- a/src/sb_pointcloud_processing/include/DBSCAN.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Class declaration for DBSCAN (Density-based clustering) - */ - -#ifndef LINE_EXTRACTOR_IGVC_DBSCAN_H -#define LINE_EXTRACTOR_IGVC_DBSCAN_H - -#include -#include -#include -#include - -using namespace std; -using namespace std::tr1; - -class DBSCAN { - /* - * This variable stores the PointCloud input that we want to cluster - */ - pcl::PointCloud _pcl; - - /* - * This variable stores the PointCloud clusters output - */ - vector> _clusters; - - /* - * Key: index of a point in the PointCloud - * Value: true if the point has already been clustered, false otherwise - */ - unordered_map _clustered; - - /* - * Key: index of a point in the PointCloud - * Value: true if the point has already been expanded, false otherwise - */ - unordered_map _expanded; - - /* - * Stores the neighbours of each point - * Index: index of a point in the PointCloud - * Value: a vector containing all of the point's neighbors - * (A neighbour is a point that is within @_radius of a point of interest) - */ - vector* _neighbors; - - int _min_neighbors = 5; - float _radius = 5; - - unsigned int _sequential_cut_off = 1000; - - public: - /* - * Constructor: - * Takes in minimum number of neighbours and radius as parameters - */ - DBSCAN(int min_neighbours = 5, float radius = 5); - - /* - * Main entry function: - * Given a PointCloud, clusters the PointCloud into a vector of smaller - * PointClouds - */ - vector> - findClusters(pcl::PointCloud::Ptr pcl_ptr); - - void setMinNeighbours(int new_min_neighour); - void setRadius(float new_radius); - - private: - static double dist(pcl::PointXYZ p1, pcl::PointXYZ p2); - bool isPointVisited(unsigned int p_index); - bool isPointExpanded(unsigned int p_index); - - /* - * Finds all the neighbours of each point in the PointCloud - * (A neighbour is a point that is within @_radius of a point of interest) - */ - void findNeighbors(); - - /* - * Expands a cluster around a given point recursively by: - * 1. Adding all of the point's neighbors to the same cluster as the point - * (unless they already belong to a cluster) - * 2. Expand recursively around each neighbor that is a core point - * (unless the neighbor has already been expanded) - */ - void expand(unsigned int center_point_index, - pcl::PointCloud& cluster); - - /* - * Given the index of a point in the PointCloud, determines whether the - * point is a core point - * A core point is a point that has at least @_min_neighbors within - * @_radius. - */ - bool isCore(unsigned int center_point_index); -}; - -#endif // PROJECT_DBSCAN_H diff --git a/src/sb_pointcloud_processing/include/LineExtractorNode.h b/src/sb_pointcloud_processing/include/LineExtractorNode.h deleted file mode 100644 index 2cec8a330..000000000 --- a/src/sb_pointcloud_processing/include/LineExtractorNode.h +++ /dev/null @@ -1,171 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Header file for Line Extractor Node - */ - -#ifndef LINE_EXTRACTOR_IGVC_NODE_H -#define LINE_EXTRACTOR_IGVC_NODE_H - -#include "DBSCAN.h" -#include "Regression.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class LineExtractorNode { - public: - /* - * @clusters stores the output from DBSCAN - */ - std::vector> clusters; - - LineExtractorNode(int argc, char** argv, std::string node_name); - - // main entry function - void extractLines(); - - /* - * @clusters: input clusters - * @cluster_points: vector to store converted cluster points - * @colors: vector to store color for each cluster point - * This function takes in @clusters, assigns a different color - * for each cluster, and converts each point to geometry_msgs::Point. - * It pushes the points and colors to the back of @cluster_points - * and @colors. - */ - static void convertClustersToPointsWithColors( - std::vector> clusters, - std::vector& cluster_points, - std::vector& colors); - - private: - ros::Subscriber subscriber; - ros::Publisher publisher; - ros::Publisher rviz_line_publisher; - ros::Publisher rviz_cluster_publisher; - - /* - * @regression takes in the output from @dbscan and outputs a LineObstacle - * for each cluster. - * A line has the same index as its corresponding cluster - * (line of clusters[i] is lines[i]) - */ - Regression regression; - - /* - * @degreePoly is a hyperparameter to regression that determines - * the degree of polynomial of the line of best fit - */ - int degreePoly; - - /* - * @lmabda is a hyperparameter to regression that determines - * the strength of regularization - */ - float lambda; - - /* - * @minNeighbours is a hyperparameter to DBSCAN that determines - * how many neighbours are required for a point to be considered - * a 'core' point - */ - int minNeighbours; - - /* - * @radius is a hyperparameter to DBSCAN that determines - * how close the points have to be together in order for them to - * be considered as 'core' points - */ - float radius; - - /* - * @x_delta is the parameter for visualizing LineObstacle. It - * determines the x interval between adjacent points in RViz. - */ - float x_delta; - - /* - * scale of the marker points - */ - float scale; - - /* - * frame_id for rviz markers - */ - std::string frame_id; - - /* - * @pclPtr stores the pointer to the PCL PointCloud after it has - * been converted from sensor_msgs PointCloud2 - */ - pcl::PointCloud::Ptr pclPtr; - - /* - * The callback function is called whenever the node receives a - * PointCloud message. It converts sensor_msgs PointCloud2 pointer - * to PCL PointCloud pointer and then extracts lines from the PointCloud - */ - void pclCallBack(const sensor_msgs::PointCloud2ConstPtr processed_pcl); - - /* - * @line_obstacles a list of LineObstacle messages - * This function takes in @line_obstacles and publishes a message - * to rviz for visualization at "~/debug/output_line_obstacle". - */ - void visualizeLineObstacles( - std::vector line_obstacles); - - /* - * This function makes a Marker for all points in @clusters - * with a different color for each cluster and publishes a message - * to rviz for visualization at "~/debug/clusters". - */ - void visualizeClusters(); - - /* - * @line_obstacles: list of line obstacle messages - * This function converts each line obstacle into a list of - * geometry_msgs:Point and then merges all of them into a single - * vector. - */ - std::vector convertLineObstaclesToPoints( - std::vector line_obstacles); - - /* - * Convert a list of vectors to a list of LineObstacle message - */ - std::vector - vectorsToMsgs(std::vector vectors); - - /* - * Convert a vector to LineObstacle message - */ - mapping_igvc::LineObstacle vectorToLineObstacle(Eigen::VectorXf vector, - unsigned int cluster_index); - - /* - * Get the minimum and maximum value of x value of all points in a cluster - * @cluster_index: the index of cluster of interest in @clusters - */ - void - getClusterXRange(double& xmin, double& xmax, unsigned int cluster_index); - - /* - * Checks whether or not all the params we are getting from NodeHandler are - * valid - * params being checked: degree_polynomial, lambda, min_neighbours, radius - */ - bool areParamsInvalid(); -}; - -#endif // PROJECT_LINEEXTRACTOR_H diff --git a/src/sb_pointcloud_processing/include/Regression.h b/src/sb_pointcloud_processing/include/Regression.h deleted file mode 100644 index 93067428d..000000000 --- a/src/sb_pointcloud_processing/include/Regression.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Class declaration for Regression, which calculates - * line of best fit for each cluster of points - */ - -#ifndef LINE_EXTRACTOR_IGVC_REGRESSION_H -#define LINE_EXTRACTOR_IGVC_REGRESSION_H - -#include -#include -#include - -// using namespace Eigen; -// using namespace std; -// using namespace pcl; - -class Regression { - public: - /* - * Returns a std::vector of Eigen::VectorXf - * Each Eigen::VectorXf corresponds to the line of best fit of a - * PointCloud cluster - * The corresponding vector and cluster have the same index within each of - * their vectors. - * @poly_degree: Degree of polynomial of the line of best fit - * @lambda: Regularization parameter (Default: 0) - */ - static std::vector - getLinesOfBestFit(std::vector> clusters, - unsigned int poly_degree, - float lambda = 0); - - private: - /* - * Returns a line of best fit given a cluster - */ - static Eigen::VectorXf - getLineOfCluster(pcl::PointCloud cluster, - unsigned int poly_degree, - float lambda = 0); - - /* - * Constructs a row in the matrix X given a data point and degree of - * polynomial - */ - static Eigen::VectorXf constructRow(float x, unsigned int poly_degree); -}; - -#endif // PROJECT_REGRESSION_H diff --git a/src/sb_pointcloud_processing/include/rgb_to_hsv.h b/src/sb_pointcloud_processing/include/rgb_to_hsv.h deleted file mode 100644 index f587a6d10..000000000 --- a/src/sb_pointcloud_processing/include/rgb_to_hsv.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * created by: valerian ratu - * Created on: 2018/01/13 - * Description: A ros nodelet which takes pointcloud input in RGB colourspace - * and returns it in the HSV colourspace. - */ - -#ifndef SB_POINTCLOUD_PROCESSING_RGB_TO_HSV_H -#define SB_POINTCLOUD_PROCESSING_RGB_TO_HSV_H - -// ROS Includes -#include -#include -#include - -// PCL Includes -#include -#include -#include - -#include - -namespace sb_pointcloud_processing { - -class RGBtoHSV : public nodelet::Nodelet { - public: - /** - * Empty constructor - */ - RGBtoHSV(); - - private: - /** - * Initializes the nodelet - */ - virtual void onInit(); - - /** - * Callback which converts a given pointcloud from RGB to HSV - * - * @param input the RGB pointcloud to be converted - */ - void callback(const sensor_msgs::PointCloud2::ConstPtr& input); - - // A converter from RGB to HSV - ColourspaceConverter converter; - - // Publishes the HSV pointcloud - ros::Publisher pub; - - // Subscribes to the RGB pointcloud - ros::Subscriber sub; -}; -} - -#endif // SB_POINTCLOUD_PROCESSING_RGB_TO_HSV_H diff --git a/src/sb_pointcloud_processing/launch/filter_lines.launch b/src/sb_pointcloud_processing/launch/filter_lines.launch deleted file mode 100644 index 6cf106aaf..000000000 --- a/src/sb_pointcloud_processing/launch/filter_lines.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/sb_pointcloud_processing/launch/filter_lines_test.launch b/src/sb_pointcloud_processing/launch/filter_lines_test.launch deleted file mode 100644 index 9d935690f..000000000 --- a/src/sb_pointcloud_processing/launch/filter_lines_test.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/sb_pointcloud_processing/launch/filter_nodelets.launch b/src/sb_pointcloud_processing/launch/filter_nodelets.launch deleted file mode 100644 index 6a8b7bfe7..000000000 --- a/src/sb_pointcloud_processing/launch/filter_nodelets.launch +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - filter_field_name: h - filter_limit_min: 40 - filter_limit_max: 170 - filter_limit_negative: False - - - - - - - filter_field_name: s - filter_limit_min: 0.1 - filter_limit_max: 0.6 - filter_limit_negative: False - - - - - - - filter_field_name: v - filter_limit_min: 0.1 - filter_limit_max: 1.0 - filter_limit_negative: False - - - - - - - filter_field_name: z - filter_limit_min: 0 - filter_limit_max: 0.2 - filter_limit_negative: False - input_frame: zed_left_camera - output_frame: zed_left_camera - - - - diff --git a/src/sb_pointcloud_processing/launch/igvc_visualizer.launch b/src/sb_pointcloud_processing/launch/igvc_visualizer.launch deleted file mode 100644 index 27ea747f2..000000000 --- a/src/sb_pointcloud_processing/launch/igvc_visualizer.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - 30 - 0.3 - 0.3 - 0.1 - - - diff --git a/src/sb_pointcloud_processing/launch/line_extractor.launch b/src/sb_pointcloud_processing/launch/line_extractor.launch deleted file mode 100644 index c6eee3ef2..000000000 --- a/src/sb_pointcloud_processing/launch/line_extractor.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/src/sb_pointcloud_processing/launch/line_extractor_rviz_degree_3.launch b/src/sb_pointcloud_processing/launch/line_extractor_rviz_degree_3.launch deleted file mode 100644 index 86ab45f90..000000000 --- a/src/sb_pointcloud_processing/launch/line_extractor_rviz_degree_3.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - [3.5, 0, -1.3, 0.3] - - [0, 0, -1.3, 0.3] - - - - - - - - - - - - [5] - - - - - diff --git a/src/sb_pointcloud_processing/launch/nodelet_manager.launch b/src/sb_pointcloud_processing/launch/nodelet_manager.launch deleted file mode 100644 index 64db6029a..000000000 --- a/src/sb_pointcloud_processing/launch/nodelet_manager.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - diff --git a/src/sb_pointcloud_processing/launch/pointcloud_filters.launch b/src/sb_pointcloud_processing/launch/pointcloud_filters.launch deleted file mode 100644 index ac1780e73..000000000 --- a/src/sb_pointcloud_processing/launch/pointcloud_filters.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - diff --git a/src/sb_pointcloud_processing/nodelet_plugins.xml b/src/sb_pointcloud_processing/nodelet_plugins.xml deleted file mode 100644 index dc5061da5..000000000 --- a/src/sb_pointcloud_processing/nodelet_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - Transforms a pcl pointcloud from the RGB colourspace to the HSV colourspace - - - - \ No newline at end of file diff --git a/src/sb_pointcloud_processing/package.xml b/src/sb_pointcloud_processing/package.xml deleted file mode 100644 index 5dd864a8e..000000000 --- a/src/sb_pointcloud_processing/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - sb_pointcloud_processing - 0.0.0 - - The point cloud processing package - - - min - - - TODO - - - - - - - - - - - - catkin - - roscpp - std_msgs - sb_utils - pcl_conversions - roscpp - sb_utils - mapping_igvc - libpcl-all-dev - nodelet - std_msgs - sensor_msgs - pcl_ros - rgbd_launch - roscpp - std_msgs - sb_utils - pcl_conversions - roscpp - sb_utils - mapping_igvc - libpcl-all - nodelet - std_msgs - sensor_msgs - pcl_ros - rgbd_launch - - - - - - diff --git a/src/sb_pointcloud_processing/scripts/nodelet_launch.sh b/src/sb_pointcloud_processing/scripts/nodelet_launch.sh deleted file mode 100755 index 5327fe467..000000000 --- a/src/sb_pointcloud_processing/scripts/nodelet_launch.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash -# This script launches the nodelet manager then waits for a second -# before launching the nodelets for pointclouds. -# -# This is done because sometimes the nodelets are instantiated before -# the manager and would cause registering issues. - -roslaunch sb_pointcloud_processing nodelet_manager.launch & -sleep 1 -roslaunch sb_pointcloud_processing filter_nodelets.launch & diff --git a/src/sb_pointcloud_processing/src/ColourspaceConverter.cpp b/src/sb_pointcloud_processing/src/ColourspaceConverter.cpp deleted file mode 100644 index 1916c478e..000000000 --- a/src/sb_pointcloud_processing/src/ColourspaceConverter.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/** - * Created by: Valerian Ratu - * Created on: 2018/01/13 - * Description: A class which converts RGB pointclouds to the HSV colourspace. - */ - -#include - -using namespace pcl; - -ColourspaceConverter::ColourspaceConverter() {} - -void ColourspaceConverter::setInputCloud(PointCloud::Ptr input) { - cloud_ = input; -} - -void ColourspaceConverter::convert(PointCloud& output) { - output.width = cloud_->width; - output.height = cloud_->height; - output.header = cloud_->header; - for (size_t i = 0; i < cloud_->size(); i++) { - PointXYZHSV p; - ColourspaceConverter::PointXYZRGBAtoXYZHSV(cloud_->points[i], p); - output.points.push_back(p); - } -} - -void ColourspaceConverter::PointXYZRGBAtoXYZHSV(const PointXYZRGB& in, - PointXYZHSV& out) { - out.x = in.x; - out.y = in.y; - out.z = in.z; - - const unsigned char max = std::max(in.r, std::max(in.g, in.b)); - const unsigned char min = std::min(in.r, std::min(in.g, in.b)); - - out.v = static_cast(max) / 255.f; - - if (max == 0) // rgb values of zero maps to hsv 0, 0, 0 - { - out.s = 0.f; - out.h = 0.f; - return; - } - - const float diff = static_cast(max - min); - out.s = diff / static_cast(max); - - if (min == max) // diff == 0 -> division by zero, set h to zero - { - out.h = 0; - return; - } - - if (max == in.r) - out.h = 60.f * (static_cast(in.g - in.b) / diff); - else if (max == in.g) - out.h = 60.f * (2.f + static_cast(in.b - in.r) / diff); - else - out.h = - 60.f * (4.f + static_cast(in.r - in.g) / diff); // max == b - - if (out.h < 0.f) out.h += 360.f; -} diff --git a/src/sb_pointcloud_processing/src/DBSCAN.cpp b/src/sb_pointcloud_processing/src/DBSCAN.cpp deleted file mode 100644 index 819ec259f..000000000 --- a/src/sb_pointcloud_processing/src/DBSCAN.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Class Implementation of DBSCAN (Density-based clustering) - */ - -#include - -DBSCAN::DBSCAN(int min_neighbours, float radius) { - this->_min_neighbors = min_neighbours; - this->_radius = radius; - this->_clusters = vector>(); -} - -void DBSCAN::setMinNeighbours(int new_min_neighour) { - this->_min_neighbors = new_min_neighour; -} - -void DBSCAN::setRadius(float new_radius) { - this->_radius = new_radius; -} - -vector> -DBSCAN::findClusters(pcl::PointCloud::Ptr pclPtr) { - this->_pcl = *pclPtr; - - findNeighbors(); - - for (unsigned int i = 0; i < this->_pcl.size(); i++) { - if (isPointVisited(i)) { continue; } - if (isCore(i)) { - // create a new cluster, and assign current point to it - pcl::PointCloud cluster = - pcl::PointCloud(); - pcl::PointXYZ current_point = this->_pcl[i]; - cluster.push_back(current_point); - this->_clustered.insert({i, true}); - - // expand the cluster, centering on current point - expand(i, cluster); - - // add this cluster to the list of clusters - this->_clusters.push_back(cluster); - } - } - - return this->_clusters; -} - -bool DBSCAN::isCore(unsigned int center_index) { - return this->_neighbors[center_index].size() >= this->_min_neighbors; -} - -void DBSCAN::expand(unsigned int center_index, - pcl::PointCloud& cluster) { - this->_expanded.insert({center_index, true}); - - vector neighbors = this->_neighbors[center_index]; - - // iterate through all the neighbors of the point - for (unsigned int i = 0; i < neighbors.size(); i++) { - unsigned int current_index = neighbors[i]; - pcl::PointXYZ current_point = this->_pcl[current_index]; - - // add neighbor point to the cluster if it hasn't already been clustered - if (!isPointVisited(current_index)) { - cluster.push_back(current_point); - this->_clustered.insert({current_index, true}); - } - - // expand on the neighbour point if it is a core and it hasn't been - // expanded yet - if (!isPointExpanded(current_index) && isCore(current_index)) { - expand(current_index, cluster); - this->_expanded.insert({current_index, true}); - } - } - - return; -} - -void DBSCAN::findNeighbors() { - this->_neighbors = new vector[this->_pcl.size()]; - -// Run the following loops in parallel if the size of the point cloud is -// bigger than -// this->_sequential_cut_off. If there aren't enough points in the point -// cloud, then -// the overhead in creating multiple threads can overpower time gained from -// multi threading. -#pragma omp parallel for if (this->_pcl.size() > this->_sequential_cut_off) - for (unsigned int i = 0; i < this->_pcl.size(); i++) { - vector neighbors; - pcl::PointXYZ current_point = this->_pcl[i]; - - // for current_point, determine all neighbour points that are within - // predetermined radius - for (unsigned int j = 0; j < this->_pcl.size(); j++) { - if (i == j) continue; - pcl::PointXYZ neighbor_point = this->_pcl[j]; - if (dist(current_point, neighbor_point) <= this->_radius) { - neighbors.push_back(j); - } - } - - // store the list of neighbours - this->_neighbors[i] = neighbors; - } -} - -double DBSCAN::dist(pcl::PointXYZ p1, pcl::PointXYZ p2) { - double dx = abs(p1.x - p2.x); - double dy = abs(p1.y - p2.y); - return sqrt(pow(dx, 2) + pow(dy, 2)); -} - -bool DBSCAN::isPointVisited(unsigned int p_index) { - return this->_clustered.find(p_index) != this->_clustered.end(); -} - -bool DBSCAN::isPointExpanded(unsigned int p_index) { - return this->_expanded.find(p_index) != this->_expanded.end(); -} diff --git a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp deleted file mode 100644 index 3765c636b..000000000 --- a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp +++ /dev/null @@ -1,270 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Node implementation for Line Extractor Node - */ - -#include - -LineExtractorNode::LineExtractorNode(int argc, - char** argv, - std::string node_name) { - ros::init(argc, argv, node_name); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - std::string degree_polynomial_param = "degree_polynomial"; - int default_degree_polynomial = 3; - SB_getParam(private_nh, - degree_polynomial_param, - this->degreePoly, - default_degree_polynomial); - - std::string lambda_param = "lambda"; - float default_lambda = 0; - SB_getParam(private_nh, lambda_param, this->lambda, default_lambda); - - std::string min_neighbours_param = "min_neighbours"; - int default_min_neighbours = 3; - SB_getParam(private_nh, - min_neighbours_param, - this->minNeighbours, - default_min_neighbours); - - std::string radius_param = "radius"; - float default_radius = 0.1; - SB_getParam(private_nh, radius_param, this->radius, default_radius); - - std::string delta_x_param = "x_delta"; - float default_delta_x = 0.01; - SB_getParam(private_nh, delta_x_param, this->x_delta, default_delta_x); - - std::string scale_param = "scale"; - float default_scale = 0.1; - SB_getParam(private_nh, scale_param, this->scale, default_scale); - - std::string frame_id_param = "frame_id"; - std::string default_frame_id = "line_extractor_test"; - SB_getParam(private_nh, frame_id_param, this->frame_id, default_frame_id); - - if (areParamsInvalid()) { - ROS_DEBUG( - "Detected invalid params - make sure all params are positive"); - ros::shutdown(); - } - - std::string topic_to_subscribe_to = "input_pointcloud"; // dummy topic name - int refresh_rate = 10; - subscriber = nh.subscribe( - topic_to_subscribe_to, refresh_rate, &LineExtractorNode::pclCallBack, this); - - std::string topic_to_publish_to = - "output_line_obstacle"; // dummy topic name - uint32_t queue_size = 1; - publisher = private_nh.advertise( - topic_to_publish_to, queue_size); - - std::string rviz_line_topic = "debug/output_line_obstacle"; - rviz_line_publisher = private_nh.advertise( - rviz_line_topic, queue_size); - - std::string rviz_cluster_topic = "debug/clusters"; - rviz_cluster_publisher = private_nh.advertise( - rviz_cluster_topic, queue_size); -} - -void LineExtractorNode::pclCallBack( -const sensor_msgs::PointCloud2ConstPtr processed_pcl) { - pcl::PCLPointCloud2 pcl_pc2; - - // convert sensor_msgs::PointCloud2 to pcl::PCLPointCloud2 - pcl_conversions::toPCL(*processed_pcl, pcl_pc2); - - pcl::PointCloud::Ptr temp_cloud( - new pcl::PointCloud); - - // convert pcl::PointCloud2 to pcl::PointCloud - pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); - - // store converted pointcloud for use - this->pclPtr = temp_cloud; - - // extract lines from the pointcloud - extractLines(); - - return; -} - -void LineExtractorNode::extractLines() { - DBSCAN dbscan(this->minNeighbours, this->radius); - this->clusters = dbscan.findClusters(this->pclPtr); - - std::vector lines = regression.getLinesOfBestFit( - this->clusters, this->degreePoly, this->lambda); - - std::vector line_obstacles = - vectorsToMsgs(lines); - - for (unsigned int i = 0; i < line_obstacles.size(); i++) { - publisher.publish(line_obstacles[i]); - } - - visualizeClusters(); - visualizeLineObstacles(line_obstacles); - - return; -} - -void LineExtractorNode::visualizeClusters() { - std::vector cluster_points; - std::vector colors; - convertClustersToPointsWithColors(this->clusters, cluster_points, colors); - - visualization_msgs::Marker::_scale_type scale = - snowbots::RvizUtils::createMarkerScale( - this->scale, this->scale, this->scale); - - std::string ns = "debug"; - - visualization_msgs::Marker marker = snowbots::RvizUtils::createMarker( - cluster_points, colors, scale, this->frame_id, ns, 0); - - rviz_cluster_publisher.publish(marker); -} - -void LineExtractorNode::convertClustersToPointsWithColors( -std::vector> clusters, -std::vector& cluster_points, -std::vector& colors) { - std::vector color_library_r = {1.0, 0.0, 0.0}; - std::vector color_library_g = {0.0, 0.0, 1.0}; - std::vector color_library_b = {0.0, 1.0, 0.0}; - - for (unsigned int c = 0; c < clusters.size(); c++) { - pcl::PointCloud cluster = clusters[c]; - - // assign color to this cluster - std_msgs::ColorRGBA color; - - color.r = color_library_r[c % color_library_r.size()]; - color.g = color_library_g[c % color_library_g.size()]; - color.b = color_library_b[c % color_library_b.size()]; - color.a = 1.0; - - // push all the points in this cluster along with its color - for (unsigned int p = 0; p < cluster.size(); p++) { - pcl::PointXYZ pcl_point = cluster[p]; - - geometry_msgs::Point msg_point; - msg_point.x = pcl_point.x; - msg_point.y = pcl_point.y; - - cluster_points.push_back(msg_point); - colors.push_back(color); - } - } -} - -void LineExtractorNode::visualizeLineObstacles( -std::vector line_obstacles) { - std::vector lines_points = - convertLineObstaclesToPoints(line_obstacles); - - visualization_msgs::Marker::_color_type color = - snowbots::RvizUtils::createMarkerColor(0.0, 1.0, 1.0, 1.0); - visualization_msgs::Marker::_scale_type scale = - snowbots::RvizUtils::createMarkerScale( - this->scale, this->scale, this->scale); - - std::string ns = "debug"; - - visualization_msgs::Marker marker = - snowbots::RvizUtils::createMarker(lines_points, - color, - scale, - this->frame_id, - ns, - visualization_msgs::Marker::POINTS); - - rviz_line_publisher.publish(marker); -} - -std::vector -LineExtractorNode::convertLineObstaclesToPoints( -std::vector line_obstacles) { - std::vector line_points; - // iterate through all lines - for (unsigned int i = 0; i < line_obstacles.size(); i++) { - mapping_igvc::LineObstacle line_obstacle = line_obstacles[i]; - - // draw the line as a series of points - for (float x = line_obstacle.x_min; x < line_obstacle.x_max; - x += this->x_delta) { - geometry_msgs::Point p; - p.x = x; - - // calculate y with a polynomial function - for (unsigned int i = 0; i < line_obstacle.coefficients.size(); - i++) { - p.y += line_obstacle.coefficients[i] * pow(x, i); - } - - line_points.push_back(p); - } - } - - return line_points; -} - -bool LineExtractorNode::areParamsInvalid() { - return this->degreePoly < 0 || this->lambda < 0 || - this->minNeighbours < 0 || this->radius < 0; -} - -std::vector -LineExtractorNode::vectorsToMsgs(std::vector vectors) { - std::vector msgs; - - for (unsigned int i = 0; i < vectors.size(); i++) { - msgs.push_back(vectorToLineObstacle(vectors[i], i)); - } - - return msgs; -} - -mapping_igvc::LineObstacle -LineExtractorNode::vectorToLineObstacle(Eigen::VectorXf v, - unsigned int cluster_index) { - mapping_igvc::LineObstacle line_obstacle = mapping_igvc::LineObstacle(); - - for (unsigned int i = 0; i < v.size(); i++) { - line_obstacle.coefficients.push_back(v(i)); - } - - getClusterXRange(line_obstacle.x_min, line_obstacle.x_max, cluster_index); - - return line_obstacle; -} - -void LineExtractorNode::getClusterXRange(double& xmin, - double& xmax, - unsigned int cluster_index) { - pcl::PointCloud cluster = this->clusters[cluster_index]; - - double min, max; - - if (cluster.size()) { - min = max = cluster[0].x; - } else { - xmin = xmax = -1; - return; - } - - for (unsigned int i = 0; i < cluster.size(); i++) { - if (cluster[i].x < min) { min = cluster[i].x; } - if (cluster[i].x > max) { max = cluster[i].x; } - } - - xmin = min; - xmax = max; -} diff --git a/src/sb_pointcloud_processing/src/Regression.cpp b/src/sb_pointcloud_processing/src/Regression.cpp deleted file mode 100644 index 5cc8247f2..000000000 --- a/src/sb_pointcloud_processing/src/Regression.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: February 3, 2018 - * Description: Calculates line of best fit for each cluster of points - */ - -#include - -std::vector Regression::getLinesOfBestFit( -std::vector> clusters, -unsigned int poly_degree, -float lambda) { - std::vector lines; - - // Calculate line of best fit for each cluster - for (unsigned int i = 0; i < clusters.size(); i++) { - lines.push_back(getLineOfCluster(clusters[i], poly_degree, lambda)); - } - - return lines; -} - -Eigen::VectorXf -Regression::getLineOfCluster(pcl::PointCloud cluster, - unsigned int poly_degree, - float lambda) { - unsigned int n = cluster.size(); - - /* - * Linear Equation to solve: - * X' * X + lambda * I = X' * y - * - * X is a matrix of size (n,d) where n is the number of points in the - * cluster, - * and d is the polynomial degree + 1. (+1 for linear bias) - * Each row of X represents the x coordinate of a point. - * Given x for the x coordinate of a point and column for the column index - * in the matrix X: - * The first column of X is 1 for linear bias, and the rest of the columns - * contain the value of pow(x, column-1). - * - * lambda is the regularization parameter. The higher this number, - * The higher the penalty on the size of the coefficients of the - * mathematical lines. - * - * y is a column vector of size (n), where n is the number of points in the - * cluster. - * Each row of the vector corresponds to the y coordinate of a point. - */ - - Eigen::MatrixXf X(n, poly_degree + 1); - Eigen::VectorXf y(n); - - for (unsigned int i = 0; i < cluster.size(); i++) { - pcl::PointXYZ point = cluster[i]; - - X.row(i) = constructRow(point.x, poly_degree); - - y(i) = point.y; - } - - Eigen::MatrixXf regularization(poly_degree + 1, poly_degree + 1); - regularization.setIdentity(); - regularization *= lambda; - - Eigen::MatrixXf left(X.transpose() * X + regularization); - Eigen::MatrixXf right(X.transpose() * y); - - Eigen::VectorXf line(n); - line = (left).ldlt().solve(right); - - return line; -} - -Eigen::VectorXf Regression::constructRow(float x, unsigned int poly_degree) { - Eigen::VectorXf row(poly_degree + 1); - - // linear bias - row(0) = 1; - - // non-linear - for (unsigned int j = 1; j < poly_degree + 1; j++) { row(j) = pow(x, j); } - - return row; -} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/src/line_extractor_node.cpp b/src/sb_pointcloud_processing/src/line_extractor_node.cpp deleted file mode 100644 index 3a3863ec7..000000000 --- a/src/sb_pointcloud_processing/src/line_extractor_node.cpp +++ /dev/null @@ -1,21 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Spawns Line Extractor Node - */ - -#include - -int main(int argc, char** argv) { - // Set up ROS node - std::string node_name = "line_extractor_node"; - - // Create an instance of the class - LineExtractorNode node(argc, argv, node_name); - - // Start up - ros::spin(); - - // Once node stops, return 0 - return 0; -} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/src/pcl_transform.cpp b/src/sb_pointcloud_processing/src/pcl_transform.cpp deleted file mode 100644 index 65ccb7642..000000000 --- a/src/sb_pointcloud_processing/src/pcl_transform.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Created By: Valerian Ratu - * Created On: May 1 2017 - * Description: A node which transforms a given pointcloud from - * it's current frame to a given frame - */ - -#include -#include -#include -#include -#include - -#include - -ros::Publisher pub; -tf2_ros::Buffer tf_buffer; - -std::string output_frame; -ros::Time last_exception_time; - -// The amount of time(s) for a new pointcloud transform to be published. -double transform_period; - -void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) { - try { - // Create an empty pointcloud - sensor_msgs::PointCloud2 output; - // Transform the pointcloud to the requested frame - geometry_msgs::TransformStamped tf_stamped = tf_buffer.lookupTransform( - msg->header.frame_id, output_frame, ros::Time(0), ros::Duration(0.1)); - - tf2::doTransform(*msg, output, tf_stamped); - - output.header.frame_id = output_frame; - - // Publish the transformed pointcloud - pub.publish(output); - - ros::Duration(transform_period).sleep(); - } catch (tf2::TransformException ex) { - if (ros::Time::now().toSec() - last_exception_time.toSec() > 1.0) - ROS_WARN("%s", ex.what()); - - last_exception_time = ros::Time::now(); - } -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "pointcloud_transformer"); - ros::NodeHandle private_nh("~"); - if (!SB_getParam(private_nh, "output_frame", output_frame)) { - // Error and exit if we didn't get a frame to transform to. - // We need this to transform anything, and there is no reasonable - // default - ROS_ERROR( - "Param 'output_frame' not provided. " - "Can't transform anything without a frame to transform it to"); - return 1; - } - - double default_transform_period = 0; - SB_getParam( - private_nh, "transform_period", transform_period, default_transform_period); - - ros::Subscriber sub = - private_nh.subscribe("/input_pointcloud", 1, pointCloudCallback); - pub = - private_nh.advertise("/output_pointcloud", 1); - tf2_ros::TransformListener tfListener(tf_buffer); - ros::spin(); - return 0; -} diff --git a/src/sb_pointcloud_processing/src/rgb_to_hsv.cpp b/src/sb_pointcloud_processing/src/rgb_to_hsv.cpp deleted file mode 100644 index cb3692d43..000000000 --- a/src/sb_pointcloud_processing/src/rgb_to_hsv.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/** - * created by: valerian ratu - * Created on: 2018/01/13 - * Description: A ros nodelet which takes pointcloud input in RGB colourspace - * and returns it in the HSV colourspace. - */ - -#include -#include - -using namespace sb_pointcloud_processing; -using namespace pcl_ros; -using namespace pcl; - -RGBtoHSV::RGBtoHSV() {} - -void RGBtoHSV::onInit() { - NODELET_DEBUG("Initializing Nodelet..."); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - sub = private_nh.subscribe("input", 1, &RGBtoHSV::callback, this); - pub = private_nh.advertise>("output", 1); - NODELET_DEBUG("Nodelet Initialized"); -} - -void RGBtoHSV::callback(const sensor_msgs::PointCloud2::ConstPtr& input) { - // Obtain the ROS pointcloud and convert into PCL Pointcloud2 - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - - // Converts from the Pointcloud2 format to PointcloudRGB - pcl::PointCloud::Ptr pcl_rgb( - new pcl::PointCloud()); - pcl::fromPCLPointCloud2(*pcl_input, *pcl_rgb); - - // Converts RGB pointcloud to HSV - converter.setInputCloud(pcl_rgb); - pcl::PointCloud::Ptr pcl_output( - new pcl::PointCloud()); - converter.convert(*pcl_output); - - // Publishes the new cloud - pub.publish(*pcl_output); -} - -// Allows this node to be exported and registered as a nodelet -PLUGINLIB_EXPORT_CLASS(RGBtoHSV, nodelet::Nodelet) diff --git a/src/sb_pointcloud_processing/src/test_pcl_generator_node.cpp b/src/sb_pointcloud_processing/src/test_pcl_generator_node.cpp deleted file mode 100644 index 10075e3be..000000000 --- a/src/sb_pointcloud_processing/src/test_pcl_generator_node.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Created by: Min Gyo Kim - * Created On: March 11th 2018 - * Description: Generates a point cloud with two lines and an optional outlier - * line and publishes it to "/input/pointcloud" - */ - -#include -#include -#include -#include -#include -#include - -std::vector first_line; -std::vector second_line; - -float x_min; -float x_max; -float x_delta; -float max_noise_x; -float max_noise_y; - -float outlier_x_delta; -std::vector outlier_line; - -sensor_msgs::PointCloud2 generatePclMessage(bool include_outlier); -unsigned int getSeed(); - -unsigned int counter = 0; - -int main(int argc, char** argv) { - ros::init(argc, argv, "test_pcl_generator_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - ros::Rate loop_rate = 0.75; - ros::Publisher publisher = - nh.advertise("input_pointcloud", 1); - - std::string first_line_param = "first_line"; - std::vector default_first_line = {50, 0, -0.01}; - private_nh.param(first_line_param, first_line, default_first_line); - - std::string second_line_param = "second_line"; - std::vector default_second_line = {0, 0, -0.01}; - private_nh.param(second_line_param, second_line, default_second_line); - - std::string x_min_param = "x_min"; - float default_x_min = -5; - SB_getParam(private_nh, x_min_param, x_min, default_x_min); - - std::string x_max_param = "x_max"; - float default_x_max = 5; - SB_getParam(private_nh, x_max_param, x_max, default_x_max); - - std::string x_delta_param = "x_delta"; - float default_x_delta = 0.01; - SB_getParam(private_nh, x_delta_param, x_delta, default_x_delta); - - std::string max_noise_x_param = "max_noise_x"; - float default_max_noise_x = 5; - SB_getParam( - private_nh, max_noise_x_param, max_noise_x, default_max_noise_x); - - std::string max_noise_y_param = "max_noise_y"; - float default_max_noise_y = 5; - SB_getParam( - private_nh, max_noise_y_param, max_noise_y, default_max_noise_y); - - std::string outlier_param = "outlier"; - bool default_outlier = false; - bool outlier; - SB_getParam(private_nh, outlier_param, outlier, default_outlier); - - std::string frame_id_param = "frame_id"; - std::string default_frame_id = "line_extractor_test"; - std::string frame_id; - SB_getParam(private_nh, frame_id_param, frame_id, default_frame_id); - - if (outlier) { - std::string outlier_line_param = "outlier_line"; - std::vector default_outlier_line = {0}; - private_nh.param( - outlier_line_param, outlier_line, default_outlier_line); - - std::string outlier_x_delta_param = "outlier_x_delta"; - float default_outlier_x_delta = 1; - private_nh.param( - outlier_x_delta_param, outlier_x_delta, default_outlier_x_delta); - } - - while (ros::ok()) { - sensor_msgs::PointCloud2 msg_to_publish = generatePclMessage(outlier); - msg_to_publish.header.frame_id = frame_id; - - publisher.publish(msg_to_publish); - - ros::spinOnce(); - loop_rate.sleep(); - } -} - -sensor_msgs::PointCloud2 generatePclMessage(bool include_outlier) { - // coefficients is the same as the one in LineObstacle message - LineExtractor::TestUtils::LineArgs args(first_line, x_min, x_max, x_delta); - - // Generate a single PointCloud with noise - pcl::PointCloud pcl; - LineExtractor::TestUtils::addLineToPointCloud( - args, pcl, max_noise_x, max_noise_y, getSeed()); - - // Add second line to the pointcloud - args.coefficients = second_line; - LineExtractor::TestUtils::addLineToPointCloud( - args, pcl, max_noise_x, max_noise_y, getSeed()); - - // Add outlier if wanted - if (include_outlier) { - args.coefficients = outlier_line; - args.x_delta = outlier_x_delta; - - LineExtractor::TestUtils::addLineToPointCloud( - args, pcl, max_noise_x, max_noise_y, getSeed()); - } - - // convert pointcloud to sensor msgs - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg(pcl, msg); - - return msg; -} - -/** - * function that returns a seed; - * the seed returned increments every time this function is called, - * and once the seed reaches 9, the next seed resets to 0; - * @return - */ -unsigned int getSeed() { - return counter++ % 10; -} diff --git a/src/sb_pointcloud_processing/test/DBSCAN-test.cpp b/src/sb_pointcloud_processing/test/DBSCAN-test.cpp deleted file mode 100644 index 8343ec1d3..000000000 --- a/src/sb_pointcloud_processing/test/DBSCAN-test.cpp +++ /dev/null @@ -1,345 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 27th 2018 - * Description: GTest for DBSCAN implementation - */ - -#include "./TestUtils.h" -#include -#include - -TEST(DBSCAN, ClusterTwoNearPoints) { - float min_neighbours = 1; - float radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - // Test two near points - pcl::PointXYZ p1; - p1.x = 1; - p1.y = 1; - pcl.push_back(p1); - - pcl::PointXYZ p2; - p2.x = 1.1; - p2.y = 1.1; - pcl.push_back(p2); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(1, clusters.size()); - ASSERT_EQ(2, clusters[0].size()); -} - -TEST(DBSCAN, TestClusterTwoFarPoints) { - int min_neighbours = 1; - int radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - // Test two far points - pcl::PointXYZ p1; - p1.x = 1; - p1.y = 1; - pcl.push_back(p1); - - pcl::PointXYZ p3; - p3.x = 10; - p3.y = 10; - pcl.push_back(p3); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - EXPECT_EQ(0, clusters.size()); -} - -TEST(DBSCAN, TestExpandCluster) { - int min_neighbours = 2; - int radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - pcl::PointXYZ p1; - p1.x = 1; - p1.y = 1; - pcl.push_back(p1); - - pcl::PointXYZ p2; - p2.x = 2; - p2.y = 1; - pcl.push_back(p2); - - pcl::PointXYZ p3; - p3.x = 3; - p3.y = 1; - pcl.push_back(p3); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(1, clusters.size()); - EXPECT_EQ(3, clusters[0].size()); -} - -TEST(DBSCAN, TestClusterTwoShortHorizontalLines) { - int min_neighbours = 2; - int radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = 0; - float x_max = 3; - float x_delta = 1; - vector coefficients = {10}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-10}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(4, clusters[0].size()); - EXPECT_EQ(4, clusters[1].size()); -} - -TEST(DBSCAN, TestClusterTwoSlopedLines) { - int min_neighbours = 2; - int radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = 0; - float x_max = 99; - float x_delta = 1; - vector coefficients = {100, 1}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-100, 1}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[1].size()); -} - -TEST(DBSCAN, TestClusterTwoSlopedLinesWithOutliers) { - int min_neighbours = 2; - int radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = 0; - float x_max = 99; - float x_delta = 1; - vector coefficients = {100, 1}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-100, 1}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // outliers - pcl.push_back(pcl::PointXYZ(999999, 999999, 0)); - pcl.push_back(pcl::PointXYZ(-999999, -999999, 0)); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[1].size()); -} - -TEST(DBSCAN, TestClusterTwoLongHorizontalLines) { - int min_neighbours = 1; - int radius = 5; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = -10; - float x_max = 9989; - float x_delta = 1; - vector coefficients = {3}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-3}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - std::vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[1].size()); -} - -TEST(DBSCAN, TestClusterBorder) { - int min_neighbours = 1; - int radius = 6; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = -10; - float x_max = -8; - float x_delta = 1; - vector coefficients = {3.000001}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-3}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[1].size()); -} - -TEST(DBSCAN, TestClusterTwoPolynomialLines) { - int min_neighbours = 1; - int radius = 3; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = -20; - float x_max = 19; - float x_delta = 1; - vector coefficients = {10, 0, 0, 0.002}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-10, 0, 0, 0.002}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[1].size()); -} - -TEST(DBSCAN, TestClusterOnePolynomialLinesWithNoise) { - int min_neighbours = 1; - int radius = 80; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = 0; - float x_max = 99; - float x_delta = 1; - vector coefficients = {1000, 7, -0.7, 0.007}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - float true_min, true_max; - LineExtractor::TestUtils::getMinAndMaxOfPointCloud(true_min, true_max, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - - ASSERT_EQ(clusters.size(), 1); - - float actual_min, actual_max; - LineExtractor::TestUtils::getMinAndMaxOfPointCloud( - actual_min, actual_max, clusters[0]); - - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_FLOAT_EQ(true_min, actual_min); - EXPECT_FLOAT_EQ(true_max, actual_max); -} - -TEST(DBSCAN, TestClusterTwoPolynomialLinesWithNoise) { - int min_neighbours = 1; - int radius = 80; - DBSCAN dbscan(min_neighbours, radius); - - pcl::PointCloud pcl; - - float x_min = 0; - float x_max = 99; - float x_delta = 1; - vector coefficients = {1000, 7, -0.7, 0.007}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Add first line to PointCloud - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Add second line to PointCloud - args.coefficients = {-1000, 7, -0.7, 0.007}; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - pcl::PointCloud::Ptr pcl_ptr = pcl.makeShared(); - - vector> clusters = - dbscan.findClusters(pcl_ptr); - ASSERT_EQ(2, clusters.size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[0].size()); - EXPECT_EQ(LineExtractor::TestUtils::getNumPoints(args), clusters[1].size()); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/Regression-class-test.cpp b/src/sb_pointcloud_processing/test/Regression-class-test.cpp deleted file mode 100644 index a3c538530..000000000 --- a/src/sb_pointcloud_processing/test/Regression-class-test.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: February 3, 2018 - * Description: Tests calculation of line of best fit - */ - -#include "./TestUtils.h" -#include -#include - -TEST(Regression, OnePerfectLinearFit) { - // Setup PointCloud parameters - unsigned int poly_degree = 1; - - float x_min = 0; - float x_max = 99; - float x_delta = 1; - std::vector coefficients = {100, 1}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Generate a single PointCloud - pcl::PointCloud pcl; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Perform Regression - std::vector> clusters; - clusters.push_back(pcl); - - std::vector lines = - Regression::getLinesOfBestFit(clusters, poly_degree); - - // Check results - ASSERT_EQ(lines.size(), 1); - ASSERT_EQ(lines[0].size(), coefficients.size()); - - for (unsigned int i = 0; i < coefficients.size(); i++) { - EXPECT_FLOAT_EQ(lines[0](i), coefficients[i]); - } -} - -TEST(Regression, MultiplePerfectLinearFits) { - // Setup PointCloud parameters - unsigned int poly_degree = 1; - - float x_min = 0; - float x_max = 999; - float x_delta = 0.1; - std::vector> coefficients_per_line = { - {5, -2}, {10, 7}, {-99, -10}}; - - unsigned int num_lines = coefficients_per_line.size(); - - // Generate multiple PointClouds - std::vector> clusters; - - for (unsigned int i = 0; i < num_lines; i++) { - LineExtractor::TestUtils::LineArgs args( - coefficients_per_line[i], x_min, x_max, x_delta); - pcl::PointCloud pcl; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - clusters.push_back(pcl); - } - - // Preform Regression - std::vector lines = - Regression::getLinesOfBestFit(clusters, poly_degree, 10); - - // Check results - for (unsigned int i = 0; i < num_lines; i++) { - Eigen::VectorXf line = lines[i]; - ASSERT_EQ(line.size(), coefficients_per_line[i].size()); - - for (unsigned int j = 0; j < line.size(); j++) { - EXPECT_NEAR(line(j), coefficients_per_line[i][j], 1); - } - } -} - -TEST(Regression, OnePerfectNonLinearFit) { - // Setup PointCloud paramters - unsigned int poly_degree = 3; - - float x_min = 0; - float x_max = 99; - float x_delta = 1; - std::vector coefficients = {100, 7, -0.7, 0.007}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - // Generate PointCloud - pcl::PointCloud pcl; - LineExtractor::TestUtils::addLineToPointCloud(args, pcl); - - // Perform Regression - std::vector> clusters; - clusters.push_back(pcl); - - std::vector lines = - Regression::getLinesOfBestFit(clusters, poly_degree); - - // Check results - ASSERT_EQ(lines.size(), 1); - Eigen::VectorXf line = lines[0]; - - ASSERT_EQ(line.size(), coefficients.size()); - - for (unsigned int i = 0; i < line.size(); i++) { - EXPECT_NEAR(line(i), coefficients[i], 1); - } -} - -TEST(Regression, OneNonLinearFitWithNoise) { - // Setup PointCloud parameters - unsigned int poly_degree = 3; - - float x_min = 0; - float x_max = 99; - float x_delta = 0.01; - std::vector coefficients = {1000, 7, -0.7, 0.007}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - float max_noise_x = 1; - float max_noise_y = 1; - - // Generate a single PointCloud with noise - pcl::PointCloud pcl; - LineExtractor::TestUtils::addLineToPointCloud( - args, pcl, max_noise_x, max_noise_y); - - // Perform Regression - std::vector> clusters; - clusters.push_back(pcl); - - std::vector lines = - Regression::getLinesOfBestFit(clusters, poly_degree); - - // Check results - ASSERT_EQ(lines.size(), 1); - Eigen::VectorXf line = lines[0]; - - ASSERT_EQ(line.size(), coefficients.size()); - - for (unsigned int i = 0; i < line.size(); i++) { - float tol; - - // logic to allow more tolerance for y-intercept - if (i) { - tol = 5; - } else { - tol = 10; - } - - EXPECT_NEAR(line(i), coefficients[i], tol); - } -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/TestUtils.h b/src/sb_pointcloud_processing/test/TestUtils.h deleted file mode 100644 index 172f459a5..000000000 --- a/src/sb_pointcloud_processing/test/TestUtils.h +++ /dev/null @@ -1,137 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: February 17, 2018 - * Description: Helper functions for testing Regression, DBSCAN, and rostest - */ - -#ifndef LINE_EXTRACTOR_IGVC_TESTUTILS_H -#define LINE_EXTRACTOR_IGVC_TESTUTILS_H - -#include -#include -#include - -namespace LineExtractor { -class TestUtils { - public: - /** - * LineArgs is a struct representing a mathematical line. - * @param coefficients coefficients of the mathematical line, where the - * index - * of a coefficient corresponds to its degree - * @param x_min, x_max domain of the line - * @param x_delta the distance between adjacent points on the line - */ - struct LineArgs { - std::vector coefficients; - float x_min; - float x_max; - float x_delta; - LineArgs(std::vector coefficients, - float x_min, - float x_max, - float x_delta) - : coefficients(coefficients), - x_min(x_min), - x_max(x_max), - x_delta(x_delta){}; - }; - - /** - * Adds given line to the given point cloud - * @param args struct representing a mathematical line within a domain - * @param pcl point cloud to add the line to - */ - static void addLineToPointCloud(LineArgs args, - pcl::PointCloud& pcl) { - for (float x = args.x_min; x <= args.x_max; x += args.x_delta) { - pcl::PointXYZ p; - p.x = x; - p.y = 0; - - for (unsigned int i = 0; i < args.coefficients.size(); i++) { - p.y += args.coefficients[i] * pow(x, i); - } - - pcl.push_back(p); - } - }; - - /** - * Adds given line with added noise to the given point cloud - * @param args struct representing a mathematical line within a domain - * @param pcl point cloud to add the line to - * @param max_noise_x maximum deviation of a point from the line in x - * direction - * @param max_noise_y maximum deviation of a point from the line in y - * direction - * @param seed seed for rand() - */ - static void addLineToPointCloud(LineArgs args, - pcl::PointCloud& pcl, - float max_noise_x, - float max_noise_y, - unsigned int seed = 123) { - srand(seed); - - for (float x = args.x_min; x <= args.x_max; x += args.x_delta) { - float true_x = x; - float true_y = 0; - - for (unsigned int i = 0; i < args.coefficients.size(); i++) { - true_y += args.coefficients[i] * pow(x, i); - } - - float noise_y = - ((float) rand() / (RAND_MAX)) * max_noise_y * 2 - max_noise_y; - float noise_x = - ((float) rand() / (RAND_MAX)) * max_noise_x * 2 - max_noise_x; - - float deformed_x = true_x + noise_x; - float deformed_y = true_y + noise_y; - - pcl::PointXYZ p(deformed_x, deformed_y, 0); - pcl.push_back(p); - } - }; - - /** - * Gets the number of points that form the line based on its domain and - * distance between adjacent points - * @param args representation of the line in a struct - * @return the number of points that form the given line - */ - unsigned int static getNumPoints(LineArgs args) { - return (args.x_max - args.x_min) / args.x_delta + 1; - } - - /** - * Gets the minimum and maximum values of x of the point cloud - * @param min_x variable to store the minimum value - * @param max_x variable to store the maximum value - * @param pcl point cloud - */ - static void getMinAndMaxOfPointCloud(float& min_x, - float& max_x, - pcl::PointCloud pcl) { - double min, max; - - if (pcl.size()) { - min = max = pcl[0].x; - } else { - min_x = max_x = -1; - return; - } - - for (unsigned int i = 0; i < pcl.size(); i++) { - if (pcl[i].x < min) { min = pcl[i].x; } - if (pcl[i].x > max) { max = pcl[i].x; } - } - - min_x = min; - max_x = max; - } -}; -} - -#endif // LINE_EXTRACTOR_TESTUTILS_H diff --git a/src/sb_pointcloud_processing/test/colourspace-converter-test.cpp b/src/sb_pointcloud_processing/test/colourspace-converter-test.cpp deleted file mode 100644 index 4fcf44f44..000000000 --- a/src/sb_pointcloud_processing/test/colourspace-converter-test.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "ColourspaceConverter.h" -#include - -using namespace pcl; - -TEST(ColourspaceConverter, convertPointclouds) { - ColourspaceConverter c = ColourspaceConverter(); - - pcl::PointCloud::Ptr input( - new pcl::PointCloud()); - input->height = 1; - input->width = 4; - - input->points.resize(input->height * input->width); - - PointXYZRGB black; - black.r = black.g = black.b = 0; - black.x = black.y = black.z = 0; - input->points[0] = black; - - PointXYZRGB white; - white.r = white.g = white.b = 255; - white.x = white.y = white.z = 1; - input->points[1] = white; - - PointXYZRGB green; - green.r = 45; - green.g = 174; - green.b = 45; - green.x = green.y = green.z = 2; - input->points[2] = green; - - PointXYZRGB orange; - orange.r = 243; - orange.g = 136; - orange.b = 35; - orange.x = orange.y = orange.z = 3; - input->points[3] = orange; - - c.setInputCloud(input); - - pcl::PointCloud::Ptr output( - new pcl::PointCloud()); - c.convert(*output); - - int comparisons = 0; - for (auto& point : output->points) { - if (point.z == 0) { - // White - ASSERT_EQ(0, point.h); - ASSERT_EQ(0, point.s); - ASSERT_EQ(0, point.v); - comparisons++; - } else if (point.z == 1) { - // Black - ASSERT_EQ(0, point.h); - ASSERT_EQ(0, point.s); - ASSERT_EQ(1, point.v); - comparisons++; - } else if (point.z == 2) { - // Green - ASSERT_NEAR(120, point.h, 1); - ASSERT_NEAR(0.741, point.s, 0.001); - ASSERT_NEAR(0.682, point.v, 0.001); - comparisons++; - } else if (point.z == 3) { - // Orange - ASSERT_NEAR(29, point.h, 1); - ASSERT_NEAR(0.856, point.s, 0.001); - ASSERT_NEAR(0.953, point.v, 0.001); - comparisons++; - } - } - ASSERT_EQ(4, comparisons); -} - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/line_extractor_rostest.cpp b/src/sb_pointcloud_processing/test/line_extractor_rostest.cpp deleted file mode 100644 index e60230e71..000000000 --- a/src/sb_pointcloud_processing/test/line_extractor_rostest.cpp +++ /dev/null @@ -1,114 +0,0 @@ -/* - * Created By: Min Gyo Kim - * Created On: January 20, 2018 - * Description: Ros tests for Line Extractor Node - */ - -#include "./TestUtils.h" -#include -#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 LineExtractorRosTest : public testing::Test { - protected: - virtual void SetUp() { - test_publisher = - nh_.advertise("/input_pointcloud", 1); - test_subscriber = - nh_.subscribe("/line_extractor_node/output_line_obstacle", - 1, - &LineExtractorRosTest::callback, - this); - - // Let the publishers and subscribers set itself up timely - ros::Rate loop_rate(1); - loop_rate.sleep(); - } - - ros::NodeHandle nh_; - mapping_igvc::LineObstacle lineObstacle; - ros::Publisher test_publisher; - ros::Subscriber test_subscriber; - - public: - void callback(const mapping_igvc::LineObstacle& line) { - lineObstacle = line; - } -}; - -TEST_F(LineExtractorRosTest, TestTwoNonLinearLinesWithNoise) { - float x_min = 0; - float x_max = 99; - float x_delta = 1; - - // coefficients is the same as the one in LineObstacle message - std::vector coefficients = {1000, 7, -0.7, 0.007}; - LineExtractor::TestUtils::LineArgs args( - coefficients, x_min, x_max, x_delta); - - float max_noise_x = 1; - float max_noise_y = 1; - - // Generate a single PointCloud with noise - pcl::PointCloud pcl; - LineExtractor::TestUtils::addLineToPointCloud( - args, pcl, max_noise_x, max_noise_y); - - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg(pcl, msg); - - test_publisher.publish(msg); - ros::Rate loop_rate(1); - - // Wait for the message to get passed around - 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(); - - ASSERT_EQ(lineObstacle.coefficients.size(), coefficients.size()); - - for (unsigned int i = 0; i < lineObstacle.coefficients.size(); i++) { - float tol; - - // logic to allow more tolerance for y-intercept - if (i) { - tol = 5; - } else { - tol = 10; - } - - EXPECT_NEAR(lineObstacle.coefficients[i], coefficients[i], tol); - } - - EXPECT_NEAR(lineObstacle.x_min, x_min, 1); - EXPECT_NEAR(lineObstacle.x_max, x_max, 1); - - float true_min, true_max; - LineExtractor::TestUtils::getMinAndMaxOfPointCloud(true_min, true_max, pcl); - - EXPECT_FLOAT_EQ(lineObstacle.x_min, true_min); - EXPECT_FLOAT_EQ(lineObstacle.x_max, true_max); -} - -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, "line_extractor_rostest"); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/sb_pointcloud_processing/test/line_extractor_test.test b/src/sb_pointcloud_processing/test/line_extractor_test.test deleted file mode 100644 index 93156426b..000000000 --- a/src/sb_pointcloud_processing/test/line_extractor_test.test +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/sb_pointcloud_processing/test/pointcloud_nodelet_filters.test b/src/sb_pointcloud_processing/test/pointcloud_nodelet_filters.test deleted file mode 100644 index 093779418..000000000 --- a/src/sb_pointcloud_processing/test/pointcloud_nodelet_filters.test +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/pointcloud_nodelet_filters_rostest.cpp b/src/sb_pointcloud_processing/test/pointcloud_nodelet_filters_rostest.cpp deleted file mode 100644 index 0b256d484..000000000 --- a/src/sb_pointcloud_processing/test/pointcloud_nodelet_filters_rostest.cpp +++ /dev/null @@ -1,199 +0,0 @@ -#include "rgb_to_hsv.h" -#include -#include - -class PointCloudProcessingTest : public testing::Test { - protected: - virtual void SetUp() { - test_publisher = - nh_.advertise>("test_sub", 1); - test_subscriber = nh_.subscribe( - "/z/output", 1, &PointCloudProcessingTest::callback, this); - - // Let the publishers and subscribers set itself up timely - ros::Rate loop_rate(1); - loop_rate.sleep(); - } - - ros::NodeHandle nh_; - pcl::PointCloud message_output; - ros::Publisher test_publisher; - ros::Subscriber test_subscriber; - - public: - /** - * Helper function to compare floats. Defines the equality to be - * based on FLT_EPSILON: the value of the difference between one and - * the smallest value smallest from one in float representation. - * @param a - * @param b - * @return True if a and b very close in float representation, - * false otherwise. - */ - bool almostEquals(float a, float b) { return fabs(a - b) < FLT_EPSILON; } - - void callback(const pcl::PointCloud::Ptr msg) { - message_output = *msg; - } -}; - -/* - * Tests whether pointclouds in one frame are transformed - * to the output frame properly. - * @see the static transform publisher in the rostest - * - * The initial cloud should be: - * - * z - * ^ - * | * (1,3) - * | - * | - * | (0,0) - * * -----------> y - * - * And after the transform (point kept in the same place in figure): - * 2 units in the y direction followed by a 45 deg positive rotation on the - * x-axis - * - * ^ y - * | - * * (3,1) - * | - * | - * z (0,2) | - * <----*--------- - * - * - */ -TEST_F(PointCloudProcessingTest, transformsCloud) { - ros::Duration(5).sleep(); - - pcl::PointCloud::Ptr input( - new pcl::PointCloud()); - input->height = 1; - input->width = 2; - input->points.resize(input->height * input->width); - - pcl::PointXYZRGB p1; - p1.x = 0; - p1.y = 0; - p1.z = 0; - p1.r = 45; - p1.g = 174; - p1.b = 45; - - input->points[0] = p1; - - pcl::PointXYZRGB p2; - p2.x = 0; - p2.y = 1; - p2.z = 3; - p2.r = 45; - p2.g = 174; - p2.b = 45; - - input->points[1] = p2; - - input->header.frame_id = "input"; - - test_publisher.publish(input); - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - EXPECT_EQ(2, message_output.points.size()); - - // Expect points to be converted from (y, z) - // - x = 0 for all points - // Not quite exact due to floating point maths - // (0,0) -> (0,2) - // (1,3) -> (3,1) - int num_comparisons = 0; - for (auto& point : message_output.points) { - // std::cerr << "Point: y = " << point.y << " z = " << point.z << - // std::endl; - if (almostEquals(point.y, 0.f)) { - ASSERT_NEAR(2, point.z, 0.001); - num_comparisons++; - } else if (almostEquals(point.y, 3.f)) { - ASSERT_NEAR(1, point.z, 0.001); - num_comparisons++; - } - } - EXPECT_EQ(2, num_comparisons); -} - -/* - * Filters for points of interests which are points within - * a certain boundary with a certain colour (GREEN) - */ -TEST_F(PointCloudProcessingTest, filtersPointsOfInterest) { - ros::Duration(5).sleep(); - - pcl::PointCloud::Ptr input( - new pcl::PointCloud()); - input->height = 1; - input->width = 9; - input->points.resize(input->height * input->width); - int point_ind = 0; - for (int i = 1; i <= 5; i++) { - pcl::PointXYZRGB p; - p.x = 0; - p.y = 1; - p.z = i; - if (i > 1 && i < 5) { - // These are the points of interest - p.r = 45; - p.g = 174; - p.b = 45; - } - input->points[point_ind++] = p; - } - for (int i = 2; i <= 5; i++) { - pcl::PointXYZRGB p; - p.x = 0; - p.y = i; - p.z = 1; - if (i > 1 && i < 5) { - // Should be filtered out by the z filter - p.r = 45; - p.g = 174; - p.b = 45; - } - input->points[point_ind++] = p; - } - - input->header.frame_id = "input"; - - test_publisher.publish(input); - ros::Rate loop_rate(1); - loop_rate.sleep(); - ros::spinOnce(); - - EXPECT_EQ(3, message_output.points.size()); - int num_comparisons = 0; - for (auto& point : message_output.points) { - // std::cerr << "Point: y = " << point.y << " z = " << point.z << - // std::endl; - if (almostEquals(point.y, 2.f)) { - ASSERT_NEAR(1, point.z, 0.001); - num_comparisons++; - } else if (almostEquals(point.y, 3.f)) { - ASSERT_NEAR(1, point.z, 0.001); - num_comparisons++; - } else if (almostEquals(point.y, 4.f)) { - ASSERT_NEAR(1, point.z, 0.001); - num_comparisons++; - } - } - EXPECT_EQ(3, num_comparisons); -} - -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, "pointcloud_filters_rostest"); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/test_scripts/filter_nodelets.test b/src/sb_pointcloud_processing/test/test_scripts/filter_nodelets.test deleted file mode 100644 index 74d7c3181..000000000 --- a/src/sb_pointcloud_processing/test/test_scripts/filter_nodelets.test +++ /dev/null @@ -1,66 +0,0 @@ - - - - - - - - - - - - filter_field_name: h - filter_limit_min: 115 - filter_limit_max: 125 - filter_limit_negative: False - - - - - - - filter_field_name: s - filter_limit_min: 0.735 - filter_limit_max: 0.745 - filter_limit_negative: False - - - - - - - filter_field_name: v - filter_limit_min: 0.675 - filter_limit_max: 0.685 - filter_limit_negative: False - - - - - - - filter_field_name: z - filter_limit_min: 1 - filter_limit_max: 2 - filter_limit_negative: False - input_frame: output - output_frame: output - - - - diff --git a/src/sb_pointcloud_processing/test/test_scripts/nm.test b/src/sb_pointcloud_processing/test/test_scripts/nm.test deleted file mode 100644 index b1a6faed1..000000000 --- a/src/sb_pointcloud_processing/test/test_scripts/nm.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - diff --git a/src/sb_pointcloud_processing/test/test_scripts/nodelet_launch_rostest.sh b/src/sb_pointcloud_processing/test/test_scripts/nodelet_launch_rostest.sh deleted file mode 100755 index 45c719f04..000000000 --- a/src/sb_pointcloud_processing/test/test_scripts/nodelet_launch_rostest.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -# Similar to nodelet_launch.sh, this script launches the testing -# nodelet manager and, after a second, launches the nodelets -# themselves due to early instantiation registration issues. - -roslaunch sb_pointcloud_processing nm.test& -sleep 1 -roslaunch sb_pointcloud_processing filter_nodelets.test& diff --git a/src/sb_utils/CMakeLists.txt b/src/sb_utils/CMakeLists.txt index 8b57ceaf2..ffc4535ef 100644 --- a/src/sb_utils/CMakeLists.txt +++ b/src/sb_utils/CMakeLists.txt @@ -15,6 +15,8 @@ add_definitions(-std=c++14) find_package(catkin REQUIRED COMPONENTS roscpp mapping_msgs_urc + sb_geom + sb_geom_msgs ) @@ -49,9 +51,9 @@ include_directories( ## Declare a C++ library add_library(sb_utils include/sb_utils.h - include/RvizUtils.h + include/RvizUtils.h src/sb_utils.cpp - src/RvizUtils.cpp + src/RvizUtils.cpp ) ## Specify libraries to link a library or executable target against @@ -74,7 +76,7 @@ if (CATKIN_ENABLE_TESTING) # Adding rostest to the package find_package(rostest REQUIRED) - add_rostest_gtest(rviz_utils_rostest + add_rostest_gtest(rviz_utils_rostest test/rviz_utils_rostest.test test/rviz_utils_rostest.cpp src/RvizUtils.cpp diff --git a/src/sb_vision/src/HSVFilter.cpp b/src/sb_vision/src/HSVFilter.cpp index 8ace68cb9..e944d777b 100644 --- a/src/sb_vision/src/HSVFilter.cpp +++ b/src/sb_vision/src/HSVFilter.cpp @@ -35,7 +35,7 @@ int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV) { // Functions void HSVFilter::manualCalibration(void) { - cv::namedWindow(manualCalibrationWindow, CV_WINDOW_AUTOSIZE); + cv::namedWindow(manualCalibrationWindow, cv::WINDOW_AUTOSIZE); cv::createTrackbar( "LowH", manualCalibrationWindow, &_iLowH, 179); // Hue (0 - 179) cv::createTrackbar("HighH", manualCalibrationWindow, &_iHighH, 179); @@ -54,7 +54,7 @@ void HSVFilter::stopManualCalibration() { } void HSVFilter::filterImage(const cv::Mat& input, cv::Mat& output) { - cv::cvtColor(input, hsvOutput, CV_BGR2HSV, 0); + cv::cvtColor(input, hsvOutput, cv::COLOR_BGR2HSV, 0); cv::inRange(hsvOutput, cv::Scalar(_iLowH, _iLowS, _iLowV), cv::Scalar(_iHighH, _iHighS, _iHighV), @@ -98,4 +98,4 @@ std::string HSVFilter::getValues(void) { values << _iLowH << " " << _iHighH << " " << _iLowS << " " << _iHighS << " " << _iLowV << " " << _iHighV << "\n"; return values.str(); -} \ No newline at end of file +} diff --git a/src/sb_vision/src/camera_init.cpp b/src/sb_vision/src/camera_init.cpp index cfb458359..e6ca02bc5 100644 --- a/src/sb_vision/src/camera_init.cpp +++ b/src/sb_vision/src/camera_init.cpp @@ -14,7 +14,7 @@ using namespace std; int main(int argc, char** argv) { string inputWindow = "Camera"; - namedWindow(inputWindow, CV_WINDOW_AUTOSIZE); + namedWindow(inputWindow, WINDOW_AUTOSIZE); VideoCapture cap(0); // captures the first camera if (!cap.isOpened()) { 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/snowbots_ui/README.md b/src/snowbots_ui/README.md index 2b43af409..3caa19915 100644 --- a/src/snowbots_ui/README.md +++ b/src/snowbots_ui/README.md @@ -4,13 +4,6 @@ This is a UI package built for the UBC Snowbots Team ![alt text](https://github.com/adamsnguyen/Snowflake/blob/master/src/snowbots_ui/resources/demo.png) -## Installation -### Install qt build for ROS Melodic. -```bash -sudo apt-get install ros-melodic-qt-build -``` - - # Run GUI ### First run a rosmaster ```bash 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