From ab4121e4ec3b2d2c20beacb3772c2f5a3f74dec3 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 17:10:20 -0400 Subject: [PATCH 01/17] Adding rostest GitHub action and removing install.sh --- .github/workflows/rostest-dev.yml | 26 +++ develop.sh | 13 +- docs/CONTRIBUTING.rst | 12 +- docs/README.rst | 92 +++++----- install.sh | 267 ------------------------------ 5 files changed, 93 insertions(+), 317 deletions(-) create mode 100644 .github/workflows/rostest-dev.yml delete mode 100755 install.sh diff --git a/.github/workflows/rostest-dev.yml b/.github/workflows/rostest-dev.yml new file mode 100644 index 00000000..54a9f0c0 --- /dev/null +++ b/.github/workflows/rostest-dev.yml @@ -0,0 +1,26 @@ +name: rostest-dev + +on: + push: + branches: + - github-actions + +jobs: + + rostest: + + runs-on: ubuntu-latest + container: ros:melodic + + steps: + + - uses: actions/checkout@v1 + + - name: Prep Workspace and Build Packages + run: | + sh develop.sh setup + sh develop.sh link + sh develop.sh build + + - name: Run Tests + run: sh develop.sh test diff --git a/develop.sh b/develop.sh index 3c72bd6a..2115ac77 100755 --- a/develop.sh +++ b/develop.sh @@ -4,7 +4,7 @@ USER_SHELLS="bash zsh" PACKAGES_DIR="packages" -WORKSPACE_DIR="$HOME/.workspace" +WORKSPACE_DIR="$HOME/ez-rassor_ws" WORKSPACE_SOURCE_DIR="$WORKSPACE_DIR/src" WORKSPACE_DEVEL_DIR="$WORKSPACE_DIR/devel" PACKAGE_XML_FILE="package.xml" @@ -45,12 +45,17 @@ source_setups_in_directory() { # Set up the development environment. setup_environment() { + + # Setup will run fresh every time rm -r -f "$WORKSPACE_DIR" mkdir -p "$WORKSPACE_SOURCE_DIR" + cd "$WORKSPACE_SOURCE_DIR" catkin_init_workspace cd - > /dev/null 2>&1 + source_setups_in_directory "$WORKSPACE_DEVEL_DIR" + mkdir -p ~/.gazebo/models cp -r extra_worlds/* ~/.gazebo/models/ cp -r extra_models/* ~/.gazebo/models/ } @@ -176,7 +181,13 @@ kill_ros() { # Test all packages in the workspace. test_packages() { cd "$WORKSPACE_DIR" + + # This command will run the tests but return a status of 0 catkin_make run_tests + + # This command will return a status code of 0 or 1 depending on if the previous tests succeeded + catkin_test_results + cd - > /dev/null 2>&1 } diff --git a/docs/CONTRIBUTING.rst b/docs/CONTRIBUTING.rst index 70ce6183..aafa0489 100644 --- a/docs/CONTRIBUTING.rst +++ b/docs/CONTRIBUTING.rst @@ -4,21 +4,11 @@ This project is free and open-source under the `MIT license`_. Anyone can fork t DEVELOPMENT INSTRUCTIONS ---- -Before you begin developing, you must install ROS and the ROS build tools. If you are developing on Ubuntu Xenial or Ubuntu Bionic, you can install these software packages easily with the ``install.sh`` script. Execute these commands to get started: +Before you begin developing, you must first install ROS and the ROS build tools. Clone the repository with the following command: :: git clone https://github.com/FlaSpaceInst/EZ-RASSOR.git cd EZ-RASSOR - sh install.sh ros - sh install.sh tools - ** RESTART TERMINAL ** - -If you encounter ``Sub-process /usr/bin/dpkg returned an error code...``, try to fix the broken install with the following command, then rerun the original command: -:: - sudo apt --fix-broken install - ** RERUN ORIGINAL COMMAND ** -If you need more installation information or are using a different operating system, please see the `README`_. - The next thing you should do is install the dependencies for all of the packages in this repository, which can be done using the ``develop.sh`` script like so: :: sh develop.sh setup diff --git a/docs/README.rst b/docs/README.rst index 6bbc28f0..e20ff3fe 100644 --- a/docs/README.rst +++ b/docs/README.rst @@ -13,23 +13,36 @@ For more information, our `wiki`_ contains a high-level overview of the EZ-RASSO **POTENTIAL CONTRIBUTORS:** check out the `contributing guidelines`_ and the `license`_. +INSTALLATION PREREQUISITES +-------------------------- +- `ROS Melodic`_ +- `Python 2.7`_ +- `Pip`_ +- `rosdep`_ +- `build-essential`_ + TYPICAL INSTALLATION -------------------- First, clone this repository with ``git``. -:: - git clone https://github.com/FlaSpaceInst/EZ-RASSOR.git - cd EZ-RASSOR -Then, let the ``install.sh`` script do the heavy lifting! A typical installation on Ubuntu Xenial or Ubuntu Bionic is achieved with these commands: -:: - sh install.sh ros - ** RESTART TERMINAL ** - sh install.sh tools - sh install.sh packages +.. code-block:: bash + + git clone https://github.com/FlaSpaceInst/EZ-RASSOR.git + cd EZ-RASSOR + +Then, let the ``develop.sh`` script do the heavy lifting! On Ubuntu Xenial or Ubuntu Bionic, creating a catkin workspace and building all packages is achieved with these commands::: + # By default, all ROS packages in the *packages* folder will be installed + sh develop.sh setup + sh develop.sh link + sh develop.sh resolve + sh develop.sh build + sh develop.sh install ** RESTART TERMINAL ** If you encounter ``Sub-process /usr/bin/dpkg returned an error code...``, try to fix the broken install with the following command, then rerun the original command: -:: + +.. code-block:: bash + sudo apt --fix-broken install ** RERUN ORIGINAL COMMAND ** @@ -37,40 +50,40 @@ Everything's installed now! Proceed to the `usage`_ section. CUSTOMIZED INSTALLATION ----------------------- -Before performing a custom installation, you'll need to install `ROS manually`_. You'll also want to install the `ROS build tools`_. Finally, you'll need to familiarize yourself with the installation script. The script's general syntax looks like this: -:: - sh install.sh [arguments...] - -All of the following are valid ```` options: - -``ros`` - Install the ROS suite of software. This software is required to operate the EZ-RASSOR (and must be installed first). This script will automatically install ROS for Ubuntu Xenial and Ubuntu Bionic. For all other systems, ROS must be installed manually. After installing ROS with this script, **you must restart your terminal before proceeding**. -``tools`` - Install the ROS build tools that are required to build the EZ-RASSOR core packages. Again, this suite of software can only be installed automatically on Ubuntu Xenial and Ubuntu Bionic. For all other systems, you must install these tools manually. -``packages [-e, --except | -o, --only ]`` - Install the core EZ-RASSOR packages. After installing these packages, **you must restart your terminal for changes to take effect**. Ignore specific packages with the ``-e`` or ``--except`` flag. Install specific packages with the ``-o`` or ``--only`` flag. -``help`` - Display a help menu. - -Once you have ROS and the ROS build tools installed, you can install any combination of our packages using the installation script. Here are some examples: -:: - # Install all packages except 'ezrassor_autonomous_control'. - sh install.sh packages --except ezrassor_autonomous_control - - # Install all packages except the simulation packages. - sh install.sh packages --except ezrassor_sim_control ezrassor_sim_description ezrassor_sim_gazebo +If you want to install specific EZ-RASSOR packages, you can use the same develop.sh script: + +Make sure you have already run the setup command at least once: + +.. code-block:: bash + + sh develop.sh setup + +Then, you can call the relink function and use -o to pass in the package name(s) you would like to install + +.. code-block:: bash + + sh develop.sh relink -o ezrassor_sim_control ezrassor_sim_description ezrassor_sim_gazebo + sh develop.sh build + sh develop.sh install - # Install only the communication packages. - sh install.sh packages --only ezrassor_controller_server ezrassor_joy_translator ezrassor_topic_switch +Alternatively, you can also call the relink function and use the -e flag to make the script install all *but* the specified package(s): + +.. code-block:: bash + + sh develop.sh relink -e ezrassor_swarm_control USAGE ----- The EZ-RASSOR is controlled via a collection of *launch files*. These files contain lists of commands that start up the robot's systems and the simulation environment. They are read, understood, and executed by a core ROS utility called ``roslaunch``, whose general syntax is as follows: -:: + +.. code-block:: bash + roslaunch [arguments...] Each launch file is located in one of our packages, and the most important launch files are located in the ``ezrassor_launcher`` package. To learn more about a specific launch file, visit that launch file's package's `wiki`_ page (via the navigation menu on the right). Here are some example commands that show launch files in action: -:: + +.. code-block:: bash + # Launch the simulation with a single robot controlled by the mobile app. roslaunch ezrassor_launcher configurable_simulation.launch control_methods:=app @@ -126,8 +139,6 @@ AUTHORS .. _`contributing guidelines`: CONTRIBUTING.rst .. _`license`: LICENSE.txt .. _`usage`: README.rst#Usage -.. _`ROS manually`: http://wiki.ros.org/ROS/Installation -.. _`ROS build tools`: http://wiki.ros.org/kinetic/Installation/Source#Prerequisites .. _`wiki page for the ezrassor_launcher`: https://github.com/FlaSpaceInst/EZ-RASSOR/wiki/ezrassor_launcher .. _`Sean Rapp`: https://github.com/shintoo .. _`Ron Marrero` : https://github.com/CSharpRon @@ -149,3 +160,8 @@ AUTHORS .. _`Martin Power` : https://github.com/martinpower .. _`Daniel Simoes` : https://github.com/RuptorT .. _`Autumn Esponda` : https://github.com/autumnesponda +.. _`ROS Melodic` : http://wiki.ros.org/melodic/Installation/Ubuntu +.. _`Python 2.7` : https://www.python.org/download/releases/2.7/ +.. _`Pip` : https://pip.pypa.io/en/stable/installing/ +.. _`rosdep` : http://wiki.ros.org/rosdep +.. _`build-essential` : https://packages.ubuntu.com/bionic/build-essential \ No newline at end of file diff --git a/install.sh b/install.sh deleted file mode 100755 index ca8985e6..00000000 --- a/install.sh +++ /dev/null @@ -1,267 +0,0 @@ -#!/bin/sh -# This script can install ROS, ROS build tools, and EZ-RASSOR components on systems -# running Ubuntu Xenial and Ubuntu Bionic. -# Written by Tiger Sachse. - -USER_SHELLS="bash zsh" -SH_SETUP_FILE="setup.sh" -README_FILE="docs/README.rst" -CHANGES_FILE="docs/CHANGES.txt" -LICENSE_FILE="docs/LICENSE.txt" -SUPERPACKAGES_DIR="packages" -ROS_INSTALL_PARTIAL_DIR="/opt/ros" -WORKSPACE_SOURCE_RELATIVE_DIR="src" -EZRASSOR_INSTALL_DIR="$HOME/.ezrassor" -INSTALLED_DOCS_DIR="$EZRASSOR_INSTALL_DIR/docs" -WORKSPACE_PARTIAL_DIR="/tmp/ezrassor_workspace" -RECV_KEY="C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654" -USAGE_STRING="Usage: sh install.sh [arguments...]\n" -CATKIN_MAKE_ISOLATED_BIN="$WORKSPACE_SOURCE_RELATIVE_DIR/catkin/bin/catkin_make_isolated" - -# Throw a help message at the user. -throw_help() { - printf "$USAGE_STRING" - cat "$README_FILE" \ - | grep '^``' -A 1 \ - | sed 's/ //g' \ - | sed 's/^``/#/g' \ - | fold -s -w 75 \ - | sed '/^#/! s/^/ /g' \ - | sed '/``/ s/``/"/g' \ - | sed '/^#/ s/"//g' \ - | sed 's/**//g' \ - | tr '#' '\n' -} - -# Print an error message and exit this script. -throw_error() { - printf "%s\n" "$@" - trap ":" 0 - exit 1 -} - -# If required commands don't exist, throw an error. -require() { - missing_requirement=false - for requirement in "$@"; do - set +e - command -v "$requirement" > /dev/null 2>&1 - if [ $? -ne 0 ]; then - printf "Required but not installed: $requirement\n" - missing_requirement=true - fi - set -e - done - if [ "$missing_requirement" = "true" ]; then - throw_error "Please install all missing components before proceeding. Aborting..." - fi -} - -# Source setup files within a given directory in the user's RC files. -source_setups_in_directory() { - must_restart=false - partial_source_file="$1/setup" - for user_shell in $USER_SHELLS; do - shellrc_file="$HOME/.${user_shell}rc" - if [ -f "$shellrc_file" ]; then - source_file="$partial_source_file.$user_shell" - source_line=". \"$source_file\"" - - printf "Attempting to source setup script for %s: " "$user_shell" - if cat "$shellrc_file" | grep -Fq "$source_line"; then - printf "Previously sourced.\n" - else - printf "%s\n" \ - "" \ - "# Source a ROS setup file, if it exists." \ - "if [ -f \"$source_file\" ]; then" \ - " $source_line" \ - "fi" >> "$shellrc_file" - printf "Successfully sourced.\n" - must_restart=true - fi - fi - done - - if [ "$must_restart" = "true" ]; then - printf "\n\n******** %s ********\n" \ - "Restart your terminal for changes to take effect." - fi -} - -# Check if the first argument exists in the remaining list of arguments. -argument_in_list() { - argument="$1" - shift - for item in "$@"; do - case "$argument" in - $item) - return 0 - esac - done - - return 1 -} - -# Install ROS automatically with APT. -install_ros() { - require "sudo" "apt" "apt-key" - os_version="$1" - ros_version="$2" - key_server="$3" - - # Add the correct repository key to APT for ROS. - echo_command="echo \"deb http://packages.ros.org/ros/ubuntu $os_version main\"" - ros_latest_dir="/etc/apt/sources.list.d/ros-latest.list" - sudo sh -c "$echo_command > $ros_latest_dir" - sudo apt-key adv --keyserver "$key_server" --recv-key "$RECV_KEY" - sudo apt update - - # Install ROS and initialize rosdep. - sudo apt install -y "ros-${ros_version}-ros-base" python-rosdep - set +e - sudo rosdep init - set -e - rosdep update - - # Source the ROS installation. - source_setups_in_directory "$ROS_INSTALL_PARTIAL_DIR/$ros_version" -} - -# Install build tools required to build EZ-RASSOR packages. -install_ros_tools() { - require "sudo" "apt" - sudo apt install -y python-rosdep python-pip build-essential - set +e - sudo rosdep init - set -e - rosdep update -} - -# Install build tools required to build the EZ-RASSOR controller. -install_app_tools() { - require "sudo" "apt" - sudo apt install -y curl software-properties-common - curl -sL https://deb.nodesource.com/setup_12.x | sudo -E bash - - sudo apt install nodejs - sudo npm install expo-cli --global -} - -# Install EZ-RASSOR packages from source. -install_packages() { - require "catkin_make" "rosdep" "pip" - - # Create a temporary workspace. - workspace_dir="${WORKSPACE_PARTIAL_DIR}_$(date +%s)" - workspace_source_dir="$workspace_dir/$WORKSPACE_SOURCE_RELATIVE_DIR" - mkdir -p "$workspace_source_dir" - - # Determine if the user wants to exclude or include only certain packages. - link_only_in_list=false - link_except_in_list=false - if [ $# -gt 1 ]; then - case "$1" in - "-o"|"--only") - link_only_in_list=true - shift - ;; - "-e"|"--except") - link_except_in_list=true - shift - ;; - esac - fi - - # Link all of the packages that the user desires. - for collection_dir in "$PWD/$SUPERPACKAGES_DIR"; do - for superpackage_dir in "$collection_dir"/*; do - for package_dir in "$superpackage_dir"/*; do - if [ ! -d "$package_dir" ]; then - : - elif [ "$link_only_in_list" = "true" ]; then - if argument_in_list "$(basename "$package_dir")" "$@"; then - ln -s -f "$package_dir" "$workspace_source_dir" - fi - elif [ "$link_except_in_list" = "true" ]; then - if ! argument_in_list "$(basename "$package_dir")" "$@"; then - ln -s -f "$package_dir" "$workspace_source_dir" - fi - else - ln -s -f "$package_dir" "$workspace_source_dir" - fi - done - done - done - - # Install all of the dependencies of the linked packages in the temporary - # workspace. - cd "$workspace_dir" - rosdep install --from-paths "$WORKSPACE_SOURCE_RELATIVE_DIR" \ - --ignore-src \ - -y - - # Build and install the linked packages into the EZRASSOR_INSTALL_DIR. - catkin_make - mkdir -p "$EZRASSOR_INSTALL_DIR" - catkin_make -DCMAKE_INSTALL_PREFIX="$EZRASSOR_INSTALL_DIR" install - - cd - > /dev/null 2>&1 - - # Copy the most important documentation into the EZRASSOR_INSTALL_DIR. - mkdir -p "$INSTALLED_DOCS_DIR" - cp "$README_FILE" "$INSTALLED_DOCS_DIR" - cp "$CHANGES_FILE" "$INSTALLED_DOCS_DIR" - cp "$LICENSE_FILE" "$INSTALLED_DOCS_DIR" - - # Source the installed EZ-RASSOR packages. - source_setups_in_directory "$EZRASSOR_INSTALL_DIR" -} - -# THE SCRIPT BEGINS HERE. -# Throw a message if the script quits early, and tell the script to quit after -# any non-zero error message. -trap 'throw_error "Something went horribly wrong."' 0 -set -e - -# Set the OS version, ROS version, and necessary key server (assuming the user -# is running a supported operating system). -require "lsb_release" -os_version="$(lsb_release -sc)" -if [ "$os_version" = "xenial" ]; then - ros_version="kinetic" - key_server="hkp://ha.pool.sks-keyservers.net:80" -elif [ "$os_version" = "bionic" ]; then - ros_version="melodic" - key_server="hkp://keyserver.ubuntu.com:80" -else - throw_error "This script can only automatically install ROS for Ubuntu Xenial" \ - "and Ubuntu Bionic. Your operating system is not supported. :(" \ - "You may be able to find ROS installation instructions for your" \ - "operating system on the ROS wiki." -fi - -# Install the appropriate software based on the user's first argument to this script. -case "$1" in - "ros") - install_ros "$os_version" "$ros_version" "$key_server" - ;; - "tools"|"ros-tools") # Remove the first tag once #247 is ready to go - install_ros_tools - ;; - "app-tools") - install_app_tools - ;; - "packages") - shift - install_packages "$@" - ;; - "help") - throw_help - ;; - *) - printf "$USAGE_STRING" - ;; -esac - -# If the script makes it this far, exit normally without an error messsage. -trap ":" 0 From 2b8c4481ad9752ea77b69f12e87e8d6d9bf8a05f Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 18:28:47 -0400 Subject: [PATCH 02/17] Adding Dockerfile for GitHub Action --- .github/build/Dockerfile | 7 +++++++ .github/build/entrypoint.sh | 11 +++++++++++ .github/workflows/rostest-dev.yml | 17 ++--------------- develop.sh | 9 ++++++++- 4 files changed, 28 insertions(+), 16 deletions(-) create mode 100644 .github/build/Dockerfile create mode 100755 .github/build/entrypoint.sh diff --git a/.github/build/Dockerfile b/.github/build/Dockerfile new file mode 100644 index 00000000..ff8e1404 --- /dev/null +++ b/.github/build/Dockerfile @@ -0,0 +1,7 @@ +FROM ros:melodic + +# Copy code repository and script that specifies test procedure +COPY . /ez-rassor +COPY .github/build/entrypoint.sh /entrypoint.sh + +CMD ["/bin/sh", "/entrypoint.sh"] \ No newline at end of file diff --git a/.github/build/entrypoint.sh b/.github/build/entrypoint.sh new file mode 100755 index 00000000..95f034d5 --- /dev/null +++ b/.github/build/entrypoint.sh @@ -0,0 +1,11 @@ +#!/bin/sh -l + +cd /ez-rassor + +# Go through the setup commands +sh develop.sh setup +sh develop.sh link +sh develop.sh build + +# Run the test function (uses catkin_make run_tests) +sh develop.sh test \ No newline at end of file diff --git a/.github/workflows/rostest-dev.yml b/.github/workflows/rostest-dev.yml index 54a9f0c0..36064743 100644 --- a/.github/workflows/rostest-dev.yml +++ b/.github/workflows/rostest-dev.yml @@ -9,18 +9,5 @@ jobs: rostest: - runs-on: ubuntu-latest - container: ros:melodic - - steps: - - - uses: actions/checkout@v1 - - - name: Prep Workspace and Build Packages - run: | - sh develop.sh setup - sh develop.sh link - sh develop.sh build - - - name: Run Tests - run: sh develop.sh test + using: 'docker' + image: '.github/build/Dockerfile' \ No newline at end of file diff --git a/develop.sh b/develop.sh index 2115ac77..e7281045 100755 --- a/develop.sh +++ b/develop.sh @@ -186,9 +186,16 @@ test_packages() { catkin_make run_tests # This command will return a status code of 0 or 1 depending on if the previous tests succeeded - catkin_test_results + (catkin_test_results) + local result=$? + # After we return to the main directory, return the status code from the test results cd - > /dev/null 2>&1 + if [ $result -ne 0 ]; then + return 1 + else + return 0 + fi } # Change the version number of all the packages. From ea903aa4f879bdc7147d74da9bdd89be7c79f81d Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 18:42:22 -0400 Subject: [PATCH 03/17] Adding specific action for rostest --- .github/actions/rostest.yml | 8 ++++++++ .github/workflows/dev.yml | 15 +++++++++++++++ .github/workflows/rostest-dev.yml | 13 ------------- 3 files changed, 23 insertions(+), 13 deletions(-) create mode 100644 .github/actions/rostest.yml create mode 100644 .github/workflows/dev.yml delete mode 100644 .github/workflows/rostest-dev.yml diff --git a/.github/actions/rostest.yml b/.github/actions/rostest.yml new file mode 100644 index 00000000..dd03ae23 --- /dev/null +++ b/.github/actions/rostest.yml @@ -0,0 +1,8 @@ +# GitHub Action +name: 'rostest' +description: 'Run rostest on the official ROS:Melodic docker image' +inputs: +outputs: +runs: + using: 'docker' + image: '.github/build/Dockerfile' \ No newline at end of file diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml new file mode 100644 index 00000000..f8e2d532 --- /dev/null +++ b/.github/workflows/dev.yml @@ -0,0 +1,15 @@ +name: rostest-dev + +on: + push: + branches: + - github-actions + +jobs: + + rostest: + + runs-on: ubuntu-latest + steps: + - name: Run rostest on repository + uses: .github/actions \ No newline at end of file diff --git a/.github/workflows/rostest-dev.yml b/.github/workflows/rostest-dev.yml deleted file mode 100644 index 36064743..00000000 --- a/.github/workflows/rostest-dev.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: rostest-dev - -on: - push: - branches: - - github-actions - -jobs: - - rostest: - - using: 'docker' - image: '.github/build/Dockerfile' \ No newline at end of file From 5224849d4c72ecf5674f2b0c1697545c71de1823 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 18:51:00 -0400 Subject: [PATCH 04/17] Testing using docker commands directly in the workflow --- .github/workflows/dev.yml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index f8e2d532..d465fd34 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -1,4 +1,4 @@ -name: rostest-dev +name: dev on: push: @@ -11,5 +11,8 @@ jobs: runs-on: ubuntu-latest steps: + - uses: actions/checkout@v2 - name: Run rostest on repository - uses: .github/actions \ No newline at end of file + run: | + docker build -t ez-rassor_rostest -f .github/build/Dockerfile . + docker run ez-rassor_rostest \ No newline at end of file From 3f182081ea65b37a81951b3a888a732a9135e997 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 18:53:39 -0400 Subject: [PATCH 05/17] Intentionally causing test failure --- .../communication/ezrassor_topic_switch/test/test_topic_switch | 1 + 1 file changed, 1 insertion(+) diff --git a/packages/communication/ezrassor_topic_switch/test/test_topic_switch b/packages/communication/ezrassor_topic_switch/test/test_topic_switch index 1ebed2af..2a39a4be 100755 --- a/packages/communication/ezrassor_topic_switch/test/test_topic_switch +++ b/packages/communication/ezrassor_topic_switch/test/test_topic_switch @@ -109,6 +109,7 @@ class TestTopicSwitch(unittest.TestCase): MAXIMUM_RANDOM_INTEGER, ) self.__publish_and_sleep(publisher, random_integer) + self.assertEqual(1, 2) if expect_success: self.assertEqual(self.__observe_output_data(), random_integer) else: From 3b024e4a5c702091086db25e590de1ab3a0dd455 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 19:55:52 -0400 Subject: [PATCH 06/17] Removing failing test case --- .github/actions/rostest.yml | 8 -------- .../ezrassor_topic_switch/test/test_topic_switch | 1 - 2 files changed, 9 deletions(-) delete mode 100644 .github/actions/rostest.yml diff --git a/.github/actions/rostest.yml b/.github/actions/rostest.yml deleted file mode 100644 index dd03ae23..00000000 --- a/.github/actions/rostest.yml +++ /dev/null @@ -1,8 +0,0 @@ -# GitHub Action -name: 'rostest' -description: 'Run rostest on the official ROS:Melodic docker image' -inputs: -outputs: -runs: - using: 'docker' - image: '.github/build/Dockerfile' \ No newline at end of file diff --git a/packages/communication/ezrassor_topic_switch/test/test_topic_switch b/packages/communication/ezrassor_topic_switch/test/test_topic_switch index 2a39a4be..1ebed2af 100755 --- a/packages/communication/ezrassor_topic_switch/test/test_topic_switch +++ b/packages/communication/ezrassor_topic_switch/test/test_topic_switch @@ -109,7 +109,6 @@ class TestTopicSwitch(unittest.TestCase): MAXIMUM_RANDOM_INTEGER, ) self.__publish_and_sleep(publisher, random_integer) - self.assertEqual(1, 2) if expect_success: self.assertEqual(self.__observe_output_data(), random_integer) else: From 5baa40c6519899a99efc332e46202181f84332b7 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 21:13:30 -0400 Subject: [PATCH 07/17] Adding initial flake8 --- .github/workflows/dev.yml | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index d465fd34..2b34df5b 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -10,9 +10,19 @@ jobs: rostest: runs-on: ubuntu-latest + + # Builds a docker image with the repository inside. The executable for the container installs all packages and runs rostest. steps: - uses: actions/checkout@v2 - name: Run rostest on repository run: | docker build -t ez-rassor_rostest -f .github/build/Dockerfile . - docker run ez-rassor_rostest \ No newline at end of file + docker run ez-rassor_rostest + + flake8: + + runs-on: ubuntu-latest + + # Runs flake8 linter on all Python code. Saves changes as annotations to a PR. + steps: + - uses: TrueBrain/actions-flake8@master From 0ee8b5129d627dbd00a02e69ce9f21678aaef117 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 21:16:27 -0400 Subject: [PATCH 08/17] Adding checkout step to flake8 --- .github/workflows/dev.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index 2b34df5b..2d5bada0 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -25,4 +25,5 @@ jobs: # Runs flake8 linter on all Python code. Saves changes as annotations to a PR. steps: + - uses: actions/checkout@v2 - uses: TrueBrain/actions-flake8@master From 4f88f96a1d728d3b4891b53cd5cea8b2face62d5 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 21:22:21 -0400 Subject: [PATCH 09/17] Black formatting applied to all python files for PEP8 compliance --- dem_scripts/auto_gen_wrld/model_create.py | 53 ++- .../auto_gen_wrld/world_file_create.py | 44 +- dem_scripts/auto_move/move_model_world.py | 101 ++-- dem_scripts/extract_tile/tile.py | 28 +- dem_scripts/get_elev/get_coords.py | 9 +- dem_scripts/get_elev/readDEM.py | 18 +- dem_scripts/mk_gaz_wrld/check_dem_size.py | 4 +- experimental/cosmic-gps/cell_gps.py | 115 +++-- experimental/cosmic-gps/centroid_tests.py | 19 +- experimental/cosmic-gps/cluster_tests.py | 8 +- experimental/cosmic-gps/measurement.py | 164 ++++--- experimental/cosmic-gps/measurement_tests.py | 206 +++++--- experimental/cosmic-gps/show_image.py | 10 +- experimental/cosmic-gps/star.py | 49 +- experimental/cosmic-gps/star_cat.py | 441 ++++++++++-------- experimental/cosmic-gps/star_cat_tests.py | 32 +- experimental/cosmic-gps/star_ref.py | 16 +- experimental/cosmic-gps/star_tests.py | 31 +- experimental/cosmic-gps/thresh_tests.py | 4 +- .../ezrassor_autonomous_control/setup.py | 3 +- .../ezrassor_autonomous_control/ai_objects.py | 77 ++- .../ezrassor_autonomous_control/arm_force.py | 67 ++- .../auto_functions.py | 72 +-- .../autonomous_control.py | 215 ++++++--- .../nav_functions.py | 15 +- .../obstacle_detection.py | 87 ++-- .../park_ranger.py | 259 ++++++---- .../pointcloud_processor.py | 20 +- .../utility_functions.py | 159 ++++--- .../autonomy/ezrassor_swarm_control/setup.py | 3 +- .../ezrassor_swarm_control/constants.py | 5 +- .../ezrassor_swarm_control/path_planner.py | 68 ++- .../ezrassor_swarm_control/swarm_control.py | 129 +++-- .../ezrassor_swarm_control/swarm_utils.py | 4 +- .../ezrassor_swarm_control/waypoint_client.py | 86 +++- .../ezrassor_controller_server/setup.py | 3 +- .../controller_server.py | 71 ++- .../ezrassor_joy_translator/setup.py | 3 +- .../ezrassor_joy_translator/joy_translator.py | 102 ++-- .../ezrassor_topic_switch/setup.py | 3 +- .../ezrassor_topic_switch/topic_switch.py | 33 +- .../simulation/ezrassor_sim_control/setup.py | 3 +- .../ezrassor_sim_control/sim_arms_driver.py | 16 +- .../ezrassor_sim_control/sim_drums_driver.py | 19 +- .../ezrassor_sim_control/sim_wheels_driver.py | 5 +- 45 files changed, 1712 insertions(+), 1167 deletions(-) diff --git a/dem_scripts/auto_gen_wrld/model_create.py b/dem_scripts/auto_gen_wrld/model_create.py index cd5e2cc0..3799477d 100755 --- a/dem_scripts/auto_gen_wrld/model_create.py +++ b/dem_scripts/auto_gen_wrld/model_create.py @@ -3,6 +3,8 @@ import sys """ Given a uri tag, modify model_name and dem_name parts of the path """ + + def replace_ref(tag, model_name, dem_name): first = True tag_text = tag.text @@ -19,7 +21,10 @@ def replace_ref(tag, model_name, dem_name): tag.text = new_tag_text + """ Given a uri tag, modify model_name part of the path """ + + def replace_diff_mod(tag, model_name): first = True tag_text = tag.text @@ -36,38 +41,41 @@ def replace_diff_mod(tag, model_name): tag.text = new_tag_text + """ Update a tag's name attribute with model_name """ + + def replace_top_model_name(tag, model_name): - tag.attrib['name'] = model_name + tag.attrib["name"] = model_name + """ Update a tag's text field with string """ + + def replace_text_field(tag, string): tag.text = string + """ Fill in model.sdf template """ + + def model_trav(path_to_file, model_name, dem_name, w, h, squish_factor): tree = etree.parse(path_to_file) - top_tag = tree.xpath( - "//sdf/model" - )[0] + top_tag = tree.xpath("//sdf/model")[0] - collision_uri_tag = tree.xpath( - "//sdf/model/link/collision/geometry/heightmap/uri" - )[0] - visual_uri_tag = tree.xpath( - "//sdf/model/link/visual/geometry/heightmap/uri" - )[0] + collision_uri_tag = tree.xpath("//sdf/model/link/collision/geometry/heightmap/uri")[ + 0 + ] + visual_uri_tag = tree.xpath("//sdf/model/link/visual/geometry/heightmap/uri")[0] visual_tex_tag = tree.xpath( - "//sdf/model/link/visual/geometry/heightmap/texture/diffuse" - )[0] + "//sdf/model/link/visual/geometry/heightmap/texture/diffuse" + )[0] collision_size_tag = tree.xpath( - "//sdf/model/link/collision/geometry/heightmap/size" - )[0] - visual_size_tag = tree.xpath( - "//sdf/model/link/visual/geometry/heightmap/size" - )[0] + "//sdf/model/link/collision/geometry/heightmap/size" + )[0] + visual_size_tag = tree.xpath("//sdf/model/link/visual/geometry/heightmap/size")[0] replace_ref(collision_uri_tag, model_name, dem_name) @@ -83,6 +91,7 @@ def model_trav(path_to_file, model_name, dem_name, w, h, squish_factor): tree.write(path_to_file, xml_declaration=True) + def main(): if len(sys.argv) < 6: @@ -100,10 +109,14 @@ def main(): squish_factor = sys.argv[6] model_trav( - path_to_model, model_name, dem_img_file, - dimmensions_w, dimmensions_h, - squish_factor + path_to_model, + model_name, + dem_img_file, + dimmensions_w, + dimmensions_h, + squish_factor, ) + if __name__ == "__main__": main() diff --git a/dem_scripts/auto_gen_wrld/world_file_create.py b/dem_scripts/auto_gen_wrld/world_file_create.py index feb02f82..40bc3686 100755 --- a/dem_scripts/auto_gen_wrld/world_file_create.py +++ b/dem_scripts/auto_gen_wrld/world_file_create.py @@ -4,44 +4,39 @@ import model_create as mc """ Fill in world template """ + + def world_trav(path_to_file, model_name, dem_name, w, h, squish_factor): tree = etree.parse(path_to_file) - tree.xpath("//sdf/world/state")[0].attrib['world_name'] = model_name \ - + "_world" - tree.xpath("//sdf/world/state/model")[0].attrib['name'] = model_name + tree.xpath("//sdf/world/state")[0].attrib["world_name"] = model_name + "_world" + tree.xpath("//sdf/world/state/model")[0].attrib["name"] = model_name - include_tag = tree.xpath( - "//sdf/world/include" - )[1].getchildren()[0] + include_tag = tree.xpath("//sdf/world/include")[1].getchildren()[0] - top_tag = tree.xpath( - "//sdf/world/model" - )[0] + top_tag = tree.xpath("//sdf/world/model")[0] - world_tag = tree.xpath( - "//sdf/world" - )[0] + world_tag = tree.xpath("//sdf/world")[0] collision_uri_tag = tree.xpath( "//sdf/world/model/link/collision/geometry/heightmap/uri" - )[0] + )[0] - visual_uri_tag = tree.xpath( - "//sdf/world/model/link/visual/geometry/heightmap/uri" - )[0] + visual_uri_tag = tree.xpath("//sdf/world/model/link/visual/geometry/heightmap/uri")[ + 0 + ] visual_tex_tag = tree.xpath( "//sdf/world/model/link/visual/geometry/heightmap/texture/diffuse" - )[0] + )[0] collision_size_tag = tree.xpath( "//sdf/world/model/link/collision/geometry/heightmap/size" - )[0] + )[0] visual_size_tag = tree.xpath( "//sdf/world/model/link/visual/geometry/heightmap/size" - )[0] + )[0] mc.replace_ref(collision_uri_tag, model_name, dem_name) @@ -61,6 +56,7 @@ def world_trav(path_to_file, model_name, dem_name, w, h, squish_factor): tree.write(path_to_file, xml_declaration=True) + def main(): if len(sys.argv) < 6: @@ -78,10 +74,14 @@ def main(): squish_factor = sys.argv[6] world_trav( - path_to_world, model_name, dem_img_file, - dimmensions_w, dimmensions_h, - squish_factor + path_to_world, + model_name, + dem_img_file, + dimmensions_w, + dimmensions_h, + squish_factor, ) + if __name__ == "__main__": main() diff --git a/dem_scripts/auto_move/move_model_world.py b/dem_scripts/auto_move/move_model_world.py index c4ead27c..4f3f76b0 100755 --- a/dem_scripts/auto_move/move_model_world.py +++ b/dem_scripts/auto_move/move_model_world.py @@ -2,53 +2,58 @@ import os import sys + def main(): - world_ext = "" - choice = "queue_packs/" - - l = os.listdir("./" + choice) - - for i in l: - - print("Processing {}".format(i)) - - folder_name = "" - cool = i.split("_") - num = len(cool) - new_str = "" - for p in range(num - 1): - if p < num - 2: - new_str += cool[p] + "_" - else: - new_str += cool[p] - - world_file = "" - folder_name = new_str - lister = os.listdir("./" + choice + i) - for file in lister: - if ".world" in file: - world_file = file - tester = os.listdir("./" + choice + i + "/" + folder_name - + "/materials/textures/" - ) - cache = "" - for file in tester: - if file != "AS16-110-18026HR-512x512.jpg": - temp = file.split(".") - cache = temp[0] - print("Remove cache for {}".format(cache)) - os.system("rm -rf $HOME/.gazebo/paging/" + cache) - - print("Copying model and world to appropriate folders") - os.system( - "cp -r ./" + choice + i + "/" + folder_name - + " $HOME/.gazebo/models/" - ) - os.system( - "cp ./" + choice + i + "/" + world_file - + " ../../packages/simulation/ezrassor_sim_gazebo/worlds" - ) - -if __name__ == '__main__': - main() + world_ext = "" + choice = "queue_packs/" + + l = os.listdir("./" + choice) + + for i in l: + + print("Processing {}".format(i)) + + folder_name = "" + cool = i.split("_") + num = len(cool) + new_str = "" + for p in range(num - 1): + if p < num - 2: + new_str += cool[p] + "_" + else: + new_str += cool[p] + + world_file = "" + folder_name = new_str + lister = os.listdir("./" + choice + i) + for file in lister: + if ".world" in file: + world_file = file + tester = os.listdir( + "./" + choice + i + "/" + folder_name + "/materials/textures/" + ) + cache = "" + for file in tester: + if file != "AS16-110-18026HR-512x512.jpg": + temp = file.split(".") + cache = temp[0] + print("Remove cache for {}".format(cache)) + os.system("rm -rf $HOME/.gazebo/paging/" + cache) + + print("Copying model and world to appropriate folders") + os.system( + "cp -r ./" + choice + i + "/" + folder_name + " $HOME/.gazebo/models/" + ) + os.system( + "cp ./" + + choice + + i + + "/" + + world_file + + " ../../packages/simulation/ezrassor_sim_gazebo/worlds" + ) + + +if __name__ == "__main__": + main() diff --git a/dem_scripts/extract_tile/tile.py b/dem_scripts/extract_tile/tile.py index 76ecdf4e..d3cdc44a 100755 --- a/dem_scripts/extract_tile/tile.py +++ b/dem_scripts/extract_tile/tile.py @@ -1,13 +1,14 @@ #!/usr/bin/env python import os, gdal, sys, ntpath + def main(): input_filename = sys.argv[1] input_desire_size = int(sys.argv[2]) # Creates the beginning of output file name with the input filename without # the extension part - output_filename = os.path.splitext(ntpath.basename(sys.argv[1]))[0] + '_tile_' + output_filename = os.path.splitext(ntpath.basename(sys.argv[1]))[0] + "_tile_" ds = gdal.Open(input_filename) band = ds.GetRasterBand(1) @@ -17,13 +18,28 @@ def main(): # Moves a kernel across the DEM to create smaller DEM tiles for i in range(0, xsize, input_desire_size): for j in range(0, ysize, input_desire_size): - com_string = "gdal_translate -of GTIFF -srcwin " + str(i)+ ", " \ - + str(j) + ", " + str(input_desire_size) + ", " \ - + str(input_desire_size) + " " + str(input_filename) \ - + " " + "/tmp/results/" + str(output_filename) \ - + str(i) + "_" + str(j) + ".tif" + com_string = ( + "gdal_translate -of GTIFF -srcwin " + + str(i) + + ", " + + str(j) + + ", " + + str(input_desire_size) + + ", " + + str(input_desire_size) + + " " + + str(input_filename) + + " " + + "/tmp/results/" + + str(output_filename) + + str(i) + + "_" + + str(j) + + ".tif" + ) # Executes the command os.system(com_string) + if __name__ == "__main__": main() diff --git a/dem_scripts/get_elev/get_coords.py b/dem_scripts/get_elev/get_coords.py index 8066e0f6..1855d189 100644 --- a/dem_scripts/get_elev/get_coords.py +++ b/dem_scripts/get_elev/get_coords.py @@ -7,6 +7,8 @@ import numpy as np """ Returns the list of geocoordinates for each corner """ + + def get_corner_coordinates(geotransform, num_cols, num_rows): corners = [] @@ -24,11 +26,14 @@ def get_corner_coordinates(geotransform, num_cols, num_rows): y_arr.reverse() return corners + """ As the name says, reprojects coordinates from one system to another """ + + def reproj_coordinates(coords, src_spa_ref_sys, targ_spa_ref_sys): trans_coords = [] transform = osr.CoordinateTransformation(src_spa_ref_sys, targ_spa_ref_sys) for x, y in coords: - x, y, z = transform.TransformPoint(x,y) - trans_coords.append([x,y]) + x, y, z = transform.TransformPoint(x, y) + trans_coords.append([x, y]) return trans_coords diff --git a/dem_scripts/get_elev/readDEM.py b/dem_scripts/get_elev/readDEM.py index 9f0f4c14..f8365ffb 100755 --- a/dem_scripts/get_elev/readDEM.py +++ b/dem_scripts/get_elev/readDEM.py @@ -9,19 +9,22 @@ from scipy.signal import argrelextrema """ Print all elevation values from dem into txt file """ + + def printOriginElev(elev_arr, out_file): num_rows = elev_arr.shape[0] num_cols = elev_arr.shape[1] for i in range(0, num_rows): for j in range(0, num_cols): - print(elev_arr[i,j], end=' ', file=out_file) + print(elev_arr[i, j], end=" ", file=out_file) print("", file=out_file) + def main(): print("For: {}, extract elev data in: {}".format(sys.argv[1], sys.argv[2])) extr_data = open(sys.argv[2], "w") - #local_max = open(sys.argv[3], "w") + # local_max = open(sys.argv[3], "w") dataset = gdal.Open(sys.argv[1], gdal.GA_ReadOnly) if not dataset: @@ -38,9 +41,7 @@ def main(): num_rows = dataset.RasterYSize # Get coordinates in cartesian - ext = get_coords.get_corner_coordinates( - geotransform, num_cols, num_rows - ) + ext = get_coords.get_corner_coordinates(geotransform, num_cols, num_rows) # Get the coordinate systems to convert src_spa_ref_sys = osr.SpatialReference() @@ -53,10 +54,7 @@ def main(): ext, src_spa_ref_sys, targ_spa_ref_sys ) - print( - "Coordinates of each corner pixel in degree decimal:", - file=extr_data - ) + print("Coordinates of each corner pixel in degree decimal:", file=extr_data) print(geo_ext, file=extr_data) # loads up a channel of the image i.e. r from rgb @@ -69,7 +67,6 @@ def main(): if not min or not max: (min, max) = band.ComputeRasterMinMax(True) - # stores channel info as a large array, making it a np array allows for # functions and modules that come with numpy, we'll need it for # sampling and stuff @@ -92,5 +89,6 @@ def main(): band = None dataset = None + if __name__ == "__main__": main() diff --git a/dem_scripts/mk_gaz_wrld/check_dem_size.py b/dem_scripts/mk_gaz_wrld/check_dem_size.py index 63877ca7..765e7d8e 100755 --- a/dem_scripts/mk_gaz_wrld/check_dem_size.py +++ b/dem_scripts/mk_gaz_wrld/check_dem_size.py @@ -7,6 +7,7 @@ import osr import sys + def main(): # Open file dataset = gdal.Open(sys.argv[1], gdal.GA_ReadOnly) @@ -21,7 +22,8 @@ def main(): num_rows = dataset.RasterYSize print("{} {}".format(num_cols, num_rows)) else: - print("-1 -1") + print("-1 -1") + if __name__ == "__main__": main() diff --git a/experimental/cosmic-gps/cell_gps.py b/experimental/cosmic-gps/cell_gps.py index 93110366..65a7dfd0 100644 --- a/experimental/cosmic-gps/cell_gps.py +++ b/experimental/cosmic-gps/cell_gps.py @@ -22,39 +22,68 @@ def cell_gps_core(): # read the configuarion file - window_size = 9 # used for thresholding + window_size = 9 # used for thresholding image_center_x = 1511.5 image_center_y = 2015.5 - time_list = [13.0, 14.0, 15.0, 16.0, 17.0, 18.0, - 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, - 25.0, 26.0, 27.0, 28.0, 29.0, 30.0] - actual_gha_aries = [5.631667, 20.67333, 35.715, 50.755, 65.79667, - 80.838333, 95.878333, 110.92, 125.961667, 141.001667, - 156.04333, 171.08333, 186.125, 201.1667, 216.20667, - 231.248333, 246.29, 261.33] - - angle_size = 0.5 # keep angle size (0.5, 0.25 0.125, 0.0625) deg - fov = 90 # max fov 180 deg + time_list = [ + 13.0, + 14.0, + 15.0, + 16.0, + 17.0, + 18.0, + 19.0, + 20.0, + 21.0, + 22.0, + 23.0, + 24.0, + 25.0, + 26.0, + 27.0, + 28.0, + 29.0, + 30.0, + ] + actual_gha_aries = [ + 5.631667, + 20.67333, + 35.715, + 50.755, + 65.79667, + 80.838333, + 95.878333, + 110.92, + 125.961667, + 141.001667, + 156.04333, + 171.08333, + 186.125, + 201.1667, + 216.20667, + 231.248333, + 246.29, + 261.33, + ] + + angle_size = 0.5 # keep angle size (0.5, 0.25 0.125, 0.0625) deg + fov = 90 # max fov 180 deg scope_size = 30 search_angle = 30.0 epsilon_converge = 0.01 # initialize the star reference catalogue - star_catalogue = Star_Cat( 'star_catalogue_init.txt', - angle_size, - fov, - scope_size ) + star_catalogue = Star_Cat("star_catalogue_init.txt", angle_size, fov, scope_size) # initialize the calibration functions - gha_aries_calibration = Calibration_Function(time_list, - actual_gha_aries) + gha_aries_calibration = Calibration_Function(time_list, actual_gha_aries) - while (True): + while True: """ waits here until new image is published """ - img = cv.imread('d_03_12_20_t_22_16_15.jpg', 1) # pub by camera node - clock_time = 26.2708333 # system call + img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) # pub by camera node + clock_time = 26.2708333 # system call """ image processing step (camera dependent) """ @@ -74,7 +103,7 @@ def cell_gps_core(): # from the image center finally pair each star object with its # intensity and sort the pairs by intensity list_of_stars = [] - getkey = lambda item : item[0] + getkey = lambda item: item[0] for clustr in list_of_clusters: if clustr.shape() > 0.50: cluster_intensity = clustr.get_intensity() @@ -84,23 +113,23 @@ def cell_gps_core(): list_of_stars = sorted(list_of_stars, key=getkey, reverse=True) """ Star Matching """ - + count = 0 list_of_stars_size = len(list_of_stars) list_of_possible_matches = [] # loop five times enforced by count - while (count < 5): + while count < 5: # break early if too few stars if list_of_stars_size < 5: - break; + break - star1 = list_of_stars[count+0][1] - star2 = list_of_stars[count+1][1] - star3 = list_of_stars[count+2][1] - star4 = list_of_stars[count+3][1] - star5 = list_of_stars[count+4][1] + star1 = list_of_stars[count + 0][1] + star2 = list_of_stars[count + 1][1] + star3 = list_of_stars[count + 2][1] + star4 = list_of_stars[count + 3][1] + star5 = list_of_stars[count + 4][1] stars = (star1, star2, star3, star4, star5) # Try matching to the scope for the first try @@ -114,7 +143,7 @@ def cell_gps_core(): count += 1 list_of_stars_size -= 1 if len(list_of_possible_matches) > 0: - break # can be possibly many group of candidates. + break # can be possibly many group of candidates. # Position derivation # find the GHA using the time and calibration curve @@ -132,13 +161,23 @@ def cell_gps_core(): lamb3 = match[2][2] phi3 = match[2][3] d3 = match[2][4] - coor = determine_coordinate(lamb1, phi1, d1, - lamb2, phi2, d2, - lamb3, phi3, d3, - search_angle, epsilon_converge) + coor = determine_coordinate( + lamb1, + phi1, + d1, + lamb2, + phi2, + d2, + lamb3, + phi3, + d3, + search_angle, + epsilon_converge, + ) gp = calculate_geographic_position(coor, gha_aries) - d = calculate_angular_distance(position_longitude, - position_latitude, gp[0], gp[1]) + d = calculate_angular_distance( + position_longitude, position_latitude, gp[0], gp[1] + ) if d < min_d: save_coor = coor save_gp = gp @@ -147,9 +186,9 @@ def cell_gps_core(): position_latitude = save_gp[1] position_longitude = save_gp[0] star_catalogue.rescope(coor[0], coor[1]) - print 'Geographic Position', save_gp + print "Geographic Position", save_gp print save_match - break; + break # end diff --git a/experimental/cosmic-gps/centroid_tests.py b/experimental/cosmic-gps/centroid_tests.py index 3d0af13f..000dd41d 100644 --- a/experimental/cosmic-gps/centroid_tests.py +++ b/experimental/cosmic-gps/centroid_tests.py @@ -19,14 +19,14 @@ def main(): - img = cv.imread('d_03_12_20_t_22_16_15.jpg', 1) + img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) img = cv.cvtColor(img, cv.COLOR_RGB2GRAY) img = cv.bilateralFilter(img, 5, 150, 150) pixels = thresh_global(img, 50) list_of_clusters = cluster(pixels) list_of_stars = [] - getkey = lambda item : item[0] + getkey = lambda item: item[0] for clustr in list_of_clusters: cluster_center = clustr.centroid() cluster_intensity = clustr.get_intensity() @@ -34,16 +34,17 @@ def main(): list_of_stars.append([cluster_intensity, cluster_center, clustr]) list_of_stars = sorted(list_of_stars, key=getkey, reverse=True) - print '***Stars***' - print '' + print "***Stars***" + print "" for star in list_of_stars: - print '*****' - print 'Center:' + print "*****" + print "Center:" print star[1] - print '' - print 'Cluster:' + print "" + print "Cluster:" star[2].show() - print '_____' + print "_____" + if __name__ == "__main__": main() diff --git a/experimental/cosmic-gps/cluster_tests.py b/experimental/cosmic-gps/cluster_tests.py index 8c1d93b1..71327c14 100644 --- a/experimental/cosmic-gps/cluster_tests.py +++ b/experimental/cosmic-gps/cluster_tests.py @@ -18,18 +18,18 @@ def main(): - img = cv.imread('d_03_12_20_t_22_16_15.jpg', 1) + img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) img = cv.cvtColor(img, cv.COLOR_RGB2GRAY) img = cv.bilateralFilter(img, 5, 150, 150) pixels = thresh_global(img, 50) list_of_clusters = cluster(pixels) - print '***Clusters***' - print '' + print "***Clusters***" + print "" for clust in list_of_clusters: clust.show() - print '' + print "" if __name__ == "__main__": diff --git a/experimental/cosmic-gps/measurement.py b/experimental/cosmic-gps/measurement.py index 09e01315..f4a3eac2 100644 --- a/experimental/cosmic-gps/measurement.py +++ b/experimental/cosmic-gps/measurement.py @@ -9,31 +9,34 @@ def ra_to_decimal_deg(ra_h, ra_m, ra_s): - return (float(ra_h) + (float(ra_m) / 60) + (float(ra_s) /3600)) * 15 + return (float(ra_h) + (float(ra_m) / 60) + (float(ra_s) / 3600)) * 15 + def dec_to_decimal_deg(dec_d, dec_m, dec_s): if dec_d > 0: - return float(dec_d) + (float(dec_m) / 60) + (float(dec_s) /3600) + return float(dec_d) + (float(dec_m) / 60) + (float(dec_s) / 3600) else: - return float(dec_d) - (float(dec_m) / 60) - (float(dec_s) /3600) + return float(dec_d) - (float(dec_m) / 60) - (float(dec_s) / 3600) + def calculate_angular_distance(starA_ra, starA_dec, starB_ra, starB_dec): return degrees( - acos( - (sin(radians(starA_dec)) * - sin(radians(starB_dec)) - ) + - (cos(radians(starA_dec)) * - cos(radians(starB_dec)) * - cos(radians(starB_ra) - radians(starA_ra)) - ) - ) - ) + acos( + (sin(radians(starA_dec)) * sin(radians(starB_dec))) + + ( + cos(radians(starA_dec)) + * cos(radians(starB_dec)) + * cos(radians(starB_ra) - radians(starA_ra)) + ) + ) + ) + # used for position derivation takes three stars and distances to a shared # point and returns the coor, can be used with celestial or glocal positions -def determine_coordinate(lamb1, phi1, d1, lamb2, phi2, d2, - lamb3, phi3, d3, search_angle, epsilon): +def determine_coordinate( + lamb1, phi1, d1, lamb2, phi2, d2, lamb3, phi3, d3, search_angle, epsilon +): epsilon = 0.000000001 # take the smallest set to current if d3 > d2: @@ -58,20 +61,43 @@ def determine_coordinate(lamb1, phi1, d1, lamb2, phi2, d2, polarFlag = True if polarFlag: - cp = scan_polar(cur_lamb, cur_phi, cur_d, - lamb1, phi1, d1, - lamb2, phi2, d2, - lamb3, phi3, d3, epsilon) + cp = scan_polar( + cur_lamb, + cur_phi, + cur_d, + lamb1, + phi1, + d1, + lamb2, + phi2, + d2, + lamb3, + phi3, + d3, + epsilon, + ) else: - cp = scan_non_polar(cur_lamb, cur_phi, cur_d, - lamb1, phi1, d1, - lamb2, phi2, d2, - lamb3, phi3, d3, epsilon) + cp = scan_non_polar( + cur_lamb, + cur_phi, + cur_d, + lamb1, + phi1, + d1, + lamb2, + phi2, + d2, + lamb3, + phi3, + d3, + epsilon, + ) return cp -def scan_polar(cur_lamb, cur_phi, d, - lamb1, phi1, d1, lamb2, phi2, d2, lamb3, phi3, d3, epsilon): +def scan_polar( + cur_lamb, cur_phi, d, lamb1, phi1, d1, lamb2, phi2, d2, lamb3, phi3, d3, epsilon +): # if polar scan the equator starting at 0.0 increment = 0.1 temp_lamb = 0.0 @@ -81,19 +107,16 @@ def scan_polar(cur_lamb, cur_phi, d, while temp_lamb < 360.0: find_phi = 0.0 search_angle = 90.0 - dc = calculate_angular_distance(temp_lamb, find_phi, - cur_lamb, cur_phi) - c = abs(1 - (dc/d)) + dc = calculate_angular_distance(temp_lamb, find_phi, cur_lamb, cur_phi) + c = abs(1 - (dc / d)) while search_angle > epsilon: # Calculate distances at the search points phi_north = find_phi + search_angle phi_south = find_phi - search_angle - dn = calculate_angular_distance(temp_lamb, phi_north, - cur_lamb, cur_phi) - n = abs(1 - (dn/d)) - ds = calculate_angular_distance(temp_lamb, phi_south, - cur_lamb, cur_phi) - s = abs(1 - (ds/d)) + dn = calculate_angular_distance(temp_lamb, phi_north, cur_lamb, cur_phi) + n = abs(1 - (dn / d)) + ds = calculate_angular_distance(temp_lamb, phi_south, cur_lamb, cur_phi) + s = abs(1 - (ds / d)) # find the closest to 0.0 of n,e,s,w if s > n: t = n @@ -111,7 +134,7 @@ def scan_polar(cur_lamb, cur_phi, d, d1p = calculate_angular_distance(temp_lamb, find_phi, lamb1, phi1) d2p = calculate_angular_distance(temp_lamb, find_phi, lamb2, phi2) d3p = calculate_angular_distance(temp_lamb, find_phi, lamb3, phi3) - p = abs(1 - (d1p/d1)) + abs(1 - (d2p/d2)) + abs(1 - (d3p/d3)) + p = abs(1 - (d1p / d1)) + abs(1 - (d2p / d2)) + abs(1 - (d3p / d3)) if p_ratio > p: p_ratio = p min_lamb = temp_lamb @@ -120,10 +143,9 @@ def scan_polar(cur_lamb, cur_phi, d, return (min_lamb, min_phi) -def scan_non_polar(cur_lamb, cur_phi, d, - lamb1, phi1, d1, - lamb2, phi2, d2, - lamb3, phi3, d3, epsilon): +def scan_non_polar( + cur_lamb, cur_phi, d, lamb1, phi1, d1, lamb2, phi2, d2, lamb3, phi3, d3, epsilon +): # if not polar scan the equator starting west big_increment = 0.1 small_increment = 0.01 @@ -160,13 +182,12 @@ def scan_non_polar(cur_lamb, cur_phi, d, find_phi = cur_phi search_angle = small_increment dc = calculate_angular_distance(temp_lamb, find_phi, cur_lamb, cur_phi) - c = abs(1 - (dc/d)) + c = abs(1 - (dc / d)) while d > search_angle: # Calculate distances at the search points phi_north = cur_phi + search_angle - dn = calculate_angular_distance(temp_lamb, phi_north, - cur_lamb, cur_phi) - n = abs(1 - (dn/d)) + dn = calculate_angular_distance(temp_lamb, phi_north, cur_lamb, cur_phi) + n = abs(1 - (dn / d)) # find the closest to 0.0 of n if c > n: t = n @@ -181,26 +202,25 @@ def scan_non_polar(cur_lamb, cur_phi, d, d1p = calculate_angular_distance(temp_lamb, find_phi, lamb1, phi1) d2p = calculate_angular_distance(temp_lamb, find_phi, lamb2, phi2) d3p = calculate_angular_distance(temp_lamb, find_phi, lamb3, phi3) - p = abs(1 - (d1p/d1)) + abs(1 - (d2p/d2)) + abs(1 - (d3p/d3)) + p = abs(1 - (d1p / d1)) + abs(1 - (d2p / d2)) + abs(1 - (d3p / d3)) if p_ratio > p: p_ratio = p min_lamb = temp_lamb min_phi = find_phi temp_lamb += big_increment - + # Right to Left / Approach from center to bottom temp_lamb = lamb_east while temp_lamb > lamb_west: find_phi = cur_phi search_angle = small_increment dc = calculate_angular_distance(temp_lamb, find_phi, cur_lamb, cur_phi) - c = abs(1 - (dc/d)) + c = abs(1 - (dc / d)) while d > search_angle: # Calculate distances at the search points phi_south = cur_phi - search_angle - ds = calculate_angular_distance(temp_lamb, phi_south, - cur_lamb, cur_phi) - s = abs(1 - (ds/d)) + ds = calculate_angular_distance(temp_lamb, phi_south, cur_lamb, cur_phi) + s = abs(1 - (ds / d)) # find the closest to 0.0 of s if c > s: t = s @@ -215,7 +235,7 @@ def scan_non_polar(cur_lamb, cur_phi, d, d1p = calculate_angular_distance(temp_lamb, find_phi, lamb1, phi1) d2p = calculate_angular_distance(temp_lamb, find_phi, lamb2, phi2) d3p = calculate_angular_distance(temp_lamb, find_phi, lamb3, phi3) - p = abs(1 - (d1p/d1)) + abs(1 - (d2p/d2)) + abs(1 - (d3p/d3)) + p = abs(1 - (d1p / d1)) + abs(1 - (d2p / d2)) + abs(1 - (d3p / d3)) if p_ratio > p: p_ratio = p min_lamb = temp_lamb @@ -224,20 +244,19 @@ def scan_non_polar(cur_lamb, cur_phi, d, phi_north = cur_phi + d # Will not be greater than 90.0 phi_south = cur_phi - d # Will not be less than -90.0 - + # Bottom to Top / Approach from center to left temp_phi = phi_south while temp_phi < phi_north: find_lamb = cur_lamb search_angle = small_increment dc = calculate_angular_distance(find_lamb, temp_phi, cur_lamb, cur_phi) - c = abs(1 - (dc/d)) - while (2*d) > search_angle: + c = abs(1 - (dc / d)) + while (2 * d) > search_angle: # Calculate distances at the search points lamb_west = cur_lamb - search_angle - dw = calculate_angular_distance(lamb_west, temp_phi, - cur_lamb, cur_phi) - w = abs(1 - (dw/d)) + dw = calculate_angular_distance(lamb_west, temp_phi, cur_lamb, cur_phi) + w = abs(1 - (dw / d)) # find the closest to 0.0 of w if c > w: t = w @@ -252,26 +271,25 @@ def scan_non_polar(cur_lamb, cur_phi, d, d1p = calculate_angular_distance(find_lamb, temp_phi, lamb1, phi1) d2p = calculate_angular_distance(find_lamb, temp_phi, lamb2, phi2) d3p = calculate_angular_distance(find_lamb, temp_phi, lamb3, phi3) - p = abs(1 - (d1p/d1)) + abs(1 - (d2p/d2)) + abs(1 - (d3p/d3)) + p = abs(1 - (d1p / d1)) + abs(1 - (d2p / d2)) + abs(1 - (d3p / d3)) if p_ratio > p: p_ratio = p min_lamb = find_lamb min_phi = temp_phi temp_phi += big_increment - + # Top to Bottom / Approach from center to right temp_phi = phi_north while temp_phi > phi_south: find_lamb = cur_lamb search_angle = small_increment dc = calculate_angular_distance(find_lamb, temp_phi, cur_lamb, cur_phi) - c = abs(1 - (dc/d)) - while (2*d) > search_angle: + c = abs(1 - (dc / d)) + while (2 * d) > search_angle: # Calculate distances at the search points lamb_east = cur_lamb + search_angle - de = calculate_angular_distance(lamb_east, temp_phi, - cur_lamb, cur_phi) - e = abs(1 - (de/d)) + de = calculate_angular_distance(lamb_east, temp_phi, cur_lamb, cur_phi) + e = abs(1 - (de / d)) # find the closest to 0.0 of e if c > e: t = e @@ -286,13 +304,13 @@ def scan_non_polar(cur_lamb, cur_phi, d, d1p = calculate_angular_distance(find_lamb, temp_phi, lamb1, phi1) d2p = calculate_angular_distance(find_lamb, temp_phi, lamb2, phi2) d3p = calculate_angular_distance(find_lamb, temp_phi, lamb3, phi3) - p = abs(1 - (d1p/d1)) + abs(1 - (d2p/d2)) + abs(1 - (d3p/d3)) + p = abs(1 - (d1p / d1)) + abs(1 - (d2p / d2)) + abs(1 - (d3p / d3)) if p_ratio > p: p_ratio = p min_lamb = find_lamb min_phi = temp_phi temp_phi -= big_increment - + min_lamb = min_lamb - offset if min_lamb > 360.0: min_lamb = min_lamb - 360.0 @@ -301,6 +319,7 @@ def scan_non_polar(cur_lamb, cur_phi, d, min_phi = min_phi - offset return (min_lamb, min_phi, p_ratio) + # converts the cp to gp def calculate_geographic_position(coor, gha_aries): # clock_time @@ -316,10 +335,10 @@ def calculate_geographic_position(coor, gha_aries): latitude = coor[1] return (longitude, latitude) + # argv added in the following order: # x, x0, f(x0), x1, f(x1), ..., xn, f(xn) class Calibration_Function: - def __init__(self, var_list, resp_list): self.var_list = var_list self.resp_list = resp_list @@ -335,13 +354,12 @@ def neville_interpolation(self, query): num_coef -= 1 for i in range(num_coef): q_list1[0] = self.resp_list[i] - q_list2[0] = self.resp_list[i+1] + q_list2[0] = self.resp_list[i + 1] for j in range(num_coef): - if (j <= i): - q_list2[j+1] = (((query - self.var_list[(i+1)-(j+1)]) - * q_list2[j]) - - ((query - self.var_list[i+1]) * - q_list1[j])) / (self.var_list[i+1] - - self.var_list[(i+1)-(j+1)]) + if j <= i: + q_list2[j + 1] = ( + ((query - self.var_list[(i + 1) - (j + 1)]) * q_list2[j]) + - ((query - self.var_list[i + 1]) * q_list1[j]) + ) / (self.var_list[i + 1] - self.var_list[(i + 1) - (j + 1)]) q_list1 = list(q_list2) return q_list2[num_coef] diff --git a/experimental/cosmic-gps/measurement_tests.py b/experimental/cosmic-gps/measurement_tests.py index 097e0b01..50416671 100644 --- a/experimental/cosmic-gps/measurement_tests.py +++ b/experimental/cosmic-gps/measurement_tests.py @@ -8,6 +8,7 @@ from measurement import determine_coordinate from measurement import calculate_geographic_position + class StarTestContainer: def __init__(self, n, a, d, ra, dec): self.name = n @@ -16,16 +17,50 @@ def __init__(self, n, a, d, ra, dec): self.ra = ra self.dec = dec + def main(): # The raw data of angle corresponding to pixel distance from the center. - time_list = [13.0, 14.0, 15.0, 16.0, 17.0, 18.0, - 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, - 25.0, 26.0, 27.0, 28.0, 29.0, 30.0] - actual_gha_aries = [5.631667, 20.67333, 35.715, 50.755, 65.79667, - 80.838333, 95.878333, 110.92, 125.961667, 141.001667, - 156.04333, 171.08333, 186.125, 201.1667, 216.20667, - 231.248333, 246.29, 261.33] + time_list = [ + 13.0, + 14.0, + 15.0, + 16.0, + 17.0, + 18.0, + 19.0, + 20.0, + 21.0, + 22.0, + 23.0, + 24.0, + 25.0, + 26.0, + 27.0, + 28.0, + 29.0, + 30.0, + ] + actual_gha_aries = [ + 5.631667, + 20.67333, + 35.715, + 50.755, + 65.79667, + 80.838333, + 95.878333, + 110.92, + 125.961667, + 141.001667, + 156.04333, + 171.08333, + 186.125, + 201.1667, + 216.20667, + 231.248333, + 246.29, + 261.33, + ] gha_aries_calibration = Calibration_Function(time_list, actual_gha_aries) # coordinate covertion and distance tests @@ -33,109 +68,126 @@ def main(): stars1 = [] # Alpha Canis Minoris 1st brightest - star = StarTestContainer('Alpha Canis Minoris', - 122.405568693, - 25.1172042972, - ra_to_decimal_deg(7, 39, 18.12), - dec_to_decimal_deg(5, 13, 29.95)) + star = StarTestContainer( + "Alpha Canis Minoris", + 122.405568693, + 25.1172042972, + ra_to_decimal_deg(7, 39, 18.12), + dec_to_decimal_deg(5, 13, 29.95), + ) stars1.append(star) # Beta Geminorum 2nd brightest - star = StarTestContainer('Beta Geminorum', - 56.8996735767, - 6.70418748113, - ra_to_decimal_deg(7, 45, 18.95), - dec_to_decimal_deg(28, 1, 34.32)) + star = StarTestContainer( + "Beta Geminorum", + 56.8996735767, + 6.70418748113, + ra_to_decimal_deg(7, 45, 18.95), + dec_to_decimal_deg(28, 1, 34.32), + ) stars1.append(star) # Alpha Leonis 3rd brightest // Maybe angle and dist - star = StarTestContainer('Alpha Leonis', - 207.811973537, - 30.9907648048, - ra_to_decimal_deg(10, 8, 22.31), - dec_to_decimal_deg(11, 58, 1.95)) + star = StarTestContainer( + "Alpha Leonis", + 207.811973537, + 30.9907648048, + ra_to_decimal_deg(10, 8, 22.31), + dec_to_decimal_deg(11, 58, 1.95), + ) stars1.append(star) # Beta Aurigae 4th brightest // Maybe angle and dist - star = StarTestContainer('Beta Aurigae', - 13.6613254946, - 31.3855516368, - ra_to_decimal_deg(5, 59, 31.72), - dec_to_decimal_deg(44, 56, 50.76)) + star = StarTestContainer( + "Beta Aurigae", + 13.6613254946, + 31.3855516368, + ra_to_decimal_deg(5, 59, 31.72), + dec_to_decimal_deg(44, 56, 50.76), + ) stars1.append(star) # Alpha Geminorum 5th brightest - star = StarTestContainer('Alpha Geminorum', - 31.0045400679, - 9.44994245983, - ra_to_decimal_deg(7, 34, 35.86), - dec_to_decimal_deg(31, 53, 17.79)) + star = StarTestContainer( + "Alpha Geminorum", + 31.0045400679, + 9.44994245983, + ra_to_decimal_deg(7, 34, 35.86), + dec_to_decimal_deg(31, 53, 17.79), + ) stars1.append(star) # Gamma Leonis 6th brightest // Maybe angle and dist - star = StarTestContainer('Gamma Leonis', - 224.120815397, - 29.3912178846, - ra_to_decimal_deg(10, 19, 58.35), - dec_to_decimal_deg(19, 50, 29.35)) + star = StarTestContainer( + "Gamma Leonis", + 224.120815397, + 29.3912178846, + ra_to_decimal_deg(10, 19, 58.35), + dec_to_decimal_deg(19, 50, 29.35), + ) stars1.append(star) # Theta Aurigae 7th brightest - star = StarTestContainer('Theta Aurigae', - 28.4994353196, - 29.6438533899, - ra_to_decimal_deg(5, 59, 43.27), - dec_to_decimal_deg(37, 12, 45.30)) + star = StarTestContainer( + "Theta Aurigae", + 28.4994353196, + 29.6438533899, + ra_to_decimal_deg(5, 59, 43.27), + dec_to_decimal_deg(37, 12, 45.30), + ) stars1.append(star) # Beta Canis Minoris 8th brightest - star = StarTestContainer('Beta Canis Minoris', - 112.598947606, - 23.5008826209, - ra_to_decimal_deg(7, 27, 9.04), - dec_to_decimal_deg(8, 17, 21.54)) + star = StarTestContainer( + "Beta Canis Minoris", + 112.598947606, + 23.5008826209, + ra_to_decimal_deg(7, 27, 9.04), + dec_to_decimal_deg(8, 17, 21.54), + ) stars1.append(star) - print '' - + print "" # compare calculated distances while stars1: starA = stars1.pop() for starB in stars1: - actual = calculate_angular_distance(starA.ra, starA.dec, - starB.ra, starB.dec) - calcul = calculate_angular_distance(starA.angle, - 90.0 - starA.distance, - starB.angle, - 90.0 - starB.distance) - print 'Calculated distances between ',starA.name,' and ',starB.name - print 'Actual ', actual - print 'Calcul ', calcul - print 'diff', calcul - actual - print '' - + actual = calculate_angular_distance( + starA.ra, starA.dec, starB.ra, starB.dec + ) + calcul = calculate_angular_distance( + starA.angle, 90.0 - starA.distance, starB.angle, 90.0 - starB.distance + ) + print "Calculated distances between ", starA.name, " and ", starB.name + print "Actual ", actual + print "Calcul ", calcul + print "diff", calcul - actual + print "" + # Position derivation for top 3 - coor = determine_coordinate(ra_to_decimal_deg(7, 39, 18.12), # ACM - dec_to_decimal_deg(5, 13, 29.95), - 25.1172042972, - ra_to_decimal_deg(7, 45, 18.95), # BG - dec_to_decimal_deg(28, 1, 34.32), - 6.70418748113, - ra_to_decimal_deg(10, 8, 22.31), # AL - dec_to_decimal_deg(11, 58, 1.95), - 30.9907648048, - 30.0, 0.01) - print 'Derived celestial position, RA: ', coor[0], ' DEC: ', coor[1] - print '' - + coor = determine_coordinate( + ra_to_decimal_deg(7, 39, 18.12), # ACM + dec_to_decimal_deg(5, 13, 29.95), + 25.1172042972, + ra_to_decimal_deg(7, 45, 18.95), # BG + dec_to_decimal_deg(28, 1, 34.32), + 6.70418748113, + ra_to_decimal_deg(10, 8, 22.31), # AL + dec_to_decimal_deg(11, 58, 1.95), + 30.9907648048, + 30.0, + 0.01, + ) + print "Derived celestial position, RA: ", coor[0], " DEC: ", coor[1] + print "" + # Global Position Derivation for top 3 clock_time = 26.2708333 gha_aries = gha_aries_calibration.neville_interpolation(clock_time) - print 'GHA Aries: ', gha_aries + print "GHA Aries: ", gha_aries gp = calculate_geographic_position(coor, gha_aries) - print 'Derived geographic position, Long: ', gp[0], ' Lat: ', gp[1] - + print "Derived geographic position, Long: ", gp[0], " Lat: ", gp[1] if __name__ == "__main__": diff --git a/experimental/cosmic-gps/show_image.py b/experimental/cosmic-gps/show_image.py index a946f478..c7ae3a40 100644 --- a/experimental/cosmic-gps/show_image.py +++ b/experimental/cosmic-gps/show_image.py @@ -4,9 +4,10 @@ import numpy as np import cv2 as cv + def main(): - img = cv.imread('d_03_12_20_t_22_16_15.jpg', 1) + img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) img = cv.cvtColor(img, cv.COLOR_RGB2GRAY) img = cv.bilateralFilter(img, 5, 150, 150) @@ -18,12 +19,13 @@ def main(): window_width = int(img.shape[1] * scale) window_height = int(img.shape[0] * scale) - cv.namedWindow('Resized Window', cv.WINDOW_NORMAL) - cv.resizeWindow('Resized Window', window_width, window_height) + cv.namedWindow("Resized Window", cv.WINDOW_NORMAL) + cv.resizeWindow("Resized Window", window_width, window_height) - cv.imshow('Resized Window', img) + cv.imshow("Resized Window", img) cv.waitKey(0) cv.destroyAllWindows() + if __name__ == "__main__": main() diff --git a/experimental/cosmic-gps/star.py b/experimental/cosmic-gps/star.py index e103be26..5a0f056b 100644 --- a/experimental/cosmic-gps/star.py +++ b/experimental/cosmic-gps/star.py @@ -6,8 +6,8 @@ from math import sin from math import atan -class Star: +class Star: def __init__(self): # constructor # set by add_pixel() self.pixel_dict = dict() @@ -67,14 +67,14 @@ def centroid(self): running_total = 0 pixel_center = 0.5 for col_intensity in marginal_distribution_rows: - running_total += (pixel_center * col_intensity) + running_total += pixel_center * col_intensity pixel_center += 1 mean_i = running_total / self.intensity # get the mean of rows running_total = 0 pixel_center = 0.5 for row_intensity in marginal_distribution_cols: - running_total += (pixel_center * row_intensity) + running_total += pixel_center * row_intensity pixel_center += 1 mean_j = running_total / self.intensity self.star_center_row = mean_j + self.min_row @@ -86,33 +86,38 @@ def set_angles(self, image_center_x, image_center_y): opp = 0.0 adj = 0.0 init_direction_angle = 0.0 - if self.star_center_col >= image_center_x and (self.star_center_row < - image_center_y): + if self.star_center_col >= image_center_x and ( + self.star_center_row < image_center_y + ): opp = self.star_center_col - image_center_x adj = image_center_y - self.star_center_row init_direction_angle = 0.0 - elif self.star_center_col > image_center_x and (self.star_center_row >= - image_center_y): + elif self.star_center_col > image_center_x and ( + self.star_center_row >= image_center_y + ): opp = self.star_center_row - image_center_y adj = self.star_center_col - image_center_x init_direction_angle = 90.0 - elif self.star_center_col <= image_center_x and (self.star_center_row > - image_center_y): + elif self.star_center_col <= image_center_x and ( + self.star_center_row > image_center_y + ): opp = image_center_x - self.star_center_col adj = self.star_center_row - image_center_y init_direction_angle = 180.0 - elif self.star_center_col < image_center_x and (self.star_center_row <= - image_center_y): - opp = image_center_y - self.star_center_row + elif self.star_center_col < image_center_x and ( + self.star_center_row <= image_center_y + ): + opp = image_center_y - self.star_center_row adj = image_center_x - self.star_center_col init_direction_angle = 270.0 - temp_angle = atan(opp/adj) + temp_angle = atan(opp / adj) self.angle_of_direction = init_direction_angle + degrees(temp_angle) # find the angle distance from the center query = opp / sin(temp_angle) # Calibration Function - self.angle_distance_from_center = ((-0.0000015736242766902*query*query) - + (0.019311808618316 * query)) + self.angle_distance_from_center = (-0.0000015736242766902 * query * query) + ( + 0.019311808618316 * query + ) def get_intensity(self): return self.intensity @@ -133,16 +138,16 @@ def shape(self): # test function def show(self): - print '*****' - print 'Pixels:' + print "*****" + print "Pixels:" print self.pixel_dict - print '' - print 'Intensity:' + print "" + print "Intensity:" print self.intensity - print '' - print 'Shape:' + print "" + print "Shape:" print self.shape() - print '_____' + print "_____" # test function def quick_set_angles(self, aod, adfc): diff --git a/experimental/cosmic-gps/star_cat.py b/experimental/cosmic-gps/star_cat.py index 292d0a50..8be6ff1e 100644 --- a/experimental/cosmic-gps/star_cat.py +++ b/experimental/cosmic-gps/star_cat.py @@ -7,38 +7,36 @@ from measurement import dec_to_decimal_deg from measurement import calculate_angular_distance -class Star_Cat: +class Star_Cat: def __init__(self, image_file, angle_size, fov, scope_size): init_ra_position = 0.0 init_dec_position = 0.0 hfov = float(fov) / 2 num_angles = int(round(fov / angle_size)) + 1 - fptr = open(image_file, 'r') + fptr = open(image_file, "r") fptr.readline() # first line unimportant self.adj_star_list = dict() # each line has info for a star count_global = 0 while True: star_info = fptr.readline() - if star_info == '': - break; - star_splits = star_info.split(' ')[:-1] + if star_info == "": + break + star_splits = star_info.split(" ")[:-1] # Convert RA to decimal - ra = ra_to_decimal_deg(float(star_splits[3]), - float(star_splits[4]), - float(star_splits[5])) + ra = ra_to_decimal_deg( + float(star_splits[3]), float(star_splits[4]), float(star_splits[5]) + ) # Covert DEC to decimal - dec = dec_to_decimal_deg(float(star_splits[6]), - float(star_splits[7]), - float(star_splits[8])) + dec = dec_to_decimal_deg( + float(star_splits[6]), float(star_splits[7]), float(star_splits[8]) + ) # Create star_ref self.adj_star_list[int(star_splits[0])] = Star_Ref( - int(star_splits[0]), - star_splits[1], - int(star_splits[2]), - ra, dec) + int(star_splits[0]), star_splits[1], int(star_splits[2]), ra, dec + ) count_global += 1 fptr.close() # create angles list @@ -53,40 +51,38 @@ def __init__(self, image_file, angle_size, fov, scope_size): else: # measure angular distance between starA and StarB angular_distance = calculate_angular_distance( - self.adj_star_list[sid_starA].get_ra(), - self.adj_star_list[sid_starA].get_dec(), - self.adj_star_list[sid_starB].get_ra(), - self.adj_star_list[sid_starB].get_dec()) + self.adj_star_list[sid_starA].get_ra(), + self.adj_star_list[sid_starA].get_dec(), + self.adj_star_list[sid_starB].get_ra(), + self.adj_star_list[sid_starB].get_dec(), + ) if angular_distance < fov: - self.adj_star_list[sid_starA].add(sid_starB, - angular_distance) + self.adj_star_list[sid_starA].add(sid_starB, angular_distance) elem = int(angular_distance * (1 / angle_size)) if elem == 0: - self.angles_list[elem].update([sid_starA, - sid_starB]) - self.angles_list[elem + 1].update([sid_starA, - sid_starB]) + self.angles_list[elem].update([sid_starA, sid_starB]) + self.angles_list[elem + 1].update([sid_starA, sid_starB]) else: - self.angles_list[elem - 1].update([sid_starA, - sid_starB]) - self.angles_list[elem].update([sid_starA, - sid_starB]) - self.angles_list[elem + 1].update([sid_starA, - sid_starB]) + self.angles_list[elem - 1].update([sid_starA, sid_starB]) + self.angles_list[elem].update([sid_starA, sid_starB]) + self.angles_list[elem + 1].update([sid_starA, sid_starB]) # Create scopeAdjStarList count_scope = 0 self.scope_adj_star_list = dict() for sid_star in self.adj_star_list: ra = self.adj_star_list[sid_star].get_ra() dec = self.adj_star_list[sid_star].get_dec() - angular_distance = calculate_angular_distance(ra, dec, - init_ra_position, - init_dec_position) + angular_distance = calculate_angular_distance( + ra, dec, init_ra_position, init_dec_position + ) if angular_distance < (hfov + scope_size): - self.scope_adj_star_list[sid_star] = Star_Ref(sid_star, - self.adj_star_list[sid_star].get_name(), - self.adj_star_list[sid_star].get_mag(), - ra, dec) + self.scope_adj_star_list[sid_star] = Star_Ref( + sid_star, + self.adj_star_list[sid_star].get_name(), + self.adj_star_list[sid_star].get_mag(), + ra, + dec, + ) count_scope += 1 # create scope angles list # complete scope adjacency list for each star @@ -100,26 +96,29 @@ def __init__(self, image_file, angle_size, fov, scope_size): else: # measure angular distance between starA and StarB angular_distance = calculate_angular_distance( - self.scope_adj_star_list[sid_starA].get_ra(), - self.scope_adj_star_list[sid_starA].get_dec(), - self.scope_adj_star_list[sid_starB].get_ra(), - self.scope_adj_star_list[sid_starB].get_dec()) + self.scope_adj_star_list[sid_starA].get_ra(), + self.scope_adj_star_list[sid_starA].get_dec(), + self.scope_adj_star_list[sid_starB].get_ra(), + self.scope_adj_star_list[sid_starB].get_dec(), + ) if angular_distance < fov: - self.scope_adj_star_list[sid_starA].add(sid_starB, - angular_distance) + self.scope_adj_star_list[sid_starA].add( + sid_starB, angular_distance + ) elem = int(angular_distance * (1 / angle_size)) if elem == 0: - self.scope_angles_list[elem].update([sid_starA, - sid_starB]) - self.scope_angles_list[elem + 1].update([sid_starA, - sid_starB]) + self.scope_angles_list[elem].update([sid_starA, sid_starB]) + self.scope_angles_list[elem + 1].update( + [sid_starA, sid_starB] + ) else: - self.scope_angles_list[elem - 1].update([sid_starA, - sid_starB]) - self.scope_angles_list[elem].update([sid_starA, - sid_starB]) - self.scope_angles_list[elem + 1].update([sid_starA, - sid_starB]) + self.scope_angles_list[elem - 1].update( + [sid_starA, sid_starB] + ) + self.scope_angles_list[elem].update([sid_starA, sid_starB]) + self.scope_angles_list[elem + 1].update( + [sid_starA, sid_starB] + ) self.angle_size = angle_size self.hfov = hfov self.fov = fov @@ -131,11 +130,10 @@ def __init__(self, image_file, angle_size, fov, scope_size): self.scope_num_stars = count_scope # end - def rescope(self, ra_position, dec_position): # in degrees - angular_distance = calculate_angular_distance(ra_position, - dec_position, - self.scope_position_ra, - self.scope_position_dec) + def rescope(self, ra_position, dec_position): # in degrees + angular_distance = calculate_angular_distance( + ra_position, dec_position, self.scope_position_ra, self.scope_position_dec + ) if angular_distance < self.scope_size: return False # Create scope_adj_star_list @@ -144,13 +142,17 @@ def rescope(self, ra_position, dec_position): # in degrees for sid_star in self.adj_star_list: ra = self.adj_star_list[sid_star].get_ra() dec = self.adj_star_list[sid_star].get_dec() - angular_distance = calculate_angular_distance(ra, dec, ra_position, - dec_position) + angular_distance = calculate_angular_distance( + ra, dec, ra_position, dec_position + ) if angular_distance < (self.hfov + self.scope_size): - self.scope_adj_star_list[sid_star] = Star_Ref(sid_star, - self.adj_star_list[sid_star].get_name(), - self.adj_star_list[sid_star].get_mag(), - ra, dec) + self.scope_adj_star_list[sid_star] = Star_Ref( + sid_star, + self.adj_star_list[sid_star].get_name(), + self.adj_star_list[sid_star].get_mag(), + ra, + dec, + ) count_scope += 1 # create scope angles list # complete scope adjacency list for each star @@ -164,42 +166,55 @@ def rescope(self, ra_position, dec_position): # in degrees else: # measure angular distance between starA and StarB angular_distance = calculate_angular_distance( - self.scope_adj_star_list[sid_starA].get_ra(), - self.scope_adj_star_list[sid_starA].get_dec(), - self.scope_adj_star_list[sid_starB].get_ra(), - self.scope_adj_star_list[sid_starB].get_dec()) + self.scope_adj_star_list[sid_starA].get_ra(), + self.scope_adj_star_list[sid_starA].get_dec(), + self.scope_adj_star_list[sid_starB].get_ra(), + self.scope_adj_star_list[sid_starB].get_dec(), + ) if angular_distance < self.fov: - self.scope_adj_star_list[sid_starA].add(sid_starB, - angular_distance) + self.scope_adj_star_list[sid_starA].add( + sid_starB, angular_distance + ) elem = int(angular_distance * (1 / self.angle_size)) if elem == 0: - self.scope_angles_list[elem].update([sid_starA, - sid_starB]) - self.scope_angles_list[elem + 1].update([sid_starA, - sid_starB]) + self.scope_angles_list[elem].update([sid_starA, sid_starB]) + self.scope_angles_list[elem + 1].update( + [sid_starA, sid_starB] + ) else: - self.scope_angles_list[elem - 1].update([sid_starA, - sid_starB]) - self.scope_angles_list[elem].update([sid_starA, - sid_starB]) - self.scope_angles_list[elem + 1].update([sid_starA, - sid_starB]) + self.scope_angles_list[elem - 1].update( + [sid_starA, sid_starB] + ) + self.scope_angles_list[elem].update([sid_starA, sid_starB]) + self.scope_angles_list[elem + 1].update( + [sid_starA, sid_starB] + ) self.scope_position_ra = ra_position self.scope_position_dec = dec_position self.scope_num_stars = count_scope return True - def match_global(self, stars): # 5 stars ordered brightest to least - return match(stars, self.angle_size, self.num_stars, - self.angle_size, self.angles_list, self.adj_star_list) + def match_global(self, stars): # 5 stars ordered brightest to least + return match( + stars, + self.angle_size, + self.num_stars, + self.angle_size, + self.angles_list, + self.adj_star_list, + ) # end - def match_scope(self, stars): # 5 star objects order brightest to less - list_of_possible_matches = match(stars, self.angle_size, - self.num_stars, self.angle_size, - self.angles_list, - self.scope_adj_star_list) - # if the scope fails possibly invalid, try global. + def match_scope(self, stars): # 5 star objects order brightest to less + list_of_possible_matches = match( + stars, + self.angle_size, + self.num_stars, + self.angle_size, + self.angles_list, + self.scope_adj_star_list, + ) + # if the scope fails possibly invalid, try global. if len(list_of_possible_matches) == 0: list_of_possible_matches = self.match_global(stars) return list_of_possible_matches @@ -221,26 +236,26 @@ def print_distribution(self): for sid_star in self.adj_star_list: ra = self.adj_star_list[sid_star].get_ra() dec = self.adj_star_list[sid_star].get_dec() - print 'For quad ra', ra, 'dec', dec + print "For quad ra", ra, "dec", dec # test function def print_angles(self): # How many stars have each angle for x in range(self.num_angles): size = x * self.angle_size - print('For angle ' , size, self.angles_list.get(x)) + print ("For angle ", size, self.angles_list.get(x)) # test function def print_scope_angles(self): # How many stars have each angle for x in range(self.num_angles): size = x * self.angle_size - print('For angle ' , size, self.scope_angles_list.get(x)) + print ("For angle ", size, self.scope_angles_list.get(x)) def match(stars, epsilon, num_stars, angle_size, angles_list, adj_star_list): # start - match_size = 5 # don't change + match_size = 5 # don't change star_array = dict() # create five arrays of length star_refs for each star for x in range(match_size): @@ -254,10 +269,11 @@ def match(stars, epsilon, num_stars, angle_size, angles_list, adj_star_list): # calculate distances for each pair if x < y: calculated = calculate_angular_distance( - stars[x].get_direction_angle(), - 90.0 - stars[x].get_distance_angle(), - stars[y].get_direction_angle(), - 90.0 - stars[y].get_distance_angle()) + stars[x].get_direction_angle(), + 90.0 - stars[x].get_distance_angle(), + stars[y].get_direction_angle(), + 90.0 - stars[y].get_distance_angle(), + ) angular_distances[(x, y)] = calculated # use angles list and calculated distances to fill the five arrays for x in range(match_size): @@ -273,7 +289,7 @@ def match(stars, epsilon, num_stars, angle_size, angles_list, adj_star_list): candidates[x] = [] count = 0 for starID in range(num_stars): - if starID in adj_star_list: #included + if starID in adj_star_list: # included if star_array[x][starID] == (match_size - 1): count += 1 candidates[x].append(starID) @@ -293,14 +309,17 @@ def match(stars, epsilon, num_stars, angle_size, angles_list, adj_star_list): candidate_temp_permutations.append((combo, x)) for combo in candidate_temp_permutations: distance01 = calculate_angular_distance( - adj_star_list[combo[0]].get_ra(), - adj_star_list[combo[0]].get_dec(), - adj_star_list[combo[1]].get_ra(), - adj_star_list[combo[1]].get_dec()) - if (distance01 < (angular_distances[(0,1)] + epsilon) and - distance01 > (angular_distances[(0,1)] - epsilon)): + adj_star_list[combo[0]].get_ra(), + adj_star_list[combo[0]].get_dec(), + adj_star_list[combo[1]].get_ra(), + adj_star_list[combo[1]].get_dec(), + ) + if distance01 < (angular_distances[(0, 1)] + epsilon) and distance01 > ( + angular_distances[(0, 1)] - epsilon + ): candidate_permutations.append(combo) - if not candidate_permutations: return candidate_permutations + if not candidate_permutations: + return candidate_permutations candidate_temp_temp_permutations = candidate_permutations candidate_temp_permutations = [] candidate_permutations = [] @@ -310,119 +329,153 @@ def match(stars, epsilon, num_stars, angle_size, angles_list, adj_star_list): candidate_temp_permutations.append((combo[0], combo[1], x)) for combo in candidate_temp_permutations: distance02 = calculate_angular_distance( - adj_star_list[combo[0]].get_ra(), - adj_star_list[combo[0]].get_dec(), - adj_star_list[combo[2]].get_ra(), - adj_star_list[combo[2]].get_dec()) + adj_star_list[combo[0]].get_ra(), + adj_star_list[combo[0]].get_dec(), + adj_star_list[combo[2]].get_ra(), + adj_star_list[combo[2]].get_dec(), + ) distance12 = calculate_angular_distance( - adj_star_list[combo[1]].get_ra(), - adj_star_list[combo[1]].get_dec(), - adj_star_list[combo[2]].get_ra(), - adj_star_list[combo[2]].get_dec()) - if (distance02 < (angular_distances[(0,2)] + epsilon) and - distance02 > (angular_distances[(0,2)] - epsilon)): - if (distance12 < (angular_distances[(1,2)] + epsilon) and - distance12 > (angular_distances[(1,2)] - epsilon)): + adj_star_list[combo[1]].get_ra(), + adj_star_list[combo[1]].get_dec(), + adj_star_list[combo[2]].get_ra(), + adj_star_list[combo[2]].get_dec(), + ) + if distance02 < (angular_distances[(0, 2)] + epsilon) and distance02 > ( + angular_distances[(0, 2)] - epsilon + ): + if distance12 < (angular_distances[(1, 2)] + epsilon) and distance12 > ( + angular_distances[(1, 2)] - epsilon + ): candidate_permutations.append(combo) - if not candidate_permutations: return candidate_permutations + if not candidate_permutations: + return candidate_permutations candidate_temp_temp_permutations = candidate_permutations candidate_temp_permutations = [] candidate_permutations = [] for combo in candidate_temp_temp_permutations: for x in candidates[3]: if combo[0] != x and combo[1] != x and combo[2] != x: - candidate_temp_permutations.append((combo[0], - combo[1], combo[2], x)) + candidate_temp_permutations.append((combo[0], combo[1], combo[2], x)) for combo in candidate_temp_permutations: distance03 = calculate_angular_distance( - adj_star_list[combo[0]].get_ra(), - adj_star_list[combo[0]].get_dec(), - adj_star_list[combo[3]].get_ra(), - adj_star_list[combo[3]].get_dec()) + adj_star_list[combo[0]].get_ra(), + adj_star_list[combo[0]].get_dec(), + adj_star_list[combo[3]].get_ra(), + adj_star_list[combo[3]].get_dec(), + ) distance13 = calculate_angular_distance( - adj_star_list[combo[1]].get_ra(), - adj_star_list[combo[1]].get_dec(), - adj_star_list[combo[3]].get_ra(), - adj_star_list[combo[3]].get_dec()) + adj_star_list[combo[1]].get_ra(), + adj_star_list[combo[1]].get_dec(), + adj_star_list[combo[3]].get_ra(), + adj_star_list[combo[3]].get_dec(), + ) distance23 = calculate_angular_distance( - adj_star_list[combo[2]].get_ra(), - adj_star_list[combo[2]].get_dec(), - adj_star_list[combo[3]].get_ra(), - adj_star_list[combo[3]].get_dec()) - if (distance03 < (angular_distances[(0,3)] + epsilon) and - distance03 > (angular_distances[(0,3)] - epsilon)): - if (distance13 < (angular_distances[(1,3)] + epsilon) and - distance13 > (angular_distances[(1,3)] - epsilon)): - if (distance23 < (angular_distances[(2,3)] + epsilon) and - distance23 > (angular_distances[(2,3)] - epsilon)): + adj_star_list[combo[2]].get_ra(), + adj_star_list[combo[2]].get_dec(), + adj_star_list[combo[3]].get_ra(), + adj_star_list[combo[3]].get_dec(), + ) + if distance03 < (angular_distances[(0, 3)] + epsilon) and distance03 > ( + angular_distances[(0, 3)] - epsilon + ): + if distance13 < (angular_distances[(1, 3)] + epsilon) and distance13 > ( + angular_distances[(1, 3)] - epsilon + ): + if distance23 < (angular_distances[(2, 3)] + epsilon) and distance23 > ( + angular_distances[(2, 3)] - epsilon + ): candidate_permutations.append(combo) - if not candidate_permutations: return candidate_permutations + if not candidate_permutations: + return candidate_permutations candidate_temp_temp_permutations = candidate_permutations candidate_temp_permutations = [] candidate_permutations = [] for combo in candidate_temp_temp_permutations: for x in candidates[4]: - if (combo[0] != x and combo[1] != x and combo[2] != x and - combo[3] != x): - candidate_temp_permutations.append((combo[0], combo[1], - combo[2], combo[3], x)) + if combo[0] != x and combo[1] != x and combo[2] != x and combo[3] != x: + candidate_temp_permutations.append( + (combo[0], combo[1], combo[2], combo[3], x) + ) for combo in candidate_temp_permutations: distance04 = calculate_angular_distance( - adj_star_list[combo[0]].get_ra(), - adj_star_list[combo[0]].get_dec(), - adj_star_list[combo[4]].get_ra(), - adj_star_list[combo[4]].get_dec()) + adj_star_list[combo[0]].get_ra(), + adj_star_list[combo[0]].get_dec(), + adj_star_list[combo[4]].get_ra(), + adj_star_list[combo[4]].get_dec(), + ) distance14 = calculate_angular_distance( - adj_star_list[combo[1]].get_ra(), - adj_star_list[combo[1]].get_dec(), - adj_star_list[combo[4]].get_ra(), - adj_star_list[combo[4]].get_dec()) + adj_star_list[combo[1]].get_ra(), + adj_star_list[combo[1]].get_dec(), + adj_star_list[combo[4]].get_ra(), + adj_star_list[combo[4]].get_dec(), + ) distance24 = calculate_angular_distance( - adj_star_list[combo[2]].get_ra(), - adj_star_list[combo[2]].get_dec(), - adj_star_list[combo[4]].get_ra(), - adj_star_list[combo[4]].get_dec()) + adj_star_list[combo[2]].get_ra(), + adj_star_list[combo[2]].get_dec(), + adj_star_list[combo[4]].get_ra(), + adj_star_list[combo[4]].get_dec(), + ) distance34 = calculate_angular_distance( - adj_star_list[combo[3]].get_ra(), - adj_star_list[combo[3]].get_dec(), - adj_star_list[combo[4]].get_ra(), - adj_star_list[combo[4]].get_dec()) - if (distance04 < (angular_distances[(0,4)] + epsilon) and - distance04 > (angular_distances[(0,4)] - epsilon)): - if (distance14 < (angular_distances[(1,4)] + epsilon) and - distance14 > (angular_distances[(1,4)] - epsilon)): - if (distance24 < (angular_distances[(2,4)] + epsilon) and - distance24 > (angular_distances[(2,4)] - epsilon)): - if (distance34 < (angular_distances[(3,4)] + epsilon) and - distance34 > (angular_distances[(3,4)] - epsilon)): + adj_star_list[combo[3]].get_ra(), + adj_star_list[combo[3]].get_dec(), + adj_star_list[combo[4]].get_ra(), + adj_star_list[combo[4]].get_dec(), + ) + if distance04 < (angular_distances[(0, 4)] + epsilon) and distance04 > ( + angular_distances[(0, 4)] - epsilon + ): + if distance14 < (angular_distances[(1, 4)] + epsilon) and distance14 > ( + angular_distances[(1, 4)] - epsilon + ): + if distance24 < (angular_distances[(2, 4)] + epsilon) and distance24 > ( + angular_distances[(2, 4)] - epsilon + ): + if distance34 < ( + angular_distances[(3, 4)] + epsilon + ) and distance34 > (angular_distances[(3, 4)] - epsilon): candidate_permutations.append(combo) - if not candidate_permutations: return candidate_permutations + if not candidate_permutations: + return candidate_permutations candidate_temp_permutations = candidate_permutations candidate_permutations = [] for combo in candidate_temp_permutations: - candidate_permutations.append(( (combo[0], - adj_star_list[combo[0]].get_name(), - adj_star_list[combo[0]].get_ra(), - adj_star_list[combo[0]].get_dec(), - stars[0].get_distance_angle()), - (combo[1], - adj_star_list[combo[1]].get_name(), - adj_star_list[combo[1]].get_ra(), - adj_star_list[combo[1]].get_dec(), - stars[1].get_distance_angle()), - (combo[2], - adj_star_list[combo[2]].get_name(), - adj_star_list[combo[2]].get_ra(), - adj_star_list[combo[2]].get_dec(), - stars[2].get_distance_angle()), - (combo[3], - adj_star_list[combo[3]].get_name(), - adj_star_list[combo[3]].get_ra(), - adj_star_list[combo[3]].get_dec(), - stars[3].get_distance_angle()), - (combo[4], - adj_star_list[combo[4]].get_name(), - adj_star_list[combo[4]].get_ra(), - adj_star_list[combo[4]].get_dec(), - stars[4].get_distance_angle()) )) + candidate_permutations.append( + ( + ( + combo[0], + adj_star_list[combo[0]].get_name(), + adj_star_list[combo[0]].get_ra(), + adj_star_list[combo[0]].get_dec(), + stars[0].get_distance_angle(), + ), + ( + combo[1], + adj_star_list[combo[1]].get_name(), + adj_star_list[combo[1]].get_ra(), + adj_star_list[combo[1]].get_dec(), + stars[1].get_distance_angle(), + ), + ( + combo[2], + adj_star_list[combo[2]].get_name(), + adj_star_list[combo[2]].get_ra(), + adj_star_list[combo[2]].get_dec(), + stars[2].get_distance_angle(), + ), + ( + combo[3], + adj_star_list[combo[3]].get_name(), + adj_star_list[combo[3]].get_ra(), + adj_star_list[combo[3]].get_dec(), + stars[3].get_distance_angle(), + ), + ( + combo[4], + adj_star_list[combo[4]].get_name(), + adj_star_list[combo[4]].get_ra(), + adj_star_list[combo[4]].get_dec(), + stars[4].get_distance_angle(), + ), + ) + ) return candidate_permutations # 5 star tuples or null order as before diff --git a/experimental/cosmic-gps/star_cat_tests.py b/experimental/cosmic-gps/star_cat_tests.py index e076b12f..ae1e6c74 100644 --- a/experimental/cosmic-gps/star_cat_tests.py +++ b/experimental/cosmic-gps/star_cat_tests.py @@ -4,17 +4,15 @@ from star_cat import Star_Cat from star import Star + def main(): - angle_size = 0.5 # keep angle size (0.5, 0.25 0.125, 0.0625) deg - fov = 90 # max fov 180 deg + angle_size = 0.5 # keep angle size (0.5, 0.25 0.125, 0.0625) deg + fov = 90 # max fov 180 deg scope_size = 30 # initialize the star reference catalogue - star_catalogue = Star_Cat( 'star_catalogue_init.txt', - angle_size, - fov, - scope_size ) + star_catalogue = Star_Cat("star_catalogue_init.txt", angle_size, fov, scope_size) list_of_stars = [] @@ -49,22 +47,22 @@ def main(): list_of_stars.append([6, star]) """ Star Matching """ - + count = 0 list_of_stars_size = len(list_of_stars) list_of_possible_matches = [] # loop five times enforced by count - while (count < 5): + while count < 5: # break early if too few stars if list_of_stars_size < 5: - break; + break - star1 = list_of_stars[count+0][1] - star2 = list_of_stars[count+1][1] - star3 = list_of_stars[count+2][1] - star4 = list_of_stars[count+3][1] - star5 = list_of_stars[count+4][1] + star1 = list_of_stars[count + 0][1] + star2 = list_of_stars[count + 1][1] + star3 = list_of_stars[count + 2][1] + star4 = list_of_stars[count + 3][1] + star5 = list_of_stars[count + 4][1] stars = (star1, star2, star3, star4, star5) # Try matching to the scope for the first try @@ -73,16 +71,16 @@ def main(): else: list_of_possible_matches = star_catalogue.match_global(stars) - break #remove only test + break # remove only test # if no candidates are returned # remove the count+0 star with the next brightest count += 1 list_of_stars_size -= 1 if len(list_of_possible_matches) > 0: - break # can be possibly many group of candidates. + break # can be possibly many group of candidates. print list_of_possible_matches - + if __name__ == "__main__": main() diff --git a/experimental/cosmic-gps/star_ref.py b/experimental/cosmic-gps/star_ref.py index 7551a40a..2e483623 100644 --- a/experimental/cosmic-gps/star_ref.py +++ b/experimental/cosmic-gps/star_ref.py @@ -3,8 +3,8 @@ import numpy as np -class Star_Ref: +class Star_Ref: def __init__(self, sid, name, mag, ra, dec): self.sid = sid self.star_name = name @@ -35,9 +35,11 @@ def get_adj_list(self): return self.adj_list def print_star(self): - print (self.sid, - self.star_name, - self.star_mag, - self.star_ra, - self.star_dec, - self.adj_list) + print( + self.sid, + self.star_name, + self.star_mag, + self.star_ra, + self.star_dec, + self.adj_list, + ) diff --git a/experimental/cosmic-gps/star_tests.py b/experimental/cosmic-gps/star_tests.py index 31541a0b..0fb21fe2 100644 --- a/experimental/cosmic-gps/star_tests.py +++ b/experimental/cosmic-gps/star_tests.py @@ -15,13 +15,14 @@ from star import Star from measurement import Calibration_Function + def main(): image_center_x = 1511.5 image_center_y = 2015.5 # Gemini - img = cv.imread('d_03_12_20_t_22_16_15.jpg', 1) + img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) img = cv.cvtColor(img, cv.COLOR_RGB2GRAY) img = cv.bilateralFilter(img, 5, 150, 150) pixels = thresh_global(img, 50) @@ -29,7 +30,7 @@ def main(): list_of_clusters = cluster(pixels) list_of_stars = [] - getkey = lambda item : item[0] + getkey = lambda item: item[0] for clustr in list_of_clusters: if clustr.shape() > 0.50: # should be 0.60 cluster_intensity = clustr.get_intensity() @@ -37,25 +38,27 @@ def main(): clustr.set_angles(image_center_x, image_center_y) direction = clustr.get_direction_angle() distance = clustr.get_distance_angle() - list_of_stars.append([cluster_intensity, cluster_center, direction, distance, clustr]) + list_of_stars.append( + [cluster_intensity, cluster_center, direction, distance, clustr] + ) list_of_stars = sorted(list_of_stars, key=getkey, reverse=True) - print '***Stars***' - print '' + print "***Stars***" + print "" for star in list_of_stars: - print '*****' - print 'Center:' + print "*****" + print "Center:" print star[1] - print '' - print 'Direction Angle:' + print "" + print "Direction Angle:" print star[2] - print '' - print 'Distance Angle:' + print "" + print "Distance Angle:" print star[3] - print '' - print 'Cluster:' + print "" + print "Cluster:" star[4].show() - print '_____' + print "_____" if __name__ == "__main__": diff --git a/experimental/cosmic-gps/thresh_tests.py b/experimental/cosmic-gps/thresh_tests.py index 581b42a4..589aec4f 100644 --- a/experimental/cosmic-gps/thresh_tests.py +++ b/experimental/cosmic-gps/thresh_tests.py @@ -19,7 +19,7 @@ def main(): window_size = 9 thresh_value = 50 - img = cv.imread('d_03_12_20_t_22_16_15.jpg', 1) + img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) img = cv.cvtColor(img, cv.COLOR_RGB2GRAY) img = cv.bilateralFilter(img, 5, 150, 150) @@ -31,11 +31,13 @@ def main(): thresh_array = convert_dict_to_array(thresh_dict, rows, cols) print thresh_dict + def convert_dict_to_array(thresh_dict, rows, cols): temp_array = np.zeros((rows, cols)) for pixel in thresh_dict: temp_array.itemset((pixel[0], pixel[1]), thresh_dict[pixel]) return temp_array + if __name__ == "__main__": main() diff --git a/packages/autonomy/ezrassor_autonomous_control/setup.py b/packages/autonomy/ezrassor_autonomous_control/setup.py index 2ebf5d20..9de736eb 100644 --- a/packages/autonomy/ezrassor_autonomous_control/setup.py +++ b/packages/autonomy/ezrassor_autonomous_control/setup.py @@ -6,7 +6,6 @@ import catkin_pkg.python_setup setup_arguments = catkin_pkg.python_setup.generate_distutils_setup( - packages=("ezrassor_autonomous_control", ), - package_dir={"" : "source"}, + packages=("ezrassor_autonomous_control",), package_dir={"": "source"}, ) distutils.core.setup(**setup_arguments) diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py index 5fe6ae2f..6d0448f6 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py @@ -10,7 +10,8 @@ from sensor_msgs.msg import JointState from geometry_msgs.msg import Point, Twist -class WorldState(): + +class WorldState: """ World State Object Representing All Sensor Data """ @@ -34,7 +35,6 @@ def __init__(self): self.battery = 100 self.hardware_status = True - def jointCallBack(self, data): """ Set state_flags joint position data. """ @@ -47,7 +47,7 @@ def odometryCallBack(self, data): self.positionX = data.pose.pose.position.x + self.startPositionX self.positionY = data.pose.pose.position.y + self.startPositionY - heading = nf.quaternion_to_yaw(data.pose.pose) * 180/math.pi + heading = nf.quaternion_to_yaw(data.pose.pose) * 180 / math.pi if heading > 0: self.heading = heading @@ -71,7 +71,7 @@ def simStateCallBack(self, data): self.positionX = data.pose[index].position.x self.positionY = data.pose[index].position.y - heading = nf.quaternion_to_yaw(data.pose[index]) * 180/math.pi + heading = nf.quaternion_to_yaw(data.pose[index]) * 180 / math.pi if heading > 0: self.heading = heading @@ -87,8 +87,8 @@ def imuCallBack(self, data): self.on_side = False def get_arm_force(self): - front_arm_force = self.state_flags['front_arm_angle'] + .2 + uniform(-.2, .2) - back_arm_force = self.state_flags['back_arm_angle'] + .2 + uniform(-.2, .2) + front_arm_force = self.state_flags["front_arm_angle"] + 0.2 + uniform(-0.2, 0.2) + back_arm_force = self.state_flags["back_arm_angle"] + 0.2 + uniform(-0.2, 0.2) return front_arm_force, back_arm_force # Use initial spawn coordinates to later offset position @@ -96,64 +96,61 @@ def initial_spawn(self, start_x, start_y): self.startPositionX = start_x self.startPositionY = start_y -class ROSUtility(): + +class ROSUtility: """ ROS Utility class that provides publishers, subscribers, and convenient ROS utilies. """ - def __init__(self, movement_topic, front_arm_topic, back_arm_topic, - front_drum_topic, back_drum_topic, - max_linear_velocity, max_angular_velocity, obstacle_threshold, - obstacle_buffer, move_increment): + def __init__( + self, + movement_topic, + front_arm_topic, + back_arm_topic, + front_drum_topic, + back_drum_topic, + max_linear_velocity, + max_angular_velocity, + obstacle_threshold, + obstacle_buffer, + move_increment, + ): """ Initialize the ROS Utility Object. """ - self.movement_pub = rospy.Publisher(movement_topic, - Twist, - queue_size=10) - self.front_arm_pub = rospy.Publisher(front_arm_topic, - Float32, - queue_size=10) - self.back_arm_pub = rospy.Publisher(back_arm_topic, - Float32, - queue_size=10) - self.front_drum_pub = rospy.Publisher(front_drum_topic, - Float32, - queue_size=10) - self.back_drum_pub = rospy.Publisher(back_drum_topic, - Float32, - queue_size=10) - self.control_pub = rospy.Publisher('secondary_override_toggle', - Bool, - queue_size=10) - self.arms_up_pub = rospy.Publisher('arms_up', - Bool, - queue_size=10) - self.rate = rospy.Rate(45) # 10hz + self.movement_pub = rospy.Publisher(movement_topic, Twist, queue_size=10) + self.front_arm_pub = rospy.Publisher(front_arm_topic, Float32, queue_size=10) + self.back_arm_pub = rospy.Publisher(back_arm_topic, Float32, queue_size=10) + self.front_drum_pub = rospy.Publisher(front_drum_topic, Float32, queue_size=10) + self.back_drum_pub = rospy.Publisher(back_drum_topic, Float32, queue_size=10) + self.control_pub = rospy.Publisher( + "secondary_override_toggle", Bool, queue_size=10 + ) + self.arms_up_pub = rospy.Publisher("arms_up", Bool, queue_size=10) + self.rate = rospy.Rate(45) # 10hz self.max_linear_velocity = max_linear_velocity self.max_angular_velocity = max_angular_velocity self.auto_function_command = 0 - self.threshold = .5 + self.threshold = 0.5 self.obstacle_threshold = obstacle_threshold self.obstacle_buffer = obstacle_buffer self.move_increment = move_increment - def publish_actions(self, movement, front_arm, back_arm, - front_drum, back_drum): + def publish_actions(self, movement, front_arm, back_arm, front_drum, back_drum): """ Publishes actions for all joints and motors """ twist_message = Twist() - if movement == 'forward': + if movement == "forward": twist_message.linear.x = self.max_linear_velocity - elif movement == 'reverse': + elif movement == "reverse": twist_message.linear.x = -self.max_linear_velocity - elif movement == 'left': + elif movement == "left": twist_message.angular.z = self.max_angular_velocity - elif movement == 'right': + elif movement == "right": twist_message.angular.z = -self.max_angular_velocity else: pass diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py index 76363dd8..ba637c7b 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python import rospy from ai_objects import WorldState, ROSUtility @@ -8,34 +8,55 @@ EPSILON = 0.05 -''' Checks if current arms force is near target ''' +""" Checks if current arms force is near target """ + + def is_force_target(world_state, target_force): - front_force = target_force - EPSILON < world_state.state_flags['force_front_arm'] < target_force + EPSILON - back_force = target_force - EPSILON < world_state.state_flags['force_back_arm'] < target_force + EPSILON + front_force = ( + target_force - EPSILON + < world_state.state_flags["force_front_arm"] + < target_force + EPSILON + ) + back_force = ( + target_force - EPSILON + < world_state.state_flags["force_back_arm"] + < target_force + EPSILON + ) return front_force and back_force -''' Given a target force sets both arms to the target ''' -def set_target_force(world_state,ros_util,target_force, t): - while not is_force_target(world_state,target_force): +""" Given a target force sets both arms to the target """ + + +def set_target_force(world_state, ros_util, target_force, t): + + while not is_force_target(world_state, target_force): msg = 0b0 - if not target_force - EPSILON < world_state.state_flags['force_front_arm'] < target_force + EPSILON: - if target_force > world_state.state_flags['force_front_arm']: - msg += ros_util.commands['front_arm_up'] - elif target_force < world_state.state_flags['force_front_arm']: - msg += ros_util.commands['front_arm_down'] - - if not target_force - EPSILON < world_state.state_flags['force_back_arm'] < target_force + EPSILON: - if target_force > world_state.state_flags['force_back_arm']: - msg += ros_util.commands['back_arm_up'] - elif target_force < world_state.state_flags['force_back_arm']: - msg += ros_util.commands['back_arm_down'] - msg += ros_util.commands['front_dig'] - msg += ros_util.commands['back_dig'] - msg += ros_util.commands['forward'] + if ( + not target_force - EPSILON + < world_state.state_flags["force_front_arm"] + < target_force + EPSILON + ): + if target_force > world_state.state_flags["force_front_arm"]: + msg += ros_util.commands["front_arm_up"] + elif target_force < world_state.state_flags["force_front_arm"]: + msg += ros_util.commands["front_arm_down"] + + if ( + not target_force - EPSILON + < world_state.state_flags["force_back_arm"] + < target_force + EPSILON + ): + if target_force > world_state.state_flags["force_back_arm"]: + msg += ros_util.commands["back_arm_up"] + elif target_force < world_state.state_flags["force_back_arm"]: + msg += ros_util.commands["back_arm_down"] + msg += ros_util.commands["front_dig"] + msg += ros_util.commands["back_dig"] + msg += ros_util.commands["forward"] ros_util.command_pub.publish(msg) ros_util.rate.sleep() t += 1 - ros_util.command_pub.publish(ros_util.commands['null']) - return t \ No newline at end of file + ros_util.command_pub.publish(ros_util.commands["null"]) + return t diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py index abaa8344..22699eff 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py @@ -9,8 +9,9 @@ def at_target(positionX, positionY, targetX, targetY, threshold): """ Determine if the current position is within the desired threshold of the target position. """ - value = ((targetX - threshold) < positionX < (targetX + threshold) - and (targetY - threshold) < positionY < (targetY + threshold)) + value = (targetX - threshold) < positionX < (targetX + threshold) and ( + targetY - threshold + ) < positionY < (targetY + threshold) return value @@ -18,7 +19,7 @@ def at_target(positionX, positionY, targetX, targetY, threshold): def charge_battery(world_state, ros_util): """ Charge the rover's battery for a designated duration until battery is 100% """ - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) while world_state.battery < 100: rospy.sleep(0.1) @@ -32,9 +33,11 @@ def auto_drive_location(world_state, ros_util, waypoint_server=None): # Action server will print it's own info if waypoint_server is None: - rospy.loginfo('Auto-driving to [%s, %s]...', - str(world_state.target_location.x), - str(world_state.target_location.y)) + rospy.loginfo( + "Auto-driving to [%s, %s]...", + str(world_state.target_location.x), + str(world_state.target_location.y), + ) # Send feedback to waypoint client if being controlled by swarm controller preempted = False @@ -52,29 +55,32 @@ def auto_drive_location(world_state, ros_util, waypoint_server=None): if waypoint_server is not None: waypoint_server.set_preempted() - rospy.logdebug('Status check failed.') + rospy.logdebug("Status check failed.") return feedback, preempted # Before we head towards our goal, turn to face it. # Get new heading angle relative to current heading new_heading_degrees = nf.calculate_heading(world_state) - angle2goal_radians = nf.adjust_angle(world_state.heading, - new_heading_degrees) + angle2goal_radians = nf.adjust_angle(world_state.heading, new_heading_degrees) # If our angle is less than zero, then we would expect a right turn # otherwise turn left. if angle2goal_radians < 0: - direction = 'right' + direction = "right" else: - direction = 'left' + direction = "left" uf.turn(new_heading_degrees, direction, world_state, ros_util) - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) # Main loop until location is reached - while not at_target(world_state.positionX, world_state.positionY, - world_state.target_location.x, - world_state.target_location.y, ros_util.threshold): + while not at_target( + world_state.positionX, + world_state.positionY, + world_state.target_location.x, + world_state.target_location.y, + ros_util.threshold, + ): # Check that the waypoint client request hasnt been canceled if waypoint_server is not None and waypoint_server.is_preempt_requested(): @@ -92,18 +98,19 @@ def auto_drive_location(world_state, ros_util, waypoint_server=None): if waypoint_server is not None: waypoint_server.set_preempted() - rospy.logdebug('Status check failed.') + rospy.logdebug("Status check failed.") break angle = uf.get_turn_angle(world_state, ros_util) # If our angle is less than zero, then we would expect a right turn # otherwise turn left. - direction = 'right' if angle < 0 else 'left' + direction = "right" if angle < 0 else "left" # Turn towards the direction chosen. - uf.turn(nf.rel_to_abs(world_state.heading, angle), direction, - world_state, ros_util) + uf.turn( + nf.rel_to_abs(world_state.heading, angle), direction, world_state, ros_util + ) # Move towards the direction chosen. uf.move(ros_util.move_increment, world_state, ros_util) @@ -115,9 +122,9 @@ def auto_drive_location(world_state, ros_util, waypoint_server=None): # Action server will print its own info if waypoint_server is None: - rospy.loginfo('Destination reached!') + rospy.loginfo("Destination reached!") - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) return feedback, preempted @@ -139,18 +146,18 @@ def auto_dig(world_state, ros_util, duration, waypoint_server=None): return feedback, preempted if waypoint_server is None: - rospy.loginfo('Auto-digging for %d seconds...', duration) + rospy.loginfo("Auto-digging for %d seconds...", duration) uf.set_front_arm_angle(world_state, ros_util, -0.1) uf.set_back_arm_angle(world_state, ros_util, -0.1) # Dig for the desired duration t = 0 - direction = 'forward' + direction = "forward" while t < duration * 40: # Swap between digging forward or backward every few seconds if t % 100 == 0: - direction = 'reverse' if direction == 'forward' else 'forward' + direction = "reverse" if direction == "forward" else "forward" ros_util.publish_actions(direction, 0, 0, 1, 1) t += 1 @@ -172,13 +179,14 @@ def auto_dig(world_state, ros_util, duration, waypoint_server=None): preempted = True break - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) return feedback, preempted + def auto_dock(world_state, ros_util): """ Dock with the hopper. """ - rospy.loginfo('Auto-returning to origin...') + rospy.loginfo("Auto-returning to origin...") old_target_x = world_state.target_location.x old_target_y = world_state.target_location.y @@ -189,7 +197,7 @@ def auto_dock(world_state, ros_util): world_state.target_location.y = 0 auto_drive_location(world_state, ros_util) - ros_util.threshold = .5 + ros_util.threshold = 0.5 world_state.target_location.x = old_target_x world_state.target_location.y = old_target_y @@ -199,7 +207,7 @@ def auto_dump(world_state, ros_util, duration): """ Rotate both drums inward and drive forward for duration time in seconds. """ - rospy.loginfo('Auto-dumping drum contents...') + rospy.loginfo("Auto-dumping drum contents...") uf.set_front_arm_angle(world_state, ros_util, 1.3) uf.set_back_arm_angle(world_state, ros_util, 1.3) @@ -208,19 +216,19 @@ def auto_dump(world_state, ros_util, duration): while t < duration * 40: if uf.self_check(world_state, ros_util) != 1: return - ros_util.publish_actions('stop', 0, 0, -1, -1) + ros_util.publish_actions("stop", 0, 0, -1, -1) t += 1 ros_util.rate.sleep() new_heading = world_state.heading = (world_state.heading + 180) % 360 while not ((new_heading - 1) < world_state.heading < (new_heading + 1)): - ros_util.publish_actions('left', 0, 0, 0, 0) + ros_util.publish_actions("left", 0, 0, 0, 0) ros_util.rate.sleep() while t < duration * 30: - ros_util.publish_actions('stop', 0, 0, -1, -1) + ros_util.publish_actions("stop", 0, 0, -1, -1) t += 1 ros_util.rate.sleep() - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py index c266c221..63ff4a43 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py @@ -17,70 +17,86 @@ class RoverController: - def __init__(self, target_x, target_y, start_x, start_y, movement_topic, front_arm_topic, - back_arm_topic, front_drum_topic, back_drum_topic, - obstacle_threshold, obstacle_buffer, move_increment, - max_linear_velocity, max_angular_velocity, real_odometry, swarm_control): + def __init__( + self, + target_x, + target_y, + start_x, + start_y, + movement_topic, + front_arm_topic, + back_arm_topic, + front_drum_topic, + back_drum_topic, + obstacle_threshold, + obstacle_buffer, + move_increment, + max_linear_velocity, + max_angular_velocity, + real_odometry, + swarm_control, + ): self.namespace = rospy.get_namespace() # Create Utility Objects self.world_state = obj.WorldState() - self.ros_util = obj.ROSUtility(movement_topic, - front_arm_topic, - back_arm_topic, - front_drum_topic, - back_drum_topic, - max_linear_velocity, - max_angular_velocity, - obstacle_threshold, - obstacle_buffer, - move_increment) + self.ros_util = obj.ROSUtility( + movement_topic, + front_arm_topic, + back_arm_topic, + front_drum_topic, + back_drum_topic, + max_linear_velocity, + max_angular_velocity, + obstacle_threshold, + obstacle_buffer, + move_increment, + ) # Setup Subscriber Callbacks if real_odometry: # Get initial spawn coords self.world_state.initial_spawn(start_x, start_y) - rospy.Subscriber('odometry/filtered', - Odometry, - self.world_state.odometryCallBack) + rospy.Subscriber( + "odometry/filtered", Odometry, self.world_state.odometryCallBack + ) else: - rospy.Subscriber('/gazebo/link_states', - LinkStates, - self.world_state.simStateCallBack) - - rospy.Subscriber('imu', - Imu, - self.world_state.imuCallBack) - rospy.Subscriber('joint_states', - JointState, - self.world_state.jointCallBack) - rospy.Subscriber('autonomous_toggles', - Int8, - self.ros_util.autoCommandCallBack) - rospy.Subscriber('obstacle_detection/combined', - LaserScan, - uf.on_scan_update, - queue_size=1) + rospy.Subscriber( + "/gazebo/link_states", LinkStates, self.world_state.simStateCallBack + ) + + rospy.Subscriber("imu", Imu, self.world_state.imuCallBack) + rospy.Subscriber("joint_states", JointState, self.world_state.jointCallBack) + rospy.Subscriber("autonomous_toggles", Int8, self.ros_util.autoCommandCallBack) + rospy.Subscriber( + "obstacle_detection/combined", LaserScan, uf.on_scan_update, queue_size=1 + ) if swarm_control: self.ros_util.auto_function_command = 16 # Create waypoint action server used to control the rover via the swarm_controller - self.server_name = 'waypoint' - self.waypoint_server = actionlib.SimpleActionServer(self.server_name, waypointAction, - execute_cb=self.execute_action, auto_start=False) + self.server_name = "waypoint" + self.waypoint_server = actionlib.SimpleActionServer( + self.server_name, + waypointAction, + execute_cb=self.execute_action, + auto_start=False, + ) self.waypoint_server.start() self.waypoint_server.register_preempt_callback(self.preempt_cb) - rospy.loginfo('Rover waypoint server initialized.') + rospy.loginfo("Rover waypoint server initialized.") # Register GetRoverStatus, used by the swarm controller to retreive a rover's position and battery - self.status_service = rospy.Service('rover_status', GetRoverStatus, self.send_status) - rospy.loginfo('Rover status service initialized.') + self.status_service = rospy.Service( + "rover_status", GetRoverStatus, self.send_status + ) + rospy.loginfo("Rover status service initialized.") else: # Basic autonomous control using the autonomous control loop @@ -118,7 +134,11 @@ def execute_action(self, goal): """ if goal.target.x == -999 and goal.target.y == -999: - rospy.loginfo('Waypoint server {} executing charge command'.format(self.namespace + self.server_name)) + rospy.loginfo( + "Waypoint server {} executing charge command".format( + self.namespace + self.server_name + ) + ) # Set rover to charge af.charge_battery(self.world_state, self.ros_util) @@ -126,25 +146,44 @@ def execute_action(self, goal): self.waypoint_server.set_succeeded(result) elif goal.target.x == -998 and goal.target.y == -998: - rospy.loginfo('Waypoint server {} executing dig command'.format(self.namespace + self.server_name)) + rospy.loginfo( + "Waypoint server {} executing dig command".format( + self.namespace + self.server_name + ) + ) # Set rover to dig for 1000 seconds - feedback, preempted = af.auto_dig(self.world_state, self.ros_util, 1000, self.waypoint_server) - - if feedback is not None and not preempted and not self.waypoint_server.is_preempt_requested(): + feedback, preempted = af.auto_dig( + self.world_state, self.ros_util, 1000, self.waypoint_server + ) + + if ( + feedback is not None + and not preempted + and not self.waypoint_server.is_preempt_requested() + ): result = uf.build_result(self.world_state, preempted=0) self.waypoint_server.set_succeeded(result) else: self.world_state.target_location = goal.target - rospy.loginfo('Waypoint server {} moving rover to {}'.format( - self.namespace + self.server_name, (goal.target.x, goal.target.y))) + rospy.loginfo( + "Waypoint server {} moving rover to {}".format( + self.namespace + self.server_name, (goal.target.x, goal.target.y) + ) + ) # Set rover to autonomously navigate to target - feedback, preempted = af.auto_drive_location(self.world_state, self.ros_util, self.waypoint_server) + feedback, preempted = af.auto_drive_location( + self.world_state, self.ros_util, self.waypoint_server + ) # Send resulting state to client and set server to succeeded, as long as request wasn't preempted - if feedback is not None and not preempted and not self.waypoint_server.is_preempt_requested(): + if ( + feedback is not None + and not preempted + and not self.waypoint_server.is_preempt_requested() + ): result = waypointResult(feedback.pose, feedback.battery, 0) self.waypoint_server.set_succeeded(result) @@ -154,7 +193,7 @@ def preempt_cb(self): """ if self.waypoint_server.is_preempt_requested(): # Stop the rover - self.ros_util.publish_actions('stop', 0, 0, 0, 0) + self.ros_util.publish_actions("stop", 0, 0, 0, 0) # Send rover status while preempting the waypoint goal result = uf.build_result(self.world_state, preempted=1) @@ -163,9 +202,9 @@ def preempt_cb(self): def full_autonomy(self, world_state, ros_util): """ Full Autonomy Loop Function """ - rospy.loginfo('Full autonomy activated.') + rospy.loginfo("Full autonomy activated.") - while (ros_util.auto_function_command == 16): + while ros_util.auto_function_command == 16: af.auto_drive_location(world_state, ros_util) if ros_util.auto_function_command != 16: break @@ -185,9 +224,12 @@ def full_autonomy(self, world_state, ros_util): def autonomous_control_loop(self, world_state, ros_util): """ Control Auto Functions based on auto_function_command input. """ - while (True): - while ros_util.auto_function_command == 0 or ros_util.auto_function_command == 32: - ros_util.publish_actions('stop', 0, 0, 0, 0) + while True: + while ( + ros_util.auto_function_command == 0 + or ros_util.auto_function_command == 32 + ): + ros_util.publish_actions("stop", 0, 0, 0, 0) ros_util.rate.sleep() ros_util.control_pub.publish(True) @@ -203,31 +245,62 @@ def autonomous_control_loop(self, world_state, ros_util): elif ros_util.auto_function_command == 16: self.full_autonomy(world_state, ros_util) else: - rospy.loginfo('Invalid auto-function request: %s.', - str(ros_util.auto_function_command)) + rospy.loginfo( + "Invalid auto-function request: %s.", + str(ros_util.auto_function_command), + ) ros_util.auto_function_command = 0 - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) ros_util.control_pub.publish(False) -def on_start_up(target_x, target_y, start_x, start_y, movement_topic, front_arm_topic, - back_arm_topic, front_drum_topic, back_drum_topic, - obstacle_threshold, obstacle_buffer, move_increment, - max_linear_velocity=1, max_angular_velocity=1, real_odometry=False, swarm_control=False): +def on_start_up( + target_x, + target_y, + start_x, + start_y, + movement_topic, + front_arm_topic, + back_arm_topic, + front_drum_topic, + back_drum_topic, + obstacle_threshold, + obstacle_buffer, + move_increment, + max_linear_velocity=1, + max_angular_velocity=1, + real_odometry=False, + swarm_control=False, +): """ Initialization Function """ # ROS Node Init Parameters - rospy.init_node('autonomous_control') - - rover_controller = RoverController(target_x, target_y, start_x, start_y, movement_topic, front_arm_topic, - back_arm_topic, front_drum_topic, back_drum_topic, - obstacle_threshold, obstacle_buffer, move_increment, - max_linear_velocity, max_angular_velocity, real_odometry, swarm_control) + rospy.init_node("autonomous_control") + + rover_controller = RoverController( + target_x, + target_y, + start_x, + start_y, + movement_topic, + front_arm_topic, + back_arm_topic, + front_drum_topic, + back_drum_topic, + obstacle_threshold, + obstacle_buffer, + move_increment, + max_linear_velocity, + max_angular_velocity, + real_odometry, + swarm_control, + ) # Start autonomous control loop if rover isn't being controlled by a swarm controller if not swarm_control: - rover_controller.autonomous_control_loop(rover_controller.world_state, - rover_controller.ros_util) + rover_controller.autonomous_control_loop( + rover_controller.world_state, rover_controller.ros_util + ) - rospy.loginfo('Autonomous control initialized.') + rospy.loginfo("Autonomous control initialized.") diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py index eaff47cb..f22d2625 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py @@ -26,7 +26,7 @@ def calculate_heading(world_state): new_heading = math.atan2(dy, dx) # Convert the angle to degrees. - new_heading = (180 * new_heading / math.pi) + new_heading = 180 * new_heading / math.pi # Since gazebo only uses 0 through 360 degree, we must bound any negative # angles to positive ones. @@ -45,8 +45,12 @@ def adjust_angle(heading, new_heading): def quaternion_to_yaw(pose): - quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, - pose.orientation.w) + quaternion = ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) euler = transformations.euler_from_quaternion(quaternion) return euler[2] @@ -112,8 +116,7 @@ def get_best_angle(world_state, buffer, scan, threshold): # Aim to minimize the difference between our facing angle and the angle # to the goal new_heading_degrees = calculate_heading(world_state) - angle2goal_radians = adjust_angle(world_state.heading, - new_heading_degrees) + angle2goal_radians = adjust_angle(world_state.heading, new_heading_degrees) score = abs(angle2goal_radians - angle) if best_score is None or score < best_score: best_score = score @@ -132,7 +135,7 @@ def get_best_angle(world_state, buffer, scan, threshold): def rel_to_abs(current_heading_degrees, relative_heading_radians): # Convert from radians to degrees - relative_heading_degrees = (180 * relative_heading_radians / math.pi) + relative_heading_degrees = 180 * relative_heading_radians / math.pi # Add current heading to relative heading to get absolute heading abs_heading = relative_heading_degrees + current_heading_degrees diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py index 5b5fc8a3..98a6bf9f 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py @@ -5,10 +5,18 @@ import numpy as np from pointcloud_processor import PointCloudProcessor + class ObstacleDetector(PointCloudProcessor): - def __init__(self, max_angle, max_obstacle_dist, min_hole_diameter, - scan_time, range_min, range_max): - super(ObstacleDetector, self).__init__('obstacle_detection') + def __init__( + self, + max_angle, + max_obstacle_dist, + min_hole_diameter, + scan_time, + range_min, + range_max, + ): + super(ObstacleDetector, self).__init__("obstacle_detection") self.max_slope = np.tan(max_angle * np.pi / 180.0) self.max_obstacle_dist = max_obstacle_dist @@ -17,12 +25,15 @@ def __init__(self, max_angle, max_obstacle_dist, min_hole_diameter, self.range_min = range_min self.range_max = range_max - self.hike_pub = rospy.Publisher('obstacle_detection/hike', - LaserScan, queue_size = 10) - self.slope_pub = rospy.Publisher('obstacle_detection/slope', - LaserScan, queue_size = 10) - self.combined_pub = rospy.Publisher('obstacle_detection/combined', - LaserScan, queue_size = 10) + self.hike_pub = rospy.Publisher( + "obstacle_detection/hike", LaserScan, queue_size=10 + ) + self.slope_pub = rospy.Publisher( + "obstacle_detection/slope", LaserScan, queue_size=10 + ) + self.combined_pub = rospy.Publisher( + "obstacle_detection/combined", LaserScan, queue_size=10 + ) rospy.loginfo("Obstacle Detection initialized.") @@ -32,10 +43,12 @@ def init_pc_info(self, camera_info): # Get LaserScan-specific info from camera_info message self.ranges_size = camera_info.width self.frame_id = camera_info.header.frame_id - self.angle_increment = ((self.angle_max - self.angle_min) / - (self.ranges_size - 1)) + self.angle_increment = (self.angle_max - self.angle_min) / ( + self.ranges_size - 1 + ) """Creates and returns a LaserScan object based on the given ranges list.""" + def create_laser_scan(self, ranges): scan = LaserScan() scan.header.stamp = rospy.Time.now() @@ -64,6 +77,7 @@ def on_pc_update(self, pc): each direction. Slope is used to detect above-ground (positive) obstacles and hike is used to detect holes (negative obstacles). """ + def point_cloud_to_laser_scan(self): # Initial LaserScans assume infinite travel in every direction hike_ranges = [float("nan")] * self.ranges_size @@ -76,23 +90,26 @@ def point_cloud_to_laser_scan(self): # Perform obstacle detection if there are points in the pc if pc is not None: # Arrays for values of points in each direction - forward = pc[:,PointCloudProcessor.XYZ["FORWARD"]] - right = pc[:,PointCloudProcessor.XYZ["RIGHT"]] - down = pc[:,PointCloudProcessor.XYZ["DOWN"]] + forward = pc[:, PointCloudProcessor.XYZ["FORWARD"]] + right = pc[:, PointCloudProcessor.XYZ["RIGHT"]] + down = pc[:, PointCloudProcessor.XYZ["DOWN"]] steps, dists = self.to_laser_scan_data(forward, right) # Create matrix where steps, dists, and down arrays are the columns directions = np.column_stack((steps, dists, down)) # Sort rows by first column (steps) - directions = directions[directions[:,0].argsort()] + directions = directions[directions[:, 0].argsort()] # Group rows by step - directions = np.split(directions, np.unique(directions[:,0], - return_index=True)[1][1:],axis=0) + directions = np.split( + directions, + np.unique(directions[:, 0], return_index=True)[1][1:], + axis=0, + ) # Loop through the rows for each step and find obstacles for direction in directions: # Sort rows for this step by the second column (dist) - direction = direction[direction[:,1].argsort()] + direction = direction[direction[:, 1].argsort()] # Step is first column of any row step = int(direction[0, 0]) @@ -116,40 +133,54 @@ def point_cloud_to_laser_scan(self): index_slope = cond_slope.argmax() if cond_slope.any() else None # Populate laserscan with closest point detected - if (index_hike is not None and direction[index_hike, 1] <= - self.max_obstacle_dist): + if ( + index_hike is not None + and direction[index_hike, 1] <= self.max_obstacle_dist + ): hike_ranges[step] = direction[index_hike, 1] if index_slope is not None: slope_ranges[step] = direction[index_slope, 1] # Combine above laserscans - min_ranges[step] = np.nanmin((hike_ranges[step], - slope_ranges[step])) + min_ranges[step] = np.nanmin((hike_ranges[step], slope_ranges[step])) self.hike_pub.publish(self.create_laser_scan(hike_ranges)) self.slope_pub.publish(self.create_laser_scan(slope_ranges)) self.combined_pub.publish(self.create_laser_scan(min_ranges)) """Returns laserscan indices and dists based on the right/forward of a pc""" + def to_laser_scan_data(self, forward, right): # Multiply angles by -1 to get counterclockwise (right to left) ordering angles = np.negative(np.arctan2(right, forward)) # Group angles to discrete indices in laserscan array - steps = np.divide(np.subtract(angles,self.angle_min), - self.angle_increment).astype(int) + steps = np.divide( + np.subtract(angles, self.angle_min), self.angle_increment + ).astype(int) # Find the distance each forward, right coordinate from the robot dists = np.sqrt(np.add(np.square(forward), np.square(right))) return steps, dists + """Initializes obstacle detection.""" -def obstacle_detection(max_angle, max_obstacle_dist, min_hole_diameter, - scan_time=1./30, range_min=0.105, range_max=10.): - od = ObstacleDetector(max_angle, max_obstacle_dist, min_hole_diameter, - scan_time, range_min, range_max) + +def obstacle_detection( + max_angle, + max_obstacle_dist, + min_hole_diameter, + scan_time=1.0 / 30, + range_min=0.105, + range_max=10.0, +): + + od = ObstacleDetector( + max_angle, max_obstacle_dist, min_hole_diameter, scan_time, range_min, range_max + ) rospy.spin() + if __name__ == "__main__": try: obstacle_detection() diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py index 3068996c..6c79329c 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py @@ -19,6 +19,8 @@ from lxml import etree """Represents a single particle in the particle filter""" + + class Particle: def __init__(self, x, y, heading, weight=0): self.x = x @@ -26,6 +28,7 @@ def __init__(self, x, y, heading, weight=0): self.theta = heading self.weight = weight + class ParticleFilter: def __init__(self, resolution, dem_size): self.resolution = resolution @@ -37,6 +40,7 @@ def add_particle(self, x, y, heading, weight=0): self.particles.append(Particle(x, y, heading, weight)) """Place particle at every cell in global DEM""" + def init_particles(self): weight = 1.0 / (self.dem_size ** 2) for y in range(self.dem_size): @@ -44,6 +48,7 @@ def init_particles(self): self.add_particle(x, y, 0.0, weight) """Moves all particles by the given displacement and updates headings""" + def move_particles(self, x_diff, y_diff, theta): for p in self.particles: p.x += x_diff @@ -51,8 +56,11 @@ def move_particles(self, x_diff, y_diff, theta): p.theta = theta # Remove particles that are now out-of-bounds - self.particles = [p for p in self.particles if (0 < p.x < self.dem_size) - and (0 < p.y < self.dem_size)] + self.particles = [ + p + for p in self.particles + if (0 < p.x < self.dem_size) and (0 < p.y < self.dem_size) + ] def resample(self, indexes): # Get new particles based on given indexes @@ -62,6 +70,7 @@ def resample(self, indexes): for p in self.particles: p.weight = 1.0 / num_particles + class ParkRanger(PointCloudProcessor): # Used to determine what value to use for a cell in the local DEM when # multiple points fall into that cell @@ -69,12 +78,13 @@ class ParkRanger(PointCloudProcessor): "mean": np.nanmean, "median": np.nanmedian, "max": np.nanmax, - "min": np.nanmin + "min": np.nanmin, } debug = True """Tracks whether the arms are up (camera isn't blocked)""" + def on_arm_movement(self, data): self.arms_up = data @@ -83,9 +93,10 @@ def on_arm_movement(self, data): Get the z position (height) of the robot using the simulation state. If odometry is not enabled, update x and y positions. """ + def sim_state_callback(self, data): namespace = rospy.get_namespace() - namespace = namespace[1:-1]+"::base_link" + namespace = namespace[1:-1] + "::base_link" try: index = data.name.index(namespace) except: @@ -96,17 +107,19 @@ def sim_state_callback(self, data): if not self.real_odometry: self.position_x = data.pose[index].position.x self.position_y = data.pose[index].position.y - heading = nf.quaternion_to_yaw(data.pose[index]) * 180/np.pi + heading = nf.quaternion_to_yaw(data.pose[index]) * 180 / np.pi self.heading = heading % 360 """Updates x and y position of robot using most recent Odometry message""" + def odometry_callback(self, data): self.position_x = data.pose.pose.position.x + self.start_x self.position_y = data.pose.pose.position.y + self.start_y - heading = nf.quaternion_to_yaw(data.pose.pose) * 180/np.pi + heading = nf.quaternion_to_yaw(data.pose.pose) * 180 / np.pi self.heading = heading % 360 """Returns the path to dem_data/""" + @staticmethod def path_dem(): rospack = rospkg.RosPack() @@ -114,6 +127,7 @@ def path_dem(): return base + "/dem_data/" """Returns the path to worlds/""" + @staticmethod def path_world(): rospack = rospkg.RosPack() @@ -121,6 +135,7 @@ def path_world(): return base + "/worlds/" """Returns the path to weights_results/""" + @staticmethod def weights_results(): rospack = rospkg.RosPack() @@ -128,31 +143,34 @@ def weights_results(): return base + "/weights_results/" """Returns first heightmap tag from given world file""" + @staticmethod def get_first_heightmap_tag(world_file_string): world_root = etree.XML(world_file_string) - return world_root.find('.//heightmap') + return world_root.find(".//heightmap") """Returns the range from the world file""" + @staticmethod def get_world_dem_range(path_world_file): file = open(path_world_file, "r") first_heightmap_tag = ParkRanger.get_first_heightmap_tag(file.read()) - dem_size_path = first_heightmap_tag.find('size').text - xyz_size = dem_size_path.split(' ') + dem_size_path = first_heightmap_tag.find("size").text + xyz_size = dem_size_path.split(" ") return int(xyz_size[-1]) """Returns the name of the DEM associated with the world file""" + @staticmethod def get_world_dem_name(path_world_file): file = open(path_world_file, "r") first_heightmap_tag = ParkRanger.get_first_heightmap_tag(file.read()) - dem_jpg_path = first_heightmap_tag.find('uri').text - split_by_slash = dem_jpg_path.split('/') + dem_jpg_path = first_heightmap_tag.find("uri").text + split_by_slash = dem_jpg_path.split("/") last_item_slash = split_by_slash[len(split_by_slash) - 1] - split_by_underscore = last_item_slash.split('_') + split_by_underscore = last_item_slash.split("_") last_item_index = len(split_by_underscore) - 1 - if split_by_underscore[last_item_index] != 'converted.jpg': + if split_by_underscore[last_item_index] != "converted.jpg": return None split_by_underscore.pop() new_string = "" @@ -161,10 +179,11 @@ def get_world_dem_name(path_world_file): return new_string """Returns path of the file associated with world_dem_name in directory""" + @staticmethod def get_match_dem_n_world(world_dem_name, directory): # Get only files in directory as opposed to files and subdirectories - onlyfiles = [f for f in listdir(directory) if isfile(join(directory,f))] + onlyfiles = [f for f in listdir(directory) if isfile(join(directory, f))] # User hasn't put file in dem_data if not onlyfiles: @@ -172,7 +191,7 @@ def get_match_dem_n_world(world_dem_name, directory): return for i in onlyfiles: - split_by_underscore = i.split('_') + split_by_underscore = i.split("_") split_by_underscore.pop() split_by_underscore.pop() new_string = "" @@ -183,10 +202,11 @@ def get_match_dem_n_world(world_dem_name, directory): return None """Creates global DEM from file in dem_data/""" + def create_array_global_dem(self): file = open(self.dem_data_file_path, "r") lines = file.readlines() - dem_size = map(int,re.findall(r'-?(\d+)',lines[2])) + dem_size = map(int, re.findall(r"-?(\d+)", lines[2])) # File doesn't have dimmensions at line 3 if not dem_size: @@ -200,8 +220,7 @@ def create_array_global_dem(self): self.dem_size = int(dem_size[0]) middle = int(dem_size[0] / 2) - self.global_dem = np.empty((self.dem_size, self.dem_size), - dtype=np.float32) + self.global_dem = np.empty((self.dem_size, self.dem_size), dtype=np.float32) # Read height values into global DEM for i, line in enumerate(lines[3:]): @@ -222,13 +241,15 @@ def create_array_global_dem(self): determine the number of rows and columns and the minimum row value and column value so that a local DEM can be created from a point cloud. """ + def init_local_dem_info(self, range_min, range_max): # Find number of rows and columns and minimum row and column values in # the local DEM array based on the angles and range the camera can see # and the resolution of the global DEM self.num_rows = int(range_max - range_min) + 1 - self.num_columns = int(range_max * (np.tan(self.angle_max) - - np.tan(self.angle_min))) + 1 + self.num_columns = ( + int(range_max * (np.tan(self.angle_max) - np.tan(self.angle_min))) + 1 + ) self.min_row = range_min self.min_column = range_max * np.tan(self.angle_min) @@ -238,43 +259,51 @@ def init_local_dem_info(self, range_min, range_max): global DEM that represents what the robot would see if it were in the state described by the particle. """ + def get_predicted_local_dem(self, particle): # Get angle perpendicular to heading angle_perp = (particle.theta + 90) % 360 - predicted_local_dem = np.empty((self.num_rows, self.num_columns), - dtype=np.float32) + predicted_local_dem = np.empty( + (self.num_rows, self.num_columns), dtype=np.float32 + ) # To have the predicted local DEM centered around the particle, we need # to determine the number of columns on each side of the particle - num_cols_left = self.num_columns/2 + 1 - num_cols_right = self.num_columns/2 + 1 + num_cols_left = self.num_columns / 2 + 1 + num_cols_right = self.num_columns / 2 + 1 if self.num_columns % 2 == 0: num_cols_right -= 1 # Get line to the left of the particle - row_coords = ParkRanger.get_line(particle.y, particle.x, num_cols_left, - -angle_perp, 'left') + row_coords = ParkRanger.get_line( + particle.y, particle.x, num_cols_left, -angle_perp, "left" + ) # Fill up columns to the left of the particle for i, coord in enumerate(reversed(row_coords)): - col_coords = ParkRanger.get_line(coord[0], coord[1], self.num_rows, - particle.theta, 'right') + col_coords = ParkRanger.get_line( + coord[0], coord[1], self.num_rows, particle.theta, "right" + ) try: - predicted_local_dem[:, i] = self.global_dem[col_coords[:,0], - col_coords[:,1]] + predicted_local_dem[:, i] = self.global_dem[ + col_coords[:, 0], col_coords[:, 1] + ] except IndexError: return None # Get line to the right of the particle - row_coords = ParkRanger.get_line(particle.y, particle.x, num_cols_right, - angle_perp, 'right') + row_coords = ParkRanger.get_line( + particle.y, particle.x, num_cols_right, angle_perp, "right" + ) # Fill up columns to the right of the particle for i, coord in enumerate(row_coords): - col_coords = ParkRanger.get_line(coord[0], coord[1], self.num_rows, - particle.theta, 'right') + col_coords = ParkRanger.get_line( + coord[0], coord[1], self.num_rows, particle.theta, "right" + ) try: - predicted_local_dem[:, num_cols_left-1+i] = \ - self.global_dem[col_coords[:,0], col_coords[:,1]] + predicted_local_dem[:, num_cols_left - 1 + i] = self.global_dem[ + col_coords[:, 0], col_coords[:, 1] + ] except IndexError: return None @@ -289,6 +318,7 @@ def get_predicted_local_dem(self, particle): line described. This is done using a modified version of Bresenham's line algorithm. """ + @staticmethod def get_line(center_row, center_col, num_cells, angle, direction): # Make sure angle is non-negative @@ -297,25 +327,25 @@ def get_line(center_row, center_col, num_cells, angle, direction): # Handle cases where tangent returns the same value for different angles # and messes up the rest of the calculations - if direction is 'right' and 135 < angle <= 315: - direction = 'left' + if direction is "right" and 135 < angle <= 315: + direction = "left" angle = 180 - angle - if direction is 'left' and 45 < angle <= 225: - direction = 'right' + if direction is "left" and 45 < angle <= 225: + direction = "right" angle = 180 - angle - slope = np.tan(angle / 180. * np.pi) + slope = np.tan(angle / 180.0 * np.pi) # The below algorithm only works for slope <= 1, so if slope > 1, invert # the slope and set a flag so we know to swap the way we treat the axes if abs(slope) > 1: - slope = 1. / slope + slope = 1.0 / slope line_high = True else: line_high = False # Bresenham's algorithm: - D = 2*abs(slope) - 1 + D = 2 * abs(slope) - 1 row = center_row col = center_col indexes = [] @@ -327,29 +357,32 @@ def get_line(center_row, center_col, num_cells, angle, direction): else: row += -1 if slope > 0 else 1 D -= 2 - D += 2*abs(slope) + D += 2 * abs(slope) if line_high: - row += -1 if direction is 'right' else 1 + row += -1 if direction is "right" else 1 else: - col += 1 if direction is 'right' else -1 + col += 1 if direction is "right" else -1 return np.array(indexes) """Produces similarity score between 0 and 2 for the given numpy arrays""" + @staticmethod def histogram_match(a, b): indexes = ~np.isnan(a) h1 = np.histogram(a[indexes]) h2 = np.histogram(b[indexes]) - correlation = cv2.compareHist(ParkRanger.np_hist_to_cv(h1), - ParkRanger.np_hist_to_cv(h2), - cv2.HISTCMP_CORREL) + correlation = cv2.compareHist( + ParkRanger.np_hist_to_cv(h1), + ParkRanger.np_hist_to_cv(h2), + cv2.HISTCMP_CORREL, + ) return 1 + correlation @staticmethod def np_hist_to_cv(np_hist): counts, bin_edges = np_hist - return counts.ravel().astype('float32') + return counts.ravel().astype("float32") """Converts Point Cloud to Local DEM @@ -361,6 +394,7 @@ def np_hist_to_cv(np_hist): in the local DEM, the height value chosen for that cell is chosen according to the given comparison type ("mean", "median", "min", or "max"). """ + def point_cloud_to_local_dem(self, pc, comparison_type): # Get the comparison function to use when multiple points map to the # same cell in the local DEM @@ -391,31 +425,34 @@ def point_cloud_to_local_dem(self, pc, comparison_type): return local_dem.astype(np.float32) """Finds local DEM row, column indexes and heights for given point cloud""" + def get_local_dem_info(self, pc): - forward = pc[:,PointCloudProcessor.XYZ["FORWARD"]] - right = pc[:,PointCloudProcessor.XYZ["RIGHT"]] - down = pc[:,PointCloudProcessor.XYZ["DOWN"]] + forward = pc[:, PointCloudProcessor.XYZ["FORWARD"]] + right = pc[:, PointCloudProcessor.XYZ["RIGHT"]] + down = pc[:, PointCloudProcessor.XYZ["DOWN"]] - row_indexes = np.subtract(self.num_rows-1, np.subtract(forward, - self.min_row)).astype(int) + row_indexes = np.subtract( + self.num_rows - 1, np.subtract(forward, self.min_row) + ).astype(int) column_indexes = np.subtract(right, self.min_column).astype(int) - heights = np.add(np.add(self.position_z, self.camera_height), - np.negative(down)) + heights = np.add(np.add(self.position_z, self.camera_height), np.negative(down)) return row_indexes, column_indexes, heights """Returns number of effective particles based on the given weights""" + @staticmethod def neff(norm): return 1.0 / np.sum(np.square(norm)) """Publishes the given position estimate as an Odometry message""" + def publish_estimate(self, x, y): # Initialize odometry message odom_msg = Odometry() odom_msg.header.stamp = rospy.Time.now() - odom_msg.header.frame_id = 'odom' - odom_msg.child_frame_id = 'base_link' + odom_msg.header.frame_id = "odom" + odom_msg.child_frame_id = "base_link" # Set x and y values to x and y estimate odom_msg.pose.pose.position.x = float(x) @@ -427,17 +464,25 @@ def publish_estimate(self, x, y): self.estimate_pub.publish(odom_msg) - def __init__(self, real_odometry, world_name, resolution, - local_dem_comparison_type, period, range_min, range_max, - camera_height): - super(ParkRanger, self).__init__('park_ranger') + def __init__( + self, + real_odometry, + world_name, + resolution, + local_dem_comparison_type, + period, + range_min, + range_max, + camera_height, + ): + super(ParkRanger, self).__init__("park_ranger") self.resolution = resolution self.camera_height = camera_height self.real_odometry = real_odometry # Get spawn x and y coordinates for offsetting odometry messages - self.start_x = rospy.get_param('autonomous_control/spawn_x_coord') - self.start_y = rospy.get_param('autonomous_control/spawn_y_coord') + self.start_x = rospy.get_param("autonomous_control/spawn_x_coord") + self.start_y = rospy.get_param("autonomous_control/spawn_y_coord") self.last_x = self.start_x self.last_y = self.start_y @@ -447,8 +492,7 @@ def __init__(self, real_odometry, world_name, resolution, world_name = ParkRanger.path_world() + world_name + ".world" world_dem_name = ParkRanger.get_world_dem_name(world_name) self.new_dem_range = ParkRanger.get_world_dem_range(world_name) - self.dem_data_file_path = \ - ParkRanger.get_match_dem_n_world(world_dem_name, path) + self.dem_data_file_path = ParkRanger.get_match_dem_n_world(world_dem_name, path) if not self.dem_data_file_path: rospy.logerr("Couldn't find match") else: @@ -462,26 +506,24 @@ def __init__(self, real_odometry, world_name, resolution, # moving on link_states = rospy.wait_for_message("/gazebo/link_states", LinkStates) self.sim_state_callback(link_states) - rospy.Subscriber('/gazebo/link_states', LinkStates, - self.sim_state_callback) + rospy.Subscriber("/gazebo/link_states", LinkStates, self.sim_state_callback) if real_odometry: - rospy.Subscriber('odometry/filtered', Odometry, - self.odometry_callback) + rospy.Subscriber("odometry/filtered", Odometry, self.odometry_callback) self.arms_up = False - rospy.Subscriber('arms_up', Bool, self.on_arm_movement) + rospy.Subscriber("arms_up", Bool, self.on_arm_movement) - self.estimate_pub = rospy.Publisher('park_ranger/odom', Odometry, - queue_size=10) + self.estimate_pub = rospy.Publisher("park_ranger/odom", Odometry, queue_size=10) self.run(period, local_dem_comparison_type) rospy.loginfo("Park Ranger initialized") """Compares global DEM to point clouds to localize the robot""" + def run(self, period, local_dem_comparison_type): - r = rospy.Rate(1./period) + r = rospy.Rate(1.0 / period) init_flag = True ran_out_of_particles = False self.particle_filter = ParticleFilter(self.resolution, self.dem_size) @@ -491,12 +533,11 @@ def run(self, period, local_dem_comparison_type): pc = self.get_points() if pc is not None: - local_dem = self.point_cloud_to_local_dem(pc, - local_dem_comparison_type) + local_dem = self.point_cloud_to_local_dem(pc, local_dem_comparison_type) if not init_flag: if ParkRanger.debug: - rospy.logwarn("Prediction step") + rospy.logwarn("Prediction step") self.predict_particle_movement() if ParkRanger.debug: @@ -504,8 +545,11 @@ def run(self, period, local_dem_comparison_type): self.likelihood(local_dem) if ParkRanger.debug: - rospy.logwarn("Actual position: {} {}".format( - self.position_x, self.position_y)) + rospy.logwarn( + "Actual position: {} {}".format( + self.position_x, self.position_y + ) + ) rospy.logwarn("Estimation step") est_x, est_y = self.estimate() @@ -513,12 +557,14 @@ def run(self, period, local_dem_comparison_type): est_x = est_x - (self.dem_size / 2) est_y = (self.dem_size / 2) - est_y if ParkRanger.debug: - rospy.logwarn("Estimated position: {} {}".format( - est_x, est_y)) + rospy.logwarn( + "Estimated position: {} {}".format(est_x, est_y) + ) # Get number of unique (in terms of x, y vals) particles - num_unique = len(set([(p.x, p.y) for p in - self.particle_filter.particles])) + num_unique = len( + set([(p.x, p.y) for p in self.particle_filter.particles]) + ) # If particle filter has nearly converged, # publish estimate if num_unique <= 5: @@ -537,6 +583,7 @@ def run(self, period, local_dem_comparison_type): r.sleep() """Updates weight of each particle by comparing local and predicted DEMs""" + def likelihood(self, local_dem): img_path = self.weights_results() weights = np.zeros_like(self.global_dem) @@ -556,11 +603,13 @@ def likelihood(self, local_dem): plt.colorbar() now = datetime.now() current_time = now.strftime("%H:%M:%S") - plt.savefig("{}{}_{}.png".format(img_path, "weights", current_time), - bbox_inches='tight') + plt.savefig( + "{}{}_{}.png".format(img_path, "weights", current_time), bbox_inches="tight" + ) plt.clf() """Move particles based on movement of robot""" + def predict_particle_movement(self): # Compare movement of robot and adjust particles accordingly current_x = self.position_x @@ -576,6 +625,7 @@ def predict_particle_movement(self): self.last_y = current_y """Produce estimate for robot's position via weighted sum of particles""" + def estimate(self): sum_x = 0 sum_y = 0 @@ -583,28 +633,47 @@ def estimate(self): sum_weights = sum([p.weight for p in self.particle_filter.particles]) for p in self.particle_filter.particles: if p.weight != 0.0: - sum_x += (p.x * p.weight) - sum_y += (p.y * p.weight) + sum_x += p.x * p.weight + sum_y += p.y * p.weight if sum_weights == 0: rospy.logwarn("Sum of particle weights is zero") return None, None return int(sum_x / sum_weights), int(sum_y / sum_weights) """Perform resampling if neff value drops below threshold""" + def resample(self): weights = [p.weight for p in self.particle_filter.particles] norm = weights / np.linalg.norm(weights) - if ParkRanger.neff(norm) < (len(norm) / 8): + if ParkRanger.neff(norm) < (len(norm) / 8): indexes = mc.systematic_resample(norm) self.particle_filter.resample(indexes) + """Initializes park ranger""" -def park_ranger(real_odometry, world_name, resolution=0.5, - local_dem_comparison_type="max", period=5, range_min=0.105, - range_max=10., camera_height=0.34): - pr = ParkRanger(real_odometry, world_name, resolution, - local_dem_comparison_type, period, range_min, range_max, - camera_height) + + +def park_ranger( + real_odometry, + world_name, + resolution=0.5, + local_dem_comparison_type="max", + period=5, + range_min=0.105, + range_max=10.0, + camera_height=0.34, +): + pr = ParkRanger( + real_odometry, + world_name, + resolution, + local_dem_comparison_type, + period, + range_min, + range_max, + camera_height, + ) + if __name__ == "__main__": try: diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/pointcloud_processor.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/pointcloud_processor.py index 66b72f84..18875666 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/pointcloud_processor.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/pointcloud_processor.py @@ -5,13 +5,11 @@ import numpy as np import image_geometry + class PointCloudProcessor(object): """Coordinate System""" - XYZ = { - "RIGHT": 0, - "DOWN": 1, - "FORWARD": 2 - } + + XYZ = {"RIGHT": 0, "DOWN": 1, "FORWARD": 2} def __init__(self, node_name): rospy.init_node(node_name) @@ -21,6 +19,7 @@ def __init__(self, node_name): rospy.Subscriber("depth/points", PointCloud2, self.on_pc_update) """Stores the most recent PointCloud2 message received""" + def on_pc_update(self, pc): self.point_cloud = pc @@ -30,6 +29,7 @@ def on_pc_update(self, pc): returns a numpy array with all the non-NaN points in the point cloud. If the point cloud is entirely NaN values, None is returned. """ + def get_points(self): if self.point_cloud is None: return None @@ -44,11 +44,12 @@ def get_points(self): return pc if pc.size > 0 else None """Returns the angle (in radians) between two 3D rays""" + @staticmethod def angle_between_rays(ray1, ray2): - dot_product = ray1[0]*ray2[0] + ray1[1]*ray2[1] + ray1[2]*ray2[2] - magnitude1 = np.sqrt(((ray1[0]**2) + (ray1[1]**2) + (ray1[2]**2))) - magnitude2 = np.sqrt(((ray2[0]**2) + (ray2[1]**2) + (ray2[2]**2))) + dot_product = ray1[0] * ray2[0] + ray1[1] * ray2[1] + ray1[2] * ray2[2] + magnitude1 = np.sqrt(((ray1[0] ** 2) + (ray1[1] ** 2) + (ray1[2] ** 2))) + magnitude2 = np.sqrt(((ray2[0] ** 2) + (ray2[1] ** 2) + (ray2[2] ** 2))) return np.arccos(dot_product / (magnitude1 * magnitude2)) """Initializes information about point cloud based on CameraInfo message @@ -59,6 +60,7 @@ def angle_between_rays(ray1, ray2): method, is based heavily on the C++ source code of the "depthimage_to_laserscan" package. """ + def init_pc_info(self, camera_info): # Initialize camera cam_model = image_geometry.PinholeCameraModel() @@ -71,7 +73,7 @@ def init_pc_info(self, camera_info): left_ray = cam_model.projectPixelTo3dRay(rect_pixel_left) # Find vector for rightmost view in camera - raw_pixel_right = (width-1, cam_model.cy()) + raw_pixel_right = (width - 1, cam_model.cy()) rect_pixel_right = cam_model.rectifyPoint(raw_pixel_right) right_ray = cam_model.projectPixelTo3dRay(rect_pixel_right) diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py index 01b65a7d..e70926b5 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py @@ -23,34 +23,34 @@ def set_front_arm_angle(world_state, ros_util, target_angle): """ Set front arm to absolute angle target_angle in radians. """ if target_angle > world_state.front_arm_angle: while target_angle > world_state.front_arm_angle: - ros_util.publish_actions('stop', 1, 0, 0, 0) + ros_util.publish_actions("stop", 1, 0, 0, 0) ros_util.rate.sleep() ros_util.arms_up_pub.publish(True) else: while target_angle < world_state.front_arm_angle: - ros_util.publish_actions('stop', -1, 0, 0, 0) + ros_util.publish_actions("stop", -1, 0, 0, 0) ros_util.rate.sleep() ros_util.arms_up_pub.publish(False) - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) def set_back_arm_angle(world_state, ros_util, target_angle): """ Set back arm to absolute angle target_angle in radians. """ - '''rospy.loginfo('Setting back arm angle to %s radian%s...', + """rospy.loginfo('Setting back arm angle to %s radian%s...', str(target_angle), - "" if target_angle == 1 else "s")''' + "" if target_angle == 1 else "s")""" if target_angle > world_state.back_arm_angle: while target_angle > world_state.back_arm_angle: - ros_util.publish_actions('stop', 0, 1, 0, 0) + ros_util.publish_actions("stop", 0, 1, 0, 0) ros_util.rate.sleep() else: while target_angle < world_state.back_arm_angle: - ros_util.publish_actions('stop', 0, -1, 0, 0) + ros_util.publish_actions("stop", 0, -1, 0, 0) ros_util.rate.sleep() - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) def self_check(world_state, ros_util): @@ -59,7 +59,7 @@ def self_check(world_state, ros_util): """ if ros_util.auto_function_command == 32 or ros_util.auto_function_command == 0: rospy.loginfo("Cancelling auto-function command...") - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) ros_util.control_pub.publish(False) return -1 @@ -69,7 +69,7 @@ def self_check(world_state, ros_util): return 3 # Future status checks for physical hardware - ''' + """ if world_state.on_side == True: rospy.loginfo("On side! Attempting auto self-right...") return 2 @@ -78,7 +78,7 @@ def self_check(world_state, ros_util): ros_util.publish_actions('stop', 1, 0, 0, 0) ros_util.control_pub.publish(False) return -1 - ''' + """ return 1 @@ -99,7 +99,7 @@ def turn(new_heading, direction, world_state, ros_util): # Turn the number of degrees towards your new heading. while angle_traveled < angle_dist - 2: if self_check(world_state, ros_util) != 1: - rospy.logdebug('Status check failed.') + rospy.logdebug("Status check failed.") return old_heading = world_state.heading @@ -107,13 +107,14 @@ def turn(new_heading, direction, world_state, ros_util): # Maps angle traveled to sin(x) function for velocity ramping. # X is bounded between 0 and angle_dist # Y is bounded between 0 and max_angular_velocity - turn_velocity = (ros_util.max_angular_velocity * - math.sin((angle_traveled / math.pi) * (10 / angle_dist))) + turn_velocity = ros_util.max_angular_velocity * math.sin( + (angle_traveled / math.pi) * (10 / angle_dist) + ) # Cap our minimum velocity at 1/10 our max velocity. turn_velocity = max(turn_velocity, ros_util.max_angular_velocity / 10) - if direction is 'right': + if direction is "right": turn_velocity *= -1 twist_message = Twist() @@ -133,13 +134,15 @@ def turn(new_heading, direction, world_state, ros_util): """ -def move(dist, world_state, ros_util, direction='forward'): +def move(dist, world_state, ros_util, direction="forward"): # Get current distance from EZ-RASSOR to our current best target location. old_x = world_state.positionX old_y = world_state.positionY dist_traveled = 0 - dist_to_goal = math.sqrt((world_state.target_location.x - old_x) ** 2 + - (world_state.target_location.y - old_y) ** 2) + dist_to_goal = math.sqrt( + (world_state.target_location.x - old_x) ** 2 + + (world_state.target_location.y - old_y) ** 2 + ) # Either move dist amount or the distance to the goal, whichever is smaller. move_dist = min(dist, dist_to_goal) @@ -148,13 +151,17 @@ def move(dist, world_state, ros_util, direction='forward'): # obstacle in the way. while dist_traveled < move_dist: if self_check(world_state, ros_util) != 1: - rospy.logdebug('Status check failed.') + rospy.logdebug("Status check failed.") return # Exit if we come too close to an obstacle - if not nf.angle_is_safe(0, ros_util.obstacle_threshold / 2.0, - ros_util.obstacle_buffer, scan, - ros_util.obstacle_threshold): + if not nf.angle_is_safe( + 0, + ros_util.obstacle_threshold / 2.0, + ros_util.obstacle_buffer, + scan, + ros_util.obstacle_threshold, + ): rospy.loginfo("Obstacle too close! Stopping!") ros_util.publish_actions("stop", 0, 0, 0, 0) break @@ -162,13 +169,14 @@ def move(dist, world_state, ros_util, direction='forward'): # Maps distance traveled to sin(x) function for velocity ramping. # X is bounded between 0 and move_dist # Y is bounded between 0 and max_linear_velocity - move_velocity = (ros_util.max_linear_velocity * - math.sin((dist_traveled / math.pi) * (10 / move_dist))) + move_velocity = ros_util.max_linear_velocity * math.sin( + (dist_traveled / math.pi) * (10 / move_dist) + ) # Cap our minimum velocity at 1/10 our max velocity. move_velocity = max(move_velocity, ros_util.max_linear_velocity / 10) - if direction is 'backward': + if direction is "backward": move_velocity *= -1 twist_message = Twist() @@ -177,21 +185,22 @@ def move(dist, world_state, ros_util, direction='forward'): ros_util.rate.sleep() - dist_traveled = math.sqrt((world_state.positionX - old_x) ** 2 + - (world_state.positionY - old_y) ** 2) + dist_traveled = math.sqrt( + (world_state.positionX - old_x) ** 2 + (world_state.positionY - old_y) ** 2 + ) def reverse_turn(world_state, ros_util): """ Reverse until object no longer detected and turn left """ while world_state.warning_flag == 3: - ros_util.publish_actions('reverse', 0, 0, 0, 0) + ros_util.publish_actions("reverse", 0, 0, 0, 0) ros_util.rate.sleep() new_heading = (world_state.heading + 60) % 360 while (new_heading - 1) < world_state.heading < (new_heading + 1): - ros_util.publish_actions('left', 0, 0, 0, 0) + ros_util.publish_actions("left", 0, 0, 0, 0) def dodge_left(world_state, ros_util): @@ -203,12 +212,16 @@ def dodge_left(world_state, ros_util): while world_state.warning_flag != 0 or (threshold < 25): if world_state.warning_flag == 0: threshold += 1 - ros_util.publish_actions('left', 0, 0, 0, 0) + ros_util.publish_actions("left", 0, 0, 0, 0) ros_util.rate.sleep() - while nf.euclidean_distance(start_x, world_state.positionX, - start_y, world_state.positionY) < 2: - ros_util.publish_actions('forward', 0, 0, 0, 0) + while ( + nf.euclidean_distance( + start_x, world_state.positionX, start_y, world_state.positionY + ) + < 2 + ): + ros_util.publish_actions("forward", 0, 0, 0, 0) ros_util.rate.sleep() @@ -221,12 +234,16 @@ def dodge_right(world_state, ros_util): while world_state.warning_flag != 0 or (threshold < 25): if world_state.warning_flag == 0: threshold += 1 - ros_util.publish_actions('right', 0, 0, 0, 0) + ros_util.publish_actions("right", 0, 0, 0, 0) ros_util.rate.sleep() - while nf.euclidean_distance(start_x, world_state.positionX, - start_y, world_state.positionY) < 2: - ros_util.publish_actions('forward', 0, 0, 0, 0) + while ( + nf.euclidean_distance( + start_x, world_state.positionX, start_y, world_state.positionY + ) + < 2 + ): + ros_util.publish_actions("forward", 0, 0, 0, 0) ros_util.rate.sleep() @@ -234,11 +251,11 @@ def self_right_from_side(world_state, ros_util): """ Flip EZ-RASSOR over from its side. """ rospy.loginfo("Starting auto self-right...") - while (world_state.on_side != False): - ros_util.publish_actions('stop', 0, 1, 0, 0) - ros_util.publish_actions('stop', 1, 0, 0, 0) + while world_state.on_side != False: + ros_util.publish_actions("stop", 0, 1, 0, 0) + ros_util.publish_actions("stop", 1, 0, 0, 0) - ros_util.publish_actions('stop', 0, 0, 0, 0) + ros_util.publish_actions("stop", 0, 0, 0, 0) """ Returns the best direction to go towards to get to the goal @@ -252,8 +269,9 @@ def self_right_from_side(world_state, ros_util): def get_turn_angle(world_state, ros_util): # Iterate over all of the laser beams in our current scan and determine the # best angle to turn towards. - best_angle = nf.get_best_angle(world_state, ros_util.obstacle_buffer, scan, - ros_util.obstacle_threshold) + best_angle = nf.get_best_angle( + world_state, ros_util.obstacle_buffer, scan, ros_util.obstacle_threshold + ) # Loop until we find a safe angle. while True: @@ -265,13 +283,15 @@ def get_turn_angle(world_state, ros_util): switchDirection = -1 wedgeDist = 0 wedgeSize = (scan.angle_max - scan.angle_min) / 2.0 - rospy.loginfo("There is nowhere to go in the current wedge. " + - "Turning to an adjacent wedge.") + rospy.loginfo( + "There is nowhere to go in the current wedge. " + + "Turning to an adjacent wedge." + ) # Keep checking adjacent wedges until we find a safe angle. while best_angle is None: if self_check(world_state, ros_util) != 1: - rospy.logdebug('Status check failed.') + rospy.logdebug("Status check failed.") return set_front_arm_angle(world_state, ros_util, 1.3) @@ -281,28 +301,34 @@ def get_turn_angle(world_state, ros_util): wedgeDist += 1 if switchDirection < 0: - direction = 'left' + direction = "left" else: - direction = 'right' + direction = "right" # Turn to an adjacent wedge and check if we can see some way to # progress towards the goal. - turn(nf.rel_to_abs(world_state.heading, wedgeSize * wedgeDist), - direction, world_state, ros_util) - ros_util.publish_actions('stop', 0, 0, 0, 0) + turn( + nf.rel_to_abs(world_state.heading, wedgeSize * wedgeDist), + direction, + world_state, + ros_util, + ) + ros_util.publish_actions("stop", 0, 0, 0, 0) ros_util.rate.sleep() rospy.sleep(0.1) rospy.loginfo("Currently at wedge W{}".format(wedgeDist - 1)) - best_angle = nf.get_best_angle(world_state, - ros_util.obstacle_buffer, scan, - ros_util.obstacle_threshold) + best_angle = nf.get_best_angle( + world_state, + ros_util.obstacle_buffer, + scan, + ros_util.obstacle_threshold, + ) # If we over-turned when doing the wedge, try turning back more towards # the goal until we see an obstacle in our view. wedgeSize = (scan.angle_max - scan.angle_min) / 20.0 - buffer_angle = math.atan(ros_util.obstacle_buffer / - ros_util.obstacle_threshold) + buffer_angle = math.atan(ros_util.obstacle_buffer / ros_util.obstacle_threshold) min_angle = scan.angle_min + buffer_angle max_angle = scan.angle_max - buffer_angle best_index = int((best_angle - scan.angle_min) / scan.angle_increment) @@ -314,20 +340,24 @@ def get_turn_angle(world_state, ros_util): while best_index <= min_index or best_index >= max_index: # Get direction that would bring us closer to facing the goal. if best_angle < 0: - direction = 'right' + direction = "right" else: - direction = 'left' + direction = "left" # Turn slightly in that direction and check if it's safe to turn # more towards the goal. - turn(nf.rel_to_abs(world_state.heading, wedgeSize), direction, - world_state, ros_util) - ros_util.publish_actions('stop', 0, 0, 0, 0) + turn( + nf.rel_to_abs(world_state.heading, wedgeSize), + direction, + world_state, + ros_util, + ) + ros_util.publish_actions("stop", 0, 0, 0, 0) ros_util.rate.sleep() rospy.sleep(0.1) - best_angle = nf.get_best_angle(world_state, - ros_util.obstacle_buffer, scan, - ros_util.obstacle_threshold) + best_angle = nf.get_best_angle( + world_state, ros_util.obstacle_buffer, scan, ros_util.obstacle_threshold + ) # If we turned too far back and now have no safe angle to go to, # break and restart the wedge algorithm. @@ -347,6 +377,7 @@ def get_turn_angle(world_state, ros_util): # CLIENT-SERVER API WHICH IS ONLY USED IN SWARM CONTROL MODE # ################################################################## + def send_feedback(world_state, waypoint_server): """ Send feedback (rover current pose) from a rover's action server back to the diff --git a/packages/autonomy/ezrassor_swarm_control/setup.py b/packages/autonomy/ezrassor_swarm_control/setup.py index eee68e26..cceea266 100755 --- a/packages/autonomy/ezrassor_swarm_control/setup.py +++ b/packages/autonomy/ezrassor_swarm_control/setup.py @@ -5,7 +5,6 @@ import catkin_pkg.python_setup setup_arguments = catkin_pkg.python_setup.generate_distutils_setup( - packages=("ezrassor_swarm_control", ), - package_dir={"" : "source"}, + packages=("ezrassor_swarm_control",), package_dir={"": "source"}, ) distutils.core.setup(**setup_arguments) diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/constants.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/constants.py index 3141c652..0c106620 100644 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/constants.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/constants.py @@ -1,6 +1,3 @@ from geometry_msgs.msg import Point -commands = { - 'CHG': Point(-999, -999, -999), - 'DIG': Point(-998, -998, -998) -} +commands = {"CHG": Point(-999, -999, -999), "DIG": Point(-998, -998, -998)} diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/path_planner.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/path_planner.py index 37e0d135..f9929165 100644 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/path_planner.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/path_planner.py @@ -11,6 +11,7 @@ from geometry_msgs.msg import Point from ezrassor_swarm_control.msg import Path + class PathPlanner: """ Global A* path planner which runs on a gazebo world's elevation-map @@ -24,7 +25,16 @@ def __init__(self, map_path, rover_max_climb_slope): self.width, self.height = self.map.shape # Diagonals and cardinal direction movements allowed - self.neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0), (1, 1), (1, -1), (-1, 1), (-1, -1)] + self.neighbors = [ + (0, 1), + (0, -1), + (1, 0), + (-1, 0), + (1, 1), + (1, -1), + (-1, 1), + (-1, -1), + ] # Only cardinal directions # self.neighbors = [(0,1),(0,-1),(1,0),(-1,0)] @@ -40,19 +50,35 @@ def find_path(self, start_, goal_): """ if not self.check_bounds(start_): - rospy.loginfo('Start coordinate {} is out of bounds'.format((start_.x, start_.y))) + rospy.loginfo( + "Start coordinate {} is out of bounds".format((start_.x, start_.y)) + ) return if not self.check_bounds(goal_): - rospy.loginfo('Goal coordinate {} is out of bounds'.format((goal_.x, goal_.y))) + rospy.loginfo( + "Goal coordinate {} is out of bounds".format((goal_.x, goal_.y)) + ) return - rospy.loginfo('Searching for path from {} to {}'.format((start_.x, start_.y), (goal_.x, goal_.y))) + rospy.loginfo( + "Searching for path from {} to {}".format( + (start_.x, start_.y), (goal_.x, goal_.y) + ) + ) start_time = timer() # Convert simulation coordinates, whose origin are in the center of the map, # to image coordinates with origin in the top-left corner - start = Point(int(round(start_.x + (self.width // 2))), int(round(abs(start_.y - (self.height // 2)))), 0) - goal = Point(int(round(goal_.x + (self.width // 2))), int(round(abs(goal_.y - (self.height // 2)))), 0) + start = Point( + int(round(start_.x + (self.width // 2))), + int(round(abs(start_.y - (self.height // 2)))), + 0, + ) + goal = Point( + int(round(goal_.x + (self.width // 2))), + int(round(abs(goal_.y - (self.height // 2)))), + 0, + ) # Initialize open and closed open = [] @@ -84,7 +110,7 @@ def find_path(self, start_, goal_): # Build path by backtracking from current node to the start node path = self.backtrack_path(cur, start, previous) - rospy.loginfo('Path found in {}'.format(timer() - start_time)) + rospy.loginfo("Path found in {}".format(timer() - start_time)) return path closed.add(cur) @@ -107,22 +133,28 @@ def find_path(self, start_, goal_): # Add node to open list if it hasn't been looked at or if # we've found a new cheaper path to this node - if tent_g < g_scores.get(neighbor, sys.maxint) or neighbor not in [i[1] for i in open]: + if tent_g < g_scores.get(neighbor, sys.maxint) or neighbor not in [ + i[1] for i in open + ]: previous[neighbor] = cur g_scores[neighbor] = tent_g f_scores[neighbor] = tent_g + self.euclidean(neighbor, goal) heapq.heappush(open, (f_scores[neighbor], neighbor)) - rospy.loginfo('No path found in {} seconds. Sending straight line path.'.format(timer() - start_time)) + rospy.loginfo( + "No path found in {} seconds. Sending straight line path.".format( + timer() - start_time + ) + ) previous[goal] = start path = self.backtrack_path(goal, start, previous) return path def get_valid_neighbors(self, cur): - ''' + """ Returns a list of valid neighboring nodes as ros Point messages - ''' + """ neighbors = [] @@ -156,8 +188,10 @@ def is_valid_move(self, cur, neighbor): # Diagonal move if dx == 1 and dy == 1: # Check for mountains or craters adjacent to diagonal move - if abs(self.map[cur.y, neighbor.x] - self.map[cur.y, cur.x]) > 2 or \ - abs(self.map[neighbor.y, cur.x] - self.map[cur.y, cur.x]) > 2: + if ( + abs(self.map[cur.y, neighbor.x] - self.map[cur.y, cur.x]) > 2 + or abs(self.map[neighbor.y, cur.x] - self.map[cur.y, cur.x]) > 2 + ): return False return True @@ -181,7 +215,7 @@ def backtrack_path(self, cur, start, previous): if cur in previous: cur = previous[cur] else: - raise ValueError('Unable to backtrack to start node.') + raise ValueError("Unable to backtrack to start node.") # Add start node to path cur.x -= self.width // 2 @@ -201,7 +235,11 @@ def euclidean(self, a, b): a_x, a_y = int(round(a.x)), int(round(a.y)) b_x, b_y = int(round(b.x)), int(round(b.y)) - return np.sqrt((b.x - a.x) ** 2 + (b.y - a.y) ** 2 + (self.map[b_y, b_x] - self.map[a_y, a_x]) ** 2) + return np.sqrt( + (b.x - a.x) ** 2 + + (b.y - a.y) ** 2 + + (self.map[b_y, b_x] - self.map[a_y, a_x]) ** 2 + ) def check_bounds(self, coord): """ diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py index 981089bb..8e15a5f2 100755 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py @@ -20,7 +20,7 @@ class Rover: def __init__(self, id): self.id = id - self.activity = 'idle' + self.activity = "idle" self.path_completed = False self.battery = 100 @@ -42,21 +42,37 @@ def __init__(self, robot_count, dig_sites, lander_loc, world, elevation_map): self.rover_activity_status_db = {i: Rover(i) for i in range(1, robot_count + 1)} # Set up publishers used to send paths to each rover's waypoint client - self.waypoint_pubs = {i: rospy.Publisher('/ezrassor{}/waypoint_client'.format(i), - Path, - queue_size=10) - for i in range(1, robot_count + 1)} + self.waypoint_pubs = { + i: rospy.Publisher( + "/ezrassor{}/waypoint_client".format(i), Path, queue_size=10 + ) + for i in range(1, robot_count + 1) + } def is_at_lander(self, robot_status): - if euclidean_distance(robot_status.pose.position.x, self.lander_loc.x, robot_status.pose.position.y, - self.lander_loc.y) <= 0.5: + if ( + euclidean_distance( + robot_status.pose.position.x, + self.lander_loc.x, + robot_status.pose.position.y, + self.lander_loc.y, + ) + <= 0.5 + ): return True else: return False def is_at_digsite(self, robot_status, dig_site_idx): - if euclidean_distance(robot_status.pose.position.x, self.dig_sites[dig_site_idx].x, robot_status.pose.position.y, - self.dig_sites[dig_site_idx].y) <= 0.5: + if ( + euclidean_distance( + robot_status.pose.position.x, + self.dig_sites[dig_site_idx].x, + robot_status.pose.position.y, + self.dig_sites[dig_site_idx].y, + ) + <= 0.5 + ): return True else: return False @@ -67,16 +83,28 @@ def create_command(self, cmd): return path def run(self): - rospy.loginfo('Running the swarm controller for {} rover(s)'.format(self.robot_count)) - rospy.loginfo('{} total dig sites: {}' - .format(len(self.dig_sites), [(site.x, site.y) for site in self.dig_sites])) + rospy.loginfo( + "Running the swarm controller for {} rover(s)".format(self.robot_count) + ) + rospy.loginfo( + "{} total dig sites: {}".format( + len(self.dig_sites), [(site.x, site.y) for site in self.dig_sites] + ) + ) # wait for rovers to spawn - rospy.sleep(20.) + rospy.sleep(20.0) # Build path of the elevation map - height_map = os.path.join(os.path.expanduser('~'), - '.gazebo', 'models', self.world, 'materials', 'textures', self.elevation_map) + height_map = os.path.join( + os.path.expanduser("~"), + ".gazebo", + "models", + self.world, + "materials", + "textures", + self.elevation_map, + ) # Create A* path planner path_planner = PathPlanner(height_map, rover_max_climb_slope=2) @@ -90,53 +118,76 @@ def run(self): if rover_status: site_num = (i - 1) % len(self.dig_sites) - if self.rover_activity_status_db[i].activity == 'idle': - path = path_planner.find_path(rover_status.pose.position, self.dig_sites[site_num]) + if self.rover_activity_status_db[i].activity == "idle": + path = path_planner.find_path( + rover_status.pose.position, self.dig_sites[site_num] + ) if path: self.waypoint_pubs[i].publish(path) - self.rover_activity_status_db[i].activity = 'driving to digsite' + self.rover_activity_status_db[ + i + ].activity = "driving to digsite" - elif self.rover_activity_status_db[i].activity == 'digging': + elif self.rover_activity_status_db[i].activity == "digging": if rover_status.battery <= 35.0: preempt_rover_path(i) - path = path_planner.find_path(rover_status.pose.position, self.lander_loc) + path = path_planner.find_path( + rover_status.pose.position, self.lander_loc + ) if path: self.waypoint_pubs[i].publish(path) - self.rover_activity_status_db[i].activity = 'driving to lander' + self.rover_activity_status_db[ + i + ].activity = "driving to lander" - elif self.rover_activity_status_db[i].activity == 'driving to lander': + elif ( + self.rover_activity_status_db[i].activity == "driving to lander" + ): if self.is_at_lander(rover_status): - if self.rover_activity_status_db[i].activity != 'charging': + if self.rover_activity_status_db[i].activity != "charging": preempt_rover_path(i) - self.waypoint_pubs[i].publish(self.create_command('CHG')) - self.rover_activity_status_db[i].activity = 'charging' + self.waypoint_pubs[i].publish( + self.create_command("CHG") + ) + self.rover_activity_status_db[i].activity = "charging" - elif self.rover_activity_status_db[i].activity == 'charging': + elif self.rover_activity_status_db[i].activity == "charging": if rover_status.battery >= 95.0: - path = path_planner.find_path(rover_status.pose.position, self.dig_sites[site_num]) + path = path_planner.find_path( + rover_status.pose.position, self.dig_sites[site_num] + ) if path: self.waypoint_pubs[i].publish(path) - self.rover_activity_status_db[i].activity = 'driving to digsite' + self.rover_activity_status_db[ + i + ].activity = "driving to digsite" else: if self.is_at_digsite(rover_status, site_num): - if self.rover_activity_status_db[i].activity == 'driving to digsite': + if ( + self.rover_activity_status_db[i].activity + == "driving to digsite" + ): if rover_status.battery >= 35.0: preempt_rover_path(i) - self.waypoint_pubs[i].publish(self.create_command('DIG')) - self.rover_activity_status_db[i].activity = 'digging' + self.waypoint_pubs[i].publish( + self.create_command("DIG") + ) + self.rover_activity_status_db[ + i + ].activity = "digging" def on_start_up(robot_count, target_xs, target_ys, lander_coords, world, elevation_map): """ Initialization Function """ # ROS Node Init Parameters - rospy.init_node('swarm_control') + rospy.init_node("swarm_control") # Unpack digsite coordinates from string format - target_xs = str(target_xs).split(' ') - target_ys = str(target_ys).split(' ') + target_xs = str(target_xs).split(" ") + target_ys = str(target_ys).split(" ") - lander_location = str(lander_coords).split(' ') + lander_location = str(lander_coords).split(" ") lander_positionX = int(lander_location[0]) lander_positionY = int(lander_location[1]) @@ -146,7 +197,9 @@ def on_start_up(robot_count, target_xs, target_ys, lander_coords, world, elevati lander_point.y = lander_positionY if len(target_xs) != len(target_ys): - raise ValueError('Number of dig site x coordinates does not match the number of y coordinates') + raise ValueError( + "Number of dig site x coordinates does not match the number of y coordinates" + ) # Creat array of Point messages to store dig site locations dig_sites = [] @@ -159,5 +212,7 @@ def on_start_up(robot_count, target_xs, target_ys, lander_coords, world, elevati dig_sites.append(coord) # Run the swarm control node - swarm_controller = SwarmController(robot_count, dig_sites, lander_point, world, elevation_map) + swarm_controller = SwarmController( + robot_count, dig_sites, lander_point, world, elevation_map + ) swarm_controller.run() diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py index 5fef3241..7978e7ea 100644 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py @@ -30,7 +30,7 @@ def get_rover_status(rover_num): """ # Build the service endpoint - service = '/ezrassor{}/rover_status'.format(rover_num) + service = "/ezrassor{}/rover_status".format(rover_num) # Ensure it's running rospy.wait_for_service(service, 5.0) @@ -56,7 +56,7 @@ def preempt_rover_path(rover_num): """ # Build the service endpoint - service = '/ezrassor{}/preempt_path'.format(rover_num) + service = "/ezrassor{}/preempt_path".format(rover_num) # Ensure it's running rospy.wait_for_service(service, 5.0) diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py index 9af99c64..dfcc01ca 100755 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py @@ -23,7 +23,7 @@ class WaypointClient: """ def __init__(self, robot_num, world, elevation_map): - self.client_name = 'waypoint' + self.client_name = "waypoint" self.namespace = rospy.get_namespace() # Create the waypoint action client @@ -31,12 +31,12 @@ def __init__(self, robot_num, world, elevation_map): self.client.wait_for_server() - rospy.Subscriber('waypoint_client', - Path, - self.send_path) + rospy.Subscriber("waypoint_client", Path, self.send_path) # Service which allows the waypoint client's current path to be preempted from any other node - self.preempt_service = rospy.Service('preempt_path', PreemptPath, self.preempt_path) + self.preempt_service = rospy.Service( + "preempt_path", PreemptPath, self.preempt_path + ) # Whether a path has been canceled or not self.preempt = False @@ -51,16 +51,23 @@ def __init__(self, robot_num, world, elevation_map): self.max_veer_distance = 5 # Create path planner to be used during replanning - height_map = os.path.join(os.path.expanduser('~'), - '.gazebo', 'models', world, 'materials', 'textures', elevation_map) + height_map = os.path.join( + os.path.expanduser("~"), + ".gazebo", + "models", + world, + "materials", + "textures", + elevation_map, + ) self.path_planner = PathPlanner(height_map, rover_max_climb_slope=2) def is_dig_cmd(self, cmd): - return cmd == commands['DIG'] + return cmd == commands["DIG"] def is_charge_cmd(self, cmd): - return cmd == commands['CHG'] + return cmd == commands["CHG"] def preempt_path(self, request=None): """Callback executed when the waypoint client receives a preempt path request""" @@ -77,18 +84,28 @@ def feedback_cb(self, feedback): # format((self.namespace + self.client_name), (x, y), feedback.battery)) # Replan path if rover veers away from its current waypoint too far - if not (self.is_dig_cmd(self.goal) or self.is_charge_cmd(self.goal)) and self.goal and self.cur_waypoint: + if ( + not (self.is_dig_cmd(self.goal) or self.is_charge_cmd(self.goal)) + and self.goal + and self.cur_waypoint + ): veer_distance = euclidean2D(feedback.pose.position, self.cur_waypoint) if veer_distance > self.max_veer_distance: - rospy.loginfo('Waypoint client {} forced to replan path!'.format(self.namespace + self.client_name)) + rospy.loginfo( + "Waypoint client {} forced to replan path!".format( + self.namespace + self.client_name + ) + ) self.preempt_path() - new_path = self.path_planner.find_path(feedback.pose.position, self.goal) + new_path = self.path_planner.find_path( + feedback.pose.position, self.goal + ) # Wait for previous path to be totally cancelled before sending the new path while self.preempt is True: - rospy.sleep(1.) + rospy.sleep(1.0) self.send_path(new_path) @@ -99,11 +116,17 @@ def done_cb(self, status, result): y = round(result.pose.position.y, 2) if result.preempted: - rospy.loginfo('Waypoint client {} preempted! Current position: {} Current battery: {}'. - format((self.namespace + self.client_name), (x, y), result.battery)) + rospy.loginfo( + "Waypoint client {} preempted! Current position: {} Current battery: {}".format( + (self.namespace + self.client_name), (x, y), result.battery + ) + ) else: - rospy.loginfo('Waypoint client {} reached waypoint! Current position: {} Current battery: {}'. - format((self.namespace + self.client_name), (x, y), result.battery)) + rospy.loginfo( + "Waypoint client {} reached waypoint! Current position: {} Current battery: {}".format( + (self.namespace + self.client_name), (x, y), result.battery + ) + ) def send_waypoint(self, waypoint): @@ -125,16 +148,29 @@ def send_path(self, data): self.goal = path[-1] if self.is_dig_cmd(self.goal): - rospy.loginfo('Waypoint client {} received dig command!'.format(self.namespace + self.client_name)) + rospy.loginfo( + "Waypoint client {} received dig command!".format( + self.namespace + self.client_name + ) + ) self.send_waypoint(self.goal) elif self.is_charge_cmd(self.goal): - rospy.loginfo('Waypoint client {} received charge command!'.format(self.namespace + self.client_name)) + rospy.loginfo( + "Waypoint client {} received charge command!".format( + self.namespace + self.client_name + ) + ) self.send_waypoint(self.goal) else: - rospy.loginfo('Waypoint client {} received path from {} to {}'. - format(self.namespace + self.client_name, (path[0].x, path[0].y), (path[-1].x, path[-1].y))) + rospy.loginfo( + "Waypoint client {} received path from {} to {}".format( + self.namespace + self.client_name, + (path[0].x, path[0].y), + (path[-1].x, path[-1].y), + ) + ) # Send each waypoint in a path to the rover for node in path[1:]: @@ -146,7 +182,11 @@ def send_path(self, data): self.cur_waypoint = node self.send_waypoint(node) - rospy.loginfo('Waypoint client {} finished sending waypoints!'.format(self.namespace + self.client_name)) + rospy.loginfo( + "Waypoint client {} finished sending waypoints!".format( + self.namespace + self.client_name + ) + ) # Reset server to receive another path self.preempt = False @@ -155,6 +195,6 @@ def send_path(self, data): def on_start_up(robot_num, world, elevation_map): - rospy.init_node('waypoint_client') + rospy.init_node("waypoint_client") WaypointClient(robot_num, world, elevation_map) rospy.spin() diff --git a/packages/communication/ezrassor_controller_server/setup.py b/packages/communication/ezrassor_controller_server/setup.py index a9787262..5f297e9a 100644 --- a/packages/communication/ezrassor_controller_server/setup.py +++ b/packages/communication/ezrassor_controller_server/setup.py @@ -6,7 +6,6 @@ import catkin_pkg.python_setup setup_arguments = catkin_pkg.python_setup.generate_distutils_setup( - packages=("ezrassor_controller_server", ), - package_dir={"" : "source"}, + packages=("ezrassor_controller_server",), package_dir={"": "source"}, ) distutils.core.setup(**setup_arguments) diff --git a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py index 53a7aa03..3999a289 100644 --- a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py +++ b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py @@ -16,8 +16,10 @@ def get_custom_handler(publishers): """Get a custom HTTP request handler with appropriate publishers.""" + class CustomRequestHandler(BaseHTTPServer.BaseHTTPRequestHandler): """Handle JSON POST requests on a simple HTTP server.""" + target_coordinates_publisher = publishers[0] autonomous_toggles_publisher = publishers[1] wheel_instructions_publisher = publishers[2] @@ -32,12 +34,8 @@ def do_POST(self): self.send_header("Content-Type", "application/json") self.end_headers() content_length = int(self.headers.getheader("content-length", 0)) - instructions = json.loads( - self.rfile.read( - content_length, - ), - ) - self.wfile.write(json.dumps({"status" : 200})) + instructions = json.loads(self.rfile.read(content_length,),) + self.wfile.write(json.dumps({"status": 200})) self._publish_instructions(instructions) def _publish_instructions(self, instructions): @@ -100,7 +98,8 @@ def start_node( target_coordinates_topic, autonomous_toggles_topic, queue_size, - sleep_duration): + sleep_duration, +): """Start the node and let the fun begin!""" try: rospy.init_node(default_node_name) @@ -125,71 +124,55 @@ def start_node( # Create a whole heap of publishers. target_coordinates_publisher = rospy.Publisher( - target_coordinates_topic, - geometry_msgs.msg.Point, - queue_size=queue_size, + target_coordinates_topic, geometry_msgs.msg.Point, queue_size=queue_size, ) autonomous_toggles_publisher = rospy.Publisher( - autonomous_toggles_topic, - std_msgs.msg.Int8, - queue_size=queue_size, + autonomous_toggles_topic, std_msgs.msg.Int8, queue_size=queue_size, ) wheel_instructions_publisher = rospy.Publisher( - wheel_instructions_topic, - geometry_msgs.msg.Twist, - queue_size=queue_size, + wheel_instructions_topic, geometry_msgs.msg.Twist, queue_size=queue_size, ) front_arm_instructions_publisher = rospy.Publisher( - front_arm_instructions_topic, - std_msgs.msg.Float32, - queue_size=queue_size, + front_arm_instructions_topic, std_msgs.msg.Float32, queue_size=queue_size, ) back_arm_instructions_publisher = rospy.Publisher( - back_arm_instructions_topic, - std_msgs.msg.Float32, - queue_size=queue_size, + back_arm_instructions_topic, std_msgs.msg.Float32, queue_size=queue_size, ) front_drum_instructions_publisher = rospy.Publisher( - front_drum_instructions_topic, - std_msgs.msg.Float32, - queue_size=queue_size, + front_drum_instructions_topic, std_msgs.msg.Float32, queue_size=queue_size, ) back_drum_instructions_publisher = rospy.Publisher( - back_drum_instructions_topic, - std_msgs.msg.Float32, - queue_size=queue_size, + back_drum_instructions_topic, std_msgs.msg.Float32, queue_size=queue_size, ) - + rospy.loginfo("Creating HTTP server...") # Create an HTTP server. server = BaseHTTPServer.HTTPServer( ("", port), - get_custom_handler(( - target_coordinates_publisher, - autonomous_toggles_publisher, - wheel_instructions_publisher, - front_arm_instructions_publisher, - back_arm_instructions_publisher, - front_drum_instructions_publisher, - back_drum_instructions_publisher, - )), + get_custom_handler( + ( + target_coordinates_publisher, + autonomous_toggles_publisher, + wheel_instructions_publisher, + front_arm_instructions_publisher, + back_arm_instructions_publisher, + front_drum_instructions_publisher, + back_drum_instructions_publisher, + ) + ), ) rospy.loginfo("Creating server kill thread...") # Launch a kill thread that kills the server when roscore dies. kill_thread = threading.Thread( - target=kill_server, - args=( - server, - sleep_duration, - ), + target=kill_server, args=(server, sleep_duration,), ) kill_thread.start() rospy.loginfo("Controller server initialized.") - + # Run the server infinitely. server.serve_forever(poll_interval=sleep_duration) diff --git a/packages/communication/ezrassor_joy_translator/setup.py b/packages/communication/ezrassor_joy_translator/setup.py index 2a24e58d..97465a1c 100644 --- a/packages/communication/ezrassor_joy_translator/setup.py +++ b/packages/communication/ezrassor_joy_translator/setup.py @@ -6,7 +6,6 @@ import catkin_pkg.python_setup setup_arguments = catkin_pkg.python_setup.generate_distutils_setup( - packages=("ezrassor_joy_translator", ), - package_dir={"" : "source"}, + packages=("ezrassor_joy_translator",), package_dir={"": "source"}, ) distutils.core.setup(**setup_arguments) diff --git a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py index 2a59cec1..0f7a1b65 100644 --- a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py +++ b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py @@ -7,6 +7,7 @@ from sensor_msgs.msg import Joy from geometry_msgs.msg import Twist + def callback(data, additional_arguments): """Parse Joy data, publish Twist data.""" @@ -63,102 +64,103 @@ def callback(data, additional_arguments): return twist = Twist() - twist.linear.x = ((data.axes[4] + data.axes[1]) / 2) - twist.angular.z = ((data.axes[4] - data.axes[1]) / 2) + twist.linear.x = (data.axes[4] + data.axes[1]) / 2 + twist.angular.z = (data.axes[4] - data.axes[1]) / 2 trigger_threshold = 0.0 # Use "-(1-data.axes[5])/2" not -1 for variable speed # Front Arm if data.buttons[5] > (1 - data.axes[5]) / 2: - command_front_arm = 1 + command_front_arm = 1 elif data.buttons[5] < (1 - data.axes[5]) / 2 and trigger_threshold > data.axes[5]: - command_front_arm = -1 + command_front_arm = -1 else: - command_front_arm = 0 + command_front_arm = 0 # Back Arm if data.buttons[4] > (1 - data.axes[2]) / 2: - command_back_arm = 1 + command_back_arm = 1 elif data.buttons[4] < (1 - data.axes[2]) / 2 and trigger_threshold > data.axes[2]: - command_back_arm = -1 + command_back_arm = -1 else: - command_back_arm = 0 + command_back_arm = 0 # Front Drum if data.buttons[3] > data.buttons[1]: command_front_drum = 1 elif data.buttons[3] < data.buttons[1]: - command_front_drum = -1 + command_front_drum = -1 else: - command_front_drum = 0 + command_front_drum = 0 # Back Drum if data.buttons[2] > data.buttons[0]: command_back_drum = 1 elif data.buttons[2] < data.buttons[0]: - command_back_drum = -1 + command_back_drum = -1 else: - command_back_drum = 0 + command_back_drum = 0 pub_wheels.publish(twist) pub_front_arm.publish(command_front_arm) pub_back_arm.publish(command_back_arm) pub_front_drum.publish(command_front_drum) pub_back_drum.publish(command_back_drum) - + def start_node(): try: TOPIC = "joy" NODE = "joy_translator" rospy.init_node(NODE) - publish_topic_wheels = rospy.get_param(rospy.get_name() - + "/wheel_instructions_topic") - publish_topic_front_arm = rospy.get_param(rospy.get_name() - + "/front_arm_instructions_topic") - publish_topic_back_arm = rospy.get_param(rospy.get_name() - + "/back_arm_instructions_topic") - publish_topic_front_drum = rospy.get_param(rospy.get_name() - + "/front_drum_instructions_topic") - publish_topic_back_drum = rospy.get_param(rospy.get_name() - + "/back_drum_instructions_topic") + publish_topic_wheels = rospy.get_param( + rospy.get_name() + "/wheel_instructions_topic" + ) + publish_topic_front_arm = rospy.get_param( + rospy.get_name() + "/front_arm_instructions_topic" + ) + publish_topic_back_arm = rospy.get_param( + rospy.get_name() + "/back_arm_instructions_topic" + ) + publish_topic_front_drum = rospy.get_param( + rospy.get_name() + "/front_drum_instructions_topic" + ) + publish_topic_back_drum = rospy.get_param( + rospy.get_name() + "/back_drum_instructions_topic" + ) publish_topic_auto_toggles = "autonomous_toggles" # Publishers # Wheel twist - pub_wheels = rospy.Publisher(publish_topic_wheels, - Twist, - queue_size=10) + pub_wheels = rospy.Publisher(publish_topic_wheels, Twist, queue_size=10) # Arm Front - pub_front_arm = rospy.Publisher(publish_topic_front_arm, - Float32, - queue_size=10) + pub_front_arm = rospy.Publisher(publish_topic_front_arm, Float32, queue_size=10) # Arm Back - pub_back_arm = rospy.Publisher(publish_topic_back_arm, - Float32, - queue_size=10) + pub_back_arm = rospy.Publisher(publish_topic_back_arm, Float32, queue_size=10) # Drum Front - pub_front_drum = rospy.Publisher(publish_topic_front_drum, - Float32, - queue_size=10) + pub_front_drum = rospy.Publisher( + publish_topic_front_drum, Float32, queue_size=10 + ) # Drum Back - pub_back_drum = rospy.Publisher(publish_topic_back_drum, - Float32, - queue_size=10) + pub_back_drum = rospy.Publisher(publish_topic_back_drum, Float32, queue_size=10) # Autonomous Toggles - pub_auto_toggles = rospy.Publisher(publish_topic_auto_toggles, - Int8, - queue_size=10) + pub_auto_toggles = rospy.Publisher( + publish_topic_auto_toggles, Int8, queue_size=10 + ) rate = rospy.Rate(60) - rospy.Subscriber(TOPIC, - Joy, - callback, - callback_args=(pub_wheels, - pub_front_arm, - pub_back_arm, - pub_front_drum, - pub_back_drum, - pub_auto_toggles)) + rospy.Subscriber( + TOPIC, + Joy, + callback, + callback_args=( + pub_wheels, + pub_front_arm, + pub_back_arm, + pub_front_drum, + pub_back_drum, + pub_auto_toggles, + ), + ) rospy.loginfo("Joy translator initialized.") rospy.spin() except rospy.ROSInterruptException: diff --git a/packages/communication/ezrassor_topic_switch/setup.py b/packages/communication/ezrassor_topic_switch/setup.py index 9b85f336..35d32ca1 100644 --- a/packages/communication/ezrassor_topic_switch/setup.py +++ b/packages/communication/ezrassor_topic_switch/setup.py @@ -6,7 +6,6 @@ import catkin_pkg.python_setup setup_arguments = catkin_pkg.python_setup.generate_distutils_setup( - packages=("ezrassor_topic_switch", ), - package_dir={"" : "source"}, + packages=("ezrassor_topic_switch",), package_dir={"": "source"}, ) distutils.core.setup(**setup_arguments) diff --git a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py index b415296b..d46ff198 100755 --- a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py +++ b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py @@ -9,6 +9,7 @@ class OverrideStatus: """Automatically maintain an override status.""" + def __init__(self, override_topic): """Initialize this override status with a subscriber. @@ -16,12 +17,10 @@ def __init__(self, override_topic): """ self.__status = False rospy.Subscriber( - override_topic, - std_msgs.msg.Bool, - callback=self.__update, + override_topic, std_msgs.msg.Bool, callback=self.__update, ) self.__status_lock = threading.Lock() - + def __eq__(self, other_status): """Allow easy comparisons of this override status with a boolean.""" self.__status_lock.acquire() @@ -62,47 +61,33 @@ def start_node(default_node_name, override_topic, queue_size): # module and class. If these can't be imported, an exception is thrown. try: topic_type_module = __import__( - topic_type_module, - globals(), - locals(), - [topic_type_class], - -1, + topic_type_module, globals(), locals(), [topic_type_class], -1, ) topic_type_class = vars(topic_type_module)[topic_type_class] except ImportError: raise ImportError( - "No topic type module named \"{0}\"".format(str(topic_type_module)), + 'No topic type module named "{0}"'.format(str(topic_type_module)), ) except KeyError: raise ImportError( - "No topic type class named \"{0}\"".format(str(topic_type_class)), + 'No topic type class named "{0}"'.format(str(topic_type_class)), ) # Create all publishers and subscribers. output_publisher = rospy.Publisher( - output_topic, - topic_type_class, - queue_size=queue_size, + output_topic, topic_type_class, queue_size=queue_size, ) rospy.Subscriber( primary_topic, topic_type_class, callback=route_item, - callback_args=( - output_publisher, - override_status, - False, - ), + callback_args=(output_publisher, override_status, False,), ) rospy.Subscriber( secondary_topic, topic_type_class, callback=route_item, - callback_args=( - output_publisher, - override_status, - True, - ), + callback_args=(output_publisher, override_status, True,), ) rospy.loginfo("Topic switch initialized.") diff --git a/packages/simulation/ezrassor_sim_control/setup.py b/packages/simulation/ezrassor_sim_control/setup.py index a7f07908..a44c40a7 100755 --- a/packages/simulation/ezrassor_sim_control/setup.py +++ b/packages/simulation/ezrassor_sim_control/setup.py @@ -6,7 +6,6 @@ import catkin_pkg.python_setup setup_arguments = catkin_pkg.python_setup.generate_distutils_setup( - packages=("ezrassor_sim_control", ), - package_dir={"" : "source"}, + packages=("ezrassor_sim_control",), package_dir={"": "source"}, ) distutils.core.setup(**setup_arguments) diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py index 80120485..084f9e61 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py @@ -12,24 +12,24 @@ MAX_ARM_SPEED = 0.75 # /ezrassor/arm_back_velocity_controller/command -pub_FA = rospy.Publisher('arm_front_velocity_controller/command', - Float64, - queue_size = 10) -pub_BA = rospy.Publisher('arm_back_velocity_controller/command', - Float64, - queue_size = 10) +pub_FA = rospy.Publisher( + "arm_front_velocity_controller/command", Float64, queue_size=10 +) +pub_BA = rospy.Publisher("arm_back_velocity_controller/command", Float64, queue_size=10) + def handle_front_arm_movements(data): """Move the front arm of the robot per the commands encoded in the instruction. """ - pub_FA.publish(data.data*MAX_ARM_SPEED) + pub_FA.publish(data.data * MAX_ARM_SPEED) + def handle_back_arm_movements(data): """Move the back arm of the robot per the commands encoded in the instruction. """ - pub_BA.publish(data.data*MAX_ARM_SPEED) + pub_BA.publish(data.data * MAX_ARM_SPEED) def start_node(): diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py index 230130ad..36be475f 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py @@ -14,24 +14,26 @@ MAX_DRUM_SPEED = 5 # /ezrassor/drum_back_velocity_controller/command -pub_FA = rospy.Publisher('drum_front_velocity_controller/command', - Float64, - queue_size = 10) -pub_BA = rospy.Publisher('drum_back_velocity_controller/command', - Float64, - queue_size = 10) +pub_FA = rospy.Publisher( + "drum_front_velocity_controller/command", Float64, queue_size=10 +) +pub_BA = rospy.Publisher( + "drum_back_velocity_controller/command", Float64, queue_size=10 +) + def handle_front_drum_movements(data): """Move the front drum of the robot per the commands encoded in the instruction. """ - pub_FA.publish(data.data*MAX_DRUM_SPEED) + pub_FA.publish(data.data * MAX_DRUM_SPEED) + def handle_back_drum_movements(data): """Move the back drum of the robot per the commands encoded in the instruction. """ - pub_BA.publish(data.data*MAX_DRUM_SPEED) + pub_BA.publish(data.data * MAX_DRUM_SPEED) def start_node(): @@ -44,4 +46,3 @@ def start_node(): rospy.spin() except rospy.ROSInterruptException: pass - diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py index 40e659ae..74d9691e 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py @@ -12,9 +12,10 @@ TOPIC = "wheel_instructions" MAX_VELOCITY = 5 + def wheel_movement_callback(twist): - pub_wheels = rospy.Publisher('diff_drive_controller/cmd_vel', Twist, queue_size=10) + pub_wheels = rospy.Publisher("diff_drive_controller/cmd_vel", Twist, queue_size=10) new_twist = Twist() @@ -34,7 +35,7 @@ def wheel_movement_callback(twist): def start_node(): try: - rospy.init_node(NODE, anonymous = True) + rospy.init_node(NODE, anonymous=True) rospy.Subscriber(TOPIC, Twist, wheel_movement_callback) rospy.loginfo("Simulation wheels driver initialized.") rospy.spin() From 496abeaaf52c0d38d0977698a713ec0b39b5fdae Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 2 Jul 2020 21:34:40 -0400 Subject: [PATCH 10/17] Ignoring "line too long" --- .github/workflows/dev.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index 2d5bada0..1709f115 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -27,3 +27,5 @@ jobs: steps: - uses: actions/checkout@v2 - uses: TrueBrain/actions-flake8@master + with: + ignore: E501 From a0015e17be44b0b68c326efe4fcc222b1286780a Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 14:44:31 -0400 Subject: [PATCH 11/17] Flake8: Fixing many unused imports, whitespace errors --- .github/workflows/dev.yml | 2 +- dem_scripts/auto_move/move_model_world.py | 6 ++-- dem_scripts/extract_tile/tile.py | 5 ++- dem_scripts/get_elev/get_coords.py | 5 --- dem_scripts/get_elev/readDEM.py | 3 -- dem_scripts/mk_gaz_wrld/check_dem_size.py | 2 -- experimental/cosmic-gps/cell_gps.py | 15 ++++---- experimental/cosmic-gps/centroid_tests.py | 24 ++++++------- experimental/cosmic-gps/cluster_tests.py | 8 ++--- experimental/cosmic-gps/measurement_tests.py | 20 +++++------ experimental/cosmic-gps/show_image.py | 1 - experimental/cosmic-gps/star.py | 21 ++++++----- experimental/cosmic-gps/star_cat.py | 7 ++-- experimental/cosmic-gps/star_cat_tests.py | 2 +- experimental/cosmic-gps/star_ref.py | 2 -- experimental/cosmic-gps/star_tests.py | 36 +++++++++---------- experimental/cosmic-gps/thresh_global.py | 3 -- experimental/cosmic-gps/thresh_local_ratio.py | 1 - experimental/cosmic-gps/thresh_tests.py | 4 +-- .../ezrassor_autonomous_control/ai_objects.py | 5 +-- .../ezrassor_autonomous_control/arm_force.py | 5 --- .../auto_functions.py | 2 +- .../autonomous_control.py | 3 +- .../nav_functions.py | 1 - .../obstacle_detection.py | 8 +++-- .../park_ranger.py | 19 +++++----- .../utility_functions.py | 10 +++--- .../ezrassor_swarm_control/swarm_utils.py | 4 +-- .../ezrassor_swarm_control/waypoint_client.py | 3 +- .../controller_server.py | 1 - .../ezrassor_joy_translator/joy_translator.py | 2 +- .../ezrassor_topic_switch/topic_switch.py | 2 +- .../test/test_topic_switch | 2 +- .../ezrassor_sim_control/sim_arms_driver.py | 7 ++-- .../ezrassor_sim_control/sim_drums_driver.py | 9 +++-- .../ezrassor_sim_control/sim_wheels_driver.py | 1 - 36 files changed, 105 insertions(+), 146 deletions(-) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index 1709f115..2439e431 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -28,4 +28,4 @@ jobs: - uses: actions/checkout@v2 - uses: TrueBrain/actions-flake8@master with: - ignore: E501 + ignore: E501, W503 diff --git a/dem_scripts/auto_move/move_model_world.py b/dem_scripts/auto_move/move_model_world.py index 4f3f76b0..1ed03ce5 100755 --- a/dem_scripts/auto_move/move_model_world.py +++ b/dem_scripts/auto_move/move_model_world.py @@ -1,16 +1,14 @@ #!/usr/bin/env python import os -import sys def main(): - world_ext = "" choice = "queue_packs/" - l = os.listdir("./" + choice) + packs = os.listdir("./" + choice) - for i in l: + for i in packs: print("Processing {}".format(i)) diff --git a/dem_scripts/extract_tile/tile.py b/dem_scripts/extract_tile/tile.py index d3cdc44a..6dda9268 100755 --- a/dem_scripts/extract_tile/tile.py +++ b/dem_scripts/extract_tile/tile.py @@ -1,5 +1,8 @@ #!/usr/bin/env python -import os, gdal, sys, ntpath +import os +import gdal +import sys +import ntpath def main(): diff --git a/dem_scripts/get_elev/get_coords.py b/dem_scripts/get_elev/get_coords.py index 1855d189..9f3825ca 100644 --- a/dem_scripts/get_elev/get_coords.py +++ b/dem_scripts/get_elev/get_coords.py @@ -1,10 +1,5 @@ #!/usr/bin/env python -import gdal -import ogr import osr -import sys -import struct -import numpy as np """ Returns the list of geocoordinates for each corner """ diff --git a/dem_scripts/get_elev/readDEM.py b/dem_scripts/get_elev/readDEM.py index f8365ffb..785b0cc3 100755 --- a/dem_scripts/get_elev/readDEM.py +++ b/dem_scripts/get_elev/readDEM.py @@ -1,12 +1,9 @@ #!/usr/bin/env python import gdal -import ogr import osr import sys -import struct import numpy as np import get_coords -from scipy.signal import argrelextrema """ Print all elevation values from dem into txt file """ diff --git a/dem_scripts/mk_gaz_wrld/check_dem_size.py b/dem_scripts/mk_gaz_wrld/check_dem_size.py index 765e7d8e..d0b9771c 100755 --- a/dem_scripts/mk_gaz_wrld/check_dem_size.py +++ b/dem_scripts/mk_gaz_wrld/check_dem_size.py @@ -3,8 +3,6 @@ """ Program to check the size of a dem """ import gdal -import ogr -import osr import sys diff --git a/experimental/cosmic-gps/cell_gps.py b/experimental/cosmic-gps/cell_gps.py index 65a7dfd0..af15e538 100644 --- a/experimental/cosmic-gps/cell_gps.py +++ b/experimental/cosmic-gps/cell_gps.py @@ -1,13 +1,10 @@ #!/usr/bin/env python # Core Function/Node -import numpy as np import cv2 as cv -from thresh_local_ratio import thresh_local_ratio from thresh_global import thresh_global from cluster import cluster -from star import Star from star_cat import Star_Cat from measurement import Calibration_Function from measurement import calculate_angular_distance @@ -15,6 +12,10 @@ from measurement import calculate_geographic_position +def getkey(item): + return item[0] + + def cell_gps_core(): position_latitude = 0.0 @@ -22,7 +23,6 @@ def cell_gps_core(): # read the configuarion file - window_size = 9 # used for thresholding image_center_x = 1511.5 image_center_y = 2015.5 time_list = [ @@ -103,11 +103,9 @@ def cell_gps_core(): # from the image center finally pair each star object with its # intensity and sort the pairs by intensity list_of_stars = [] - getkey = lambda item: item[0] for clustr in list_of_clusters: if clustr.shape() > 0.50: cluster_intensity = clustr.get_intensity() - cluster_center = clustr.centroid() clustr.set_angles(image_center_x, image_center_y) list_of_stars.append([cluster_intensity, clustr]) list_of_stars = sorted(list_of_stars, key=getkey, reverse=True) @@ -179,15 +177,14 @@ def cell_gps_core(): position_longitude, position_latitude, gp[0], gp[1] ) if d < min_d: - save_coor = coor save_gp = gp save_match = match min_d = d position_latitude = save_gp[1] position_longitude = save_gp[0] star_catalogue.rescope(coor[0], coor[1]) - print "Geographic Position", save_gp - print save_match + print("Geographic Position", save_gp) + print(save_match) break # end diff --git a/experimental/cosmic-gps/centroid_tests.py b/experimental/cosmic-gps/centroid_tests.py index 000dd41d..7c6dc408 100644 --- a/experimental/cosmic-gps/centroid_tests.py +++ b/experimental/cosmic-gps/centroid_tests.py @@ -8,13 +8,14 @@ # test: outputs the centers an intensities -import numpy as np import cv2 as cv from thresh_global import thresh_global from cluster import cluster -from star import Star -from measurement import Calibration_Function + + +def getkey(item): + return item[0] def main(): @@ -26,7 +27,6 @@ def main(): list_of_clusters = cluster(pixels) list_of_stars = [] - getkey = lambda item: item[0] for clustr in list_of_clusters: cluster_center = clustr.centroid() cluster_intensity = clustr.get_intensity() @@ -34,16 +34,16 @@ def main(): list_of_stars.append([cluster_intensity, cluster_center, clustr]) list_of_stars = sorted(list_of_stars, key=getkey, reverse=True) - print "***Stars***" - print "" + print("***Stars***") + print("") for star in list_of_stars: - print "*****" - print "Center:" - print star[1] - print "" - print "Cluster:" + print("*****") + print("Center:") + print(star[1]) + print("") + print("Cluster:") star[2].show() - print "_____" + print("_____") if __name__ == "__main__": diff --git a/experimental/cosmic-gps/cluster_tests.py b/experimental/cosmic-gps/cluster_tests.py index 71327c14..702f6a70 100644 --- a/experimental/cosmic-gps/cluster_tests.py +++ b/experimental/cosmic-gps/cluster_tests.py @@ -8,12 +8,10 @@ # test: outputs the star clusters -import numpy as np import cv2 as cv from thresh_global import thresh_global from cluster import cluster -from star import Star def main(): @@ -25,11 +23,11 @@ def main(): list_of_clusters = cluster(pixels) - print "***Clusters***" - print "" + print("***Clusters***") + print("") for clust in list_of_clusters: clust.show() - print "" + print("") if __name__ == "__main__": diff --git a/experimental/cosmic-gps/measurement_tests.py b/experimental/cosmic-gps/measurement_tests.py index 50416671..21bb25c4 100644 --- a/experimental/cosmic-gps/measurement_tests.py +++ b/experimental/cosmic-gps/measurement_tests.py @@ -146,7 +146,7 @@ def main(): dec_to_decimal_deg(8, 17, 21.54), ) stars1.append(star) - print "" + print("") # compare calculated distances @@ -159,11 +159,11 @@ def main(): calcul = calculate_angular_distance( starA.angle, 90.0 - starA.distance, starB.angle, 90.0 - starB.distance ) - print "Calculated distances between ", starA.name, " and ", starB.name - print "Actual ", actual - print "Calcul ", calcul - print "diff", calcul - actual - print "" + print("Calculated distances between ", starA.name, " and ", starB.name) + print("Actual ", actual) + print("Calcul ", calcul) + print("diff", calcul - actual) + print("") # Position derivation for top 3 coor = determine_coordinate( @@ -179,15 +179,15 @@ def main(): 30.0, 0.01, ) - print "Derived celestial position, RA: ", coor[0], " DEC: ", coor[1] - print "" + print("Derived celestial position, RA: ", coor[0], " DEC: ", coor[1]) + print("") # Global Position Derivation for top 3 clock_time = 26.2708333 gha_aries = gha_aries_calibration.neville_interpolation(clock_time) - print "GHA Aries: ", gha_aries + print("GHA Aries: ", gha_aries) gp = calculate_geographic_position(coor, gha_aries) - print "Derived geographic position, Long: ", gp[0], " Lat: ", gp[1] + print("Derived geographic position, Long: ", gp[0], " Lat: ", gp[1]) if __name__ == "__main__": diff --git a/experimental/cosmic-gps/show_image.py b/experimental/cosmic-gps/show_image.py index c7ae3a40..e0a9a89e 100644 --- a/experimental/cosmic-gps/show_image.py +++ b/experimental/cosmic-gps/show_image.py @@ -1,7 +1,6 @@ #!/usr/bin/env python # show_image.py -import numpy as np import cv2 as cv diff --git a/experimental/cosmic-gps/star.py b/experimental/cosmic-gps/star.py index 5a0f056b..d75bd8ec 100644 --- a/experimental/cosmic-gps/star.py +++ b/experimental/cosmic-gps/star.py @@ -1,7 +1,6 @@ #!/usr/bin/env python # star.py -from math import radians from math import degrees from math import sin from math import atan @@ -138,16 +137,16 @@ def shape(self): # test function def show(self): - print "*****" - print "Pixels:" - print self.pixel_dict - print "" - print "Intensity:" - print self.intensity - print "" - print "Shape:" - print self.shape() - print "_____" + print("*****") + print("Pixels:") + print(self.pixel_dict) + print("") + print("Intensity:") + print(self.intensity) + print("") + print("Shape:") + print(self.shape()) + print("_____") # test function def quick_set_angles(self, aod, adfc): diff --git a/experimental/cosmic-gps/star_cat.py b/experimental/cosmic-gps/star_cat.py index 8be6ff1e..647f66d9 100644 --- a/experimental/cosmic-gps/star_cat.py +++ b/experimental/cosmic-gps/star_cat.py @@ -1,7 +1,6 @@ #!/usr/bin/env python # star_cat.py -import numpy as np from star_ref import Star_Ref from measurement import ra_to_decimal_deg from measurement import dec_to_decimal_deg @@ -236,21 +235,21 @@ def print_distribution(self): for sid_star in self.adj_star_list: ra = self.adj_star_list[sid_star].get_ra() dec = self.adj_star_list[sid_star].get_dec() - print "For quad ra", ra, "dec", dec + print("For quad ra", ra, "dec", dec) # test function def print_angles(self): # How many stars have each angle for x in range(self.num_angles): size = x * self.angle_size - print ("For angle ", size, self.angles_list.get(x)) + print("For angle ", size, self.angles_list.get(x)) # test function def print_scope_angles(self): # How many stars have each angle for x in range(self.num_angles): size = x * self.angle_size - print ("For angle ", size, self.scope_angles_list.get(x)) + print("For angle ", size, self.scope_angles_list.get(x)) def match(stars, epsilon, num_stars, angle_size, angles_list, adj_star_list): diff --git a/experimental/cosmic-gps/star_cat_tests.py b/experimental/cosmic-gps/star_cat_tests.py index ae1e6c74..73702d31 100644 --- a/experimental/cosmic-gps/star_cat_tests.py +++ b/experimental/cosmic-gps/star_cat_tests.py @@ -79,7 +79,7 @@ def main(): list_of_stars_size -= 1 if len(list_of_possible_matches) > 0: break # can be possibly many group of candidates. - print list_of_possible_matches + print(list_of_possible_matches) if __name__ == "__main__": diff --git a/experimental/cosmic-gps/star_ref.py b/experimental/cosmic-gps/star_ref.py index 2e483623..a48a7a37 100644 --- a/experimental/cosmic-gps/star_ref.py +++ b/experimental/cosmic-gps/star_ref.py @@ -1,8 +1,6 @@ #!/usr/bin/env python # star_ref.py -import numpy as np - class Star_Ref: def __init__(self, sid, name, mag, ra, dec): diff --git a/experimental/cosmic-gps/star_tests.py b/experimental/cosmic-gps/star_tests.py index 0fb21fe2..e389a672 100644 --- a/experimental/cosmic-gps/star_tests.py +++ b/experimental/cosmic-gps/star_tests.py @@ -7,13 +7,14 @@ # test: outputs the original stars # test: outputs the centers an intensities -import numpy as np import cv2 as cv from thresh_global import thresh_global from cluster import cluster -from star import Star -from measurement import Calibration_Function + + +def getkey(item): + return item[0] def main(): @@ -30,7 +31,6 @@ def main(): list_of_clusters = cluster(pixels) list_of_stars = [] - getkey = lambda item: item[0] for clustr in list_of_clusters: if clustr.shape() > 0.50: # should be 0.60 cluster_intensity = clustr.get_intensity() @@ -43,22 +43,22 @@ def main(): ) list_of_stars = sorted(list_of_stars, key=getkey, reverse=True) - print "***Stars***" - print "" + print("***Stars***") + print("") for star in list_of_stars: - print "*****" - print "Center:" - print star[1] - print "" - print "Direction Angle:" - print star[2] - print "" - print "Distance Angle:" - print star[3] - print "" - print "Cluster:" + print("*****") + print("Center:") + print(star[1]) + print("") + print("Direction Angle:") + print(star[2]) + print("") + print("Distance Angle:") + print(star[3]) + print("") + print("Cluster:") star[4].show() - print "_____" + print("_____") if __name__ == "__main__": diff --git a/experimental/cosmic-gps/thresh_global.py b/experimental/cosmic-gps/thresh_global.py index a3ad0521..8ed83c53 100644 --- a/experimental/cosmic-gps/thresh_global.py +++ b/experimental/cosmic-gps/thresh_global.py @@ -9,9 +9,6 @@ # method not optimized # # *********************# -import numpy as np -import cv2 as cv - def thresh_global(img, thresh_value=25): img_dimension = img.shape diff --git a/experimental/cosmic-gps/thresh_local_ratio.py b/experimental/cosmic-gps/thresh_local_ratio.py index 086f3f90..3524493b 100644 --- a/experimental/cosmic-gps/thresh_local_ratio.py +++ b/experimental/cosmic-gps/thresh_local_ratio.py @@ -7,7 +7,6 @@ import numpy as np -import cv2 as cv def thresh_local_ratio(img, window_size=9): diff --git a/experimental/cosmic-gps/thresh_tests.py b/experimental/cosmic-gps/thresh_tests.py index 589aec4f..23c75299 100644 --- a/experimental/cosmic-gps/thresh_tests.py +++ b/experimental/cosmic-gps/thresh_tests.py @@ -10,13 +10,11 @@ import numpy as np import cv2 as cv -from thresh_local_ratio import thresh_local_ratio from thresh_global import thresh_global def main(): - window_size = 9 thresh_value = 50 img = cv.imread("d_03_12_20_t_22_16_15.jpg", 1) @@ -29,7 +27,7 @@ def main(): thresh_dict = thresh_global(img, thresh_value) thresh_array = convert_dict_to_array(thresh_dict, rows, cols) - print thresh_dict + print(thresh_array) def convert_dict_to_array(thresh_dict, rows, cols): diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py index 6d0448f6..49442320 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/ai_objects.py @@ -4,10 +4,7 @@ import nav_functions as nf import math from random import uniform -from std_msgs.msg import String, Float32, Bool -from nav_msgs.msg import Odometry -from gazebo_msgs.msg import LinkStates -from sensor_msgs.msg import JointState +from std_msgs.msg import Float32, Bool from geometry_msgs.msg import Point, Twist diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py index ba637c7b..37316dc0 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/arm_force.py @@ -1,10 +1,5 @@ #!/usr/bin/env python -import rospy -from ai_objects import WorldState, ROSUtility -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64, Int8 - EPSILON = 0.05 diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py index 22699eff..568be06f 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/auto_functions.py @@ -205,7 +205,7 @@ def auto_dock(world_state, ros_util): def auto_dump(world_state, ros_util, duration): """ Rotate both drums inward and drive forward - for duration time in seconds. + for duration time in seconds. """ rospy.loginfo("Auto-dumping drum contents...") diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py index 63ff4a43..bc00a739 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/autonomous_control.py @@ -1,7 +1,6 @@ import rospy -import sys -from std_msgs.msg import Int8, Int16, String +from std_msgs.msg import Int8 from geometry_msgs.msg import Point from nav_msgs.msg import Odometry from gazebo_msgs.msg import LinkStates diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py index f22d2625..e9d359c8 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/nav_functions.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -import rospy import math from tf import transformations diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py index 98a6bf9f..30264bde 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/obstacle_detection.py @@ -142,7 +142,8 @@ def point_cloud_to_laser_scan(self): slope_ranges[step] = direction[index_slope, 1] # Combine above laserscans - min_ranges[step] = np.nanmin((hike_ranges[step], slope_ranges[step])) + min_ranges[step] = np.nanmin( + (hike_ranges[step], slope_ranges[step])) self.hike_pub.publish(self.create_laser_scan(hike_ranges)) self.slope_pub.publish(self.create_laser_scan(slope_ranges)) @@ -173,10 +174,11 @@ def obstacle_detection( range_min=0.105, range_max=10.0, ): - + """ Usage: od = ObstacleDetector( max_angle, max_obstacle_dist, min_hole_diameter, scan_time, range_min, range_max ) + """ rospy.spin() @@ -184,5 +186,5 @@ def obstacle_detection( if __name__ == "__main__": try: obstacle_detection() - except: + except (AttributeError, ValueError): pass diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py index 6c79329c..b2b6992e 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/park_ranger.py @@ -99,7 +99,7 @@ def sim_state_callback(self, data): namespace = namespace[1:-1] + "::base_link" try: index = data.name.index(namespace) - except: + except (AttributeError, ValueError): rospy.logdebug("Failed to get index. Skipping...") self.position_z = data.pose[index].position.z + self.origin_z @@ -327,10 +327,10 @@ def get_line(center_row, center_col, num_cells, angle, direction): # Handle cases where tangent returns the same value for different angles # and messes up the rest of the calculations - if direction is "right" and 135 < angle <= 315: + if direction == "right" and 135 < angle <= 315: direction = "left" angle = 180 - angle - if direction is "left" and 45 < angle <= 225: + if direction == "left" and 45 < angle <= 225: direction = "right" angle = 180 - angle @@ -359,9 +359,9 @@ def get_line(center_row, center_col, num_cells, angle, direction): D -= 2 D += 2 * abs(slope) if line_high: - row += -1 if direction is "right" else 1 + row += -1 if direction == "right" else 1 else: - col += 1 if direction is "right" else -1 + col += 1 if direction == "right" else -1 return np.array(indexes) @@ -525,7 +525,6 @@ def __init__( def run(self, period, local_dem_comparison_type): r = rospy.Rate(1.0 / period) init_flag = True - ran_out_of_particles = False self.particle_filter = ParticleFilter(self.resolution, self.dem_size) while not rospy.is_shutdown(): if not self.arms_up: @@ -589,9 +588,7 @@ def likelihood(self, local_dem): weights = np.zeros_like(self.global_dem) for p in self.particle_filter.particles: - old_weight = p.weight predicted_dem = self.get_predicted_local_dem(p) - valid_dem = False if predicted_dem is not None: p.weight *= ParkRanger.histogram_match(local_dem, predicted_dem) weights[p.y, p.x] = p.weight @@ -629,7 +626,6 @@ def predict_particle_movement(self): def estimate(self): sum_x = 0 sum_y = 0 - num_particles = len(self.particle_filter.particles) sum_weights = sum([p.weight for p in self.particle_filter.particles]) for p in self.particle_filter.particles: if p.weight != 0.0: @@ -663,6 +659,8 @@ def park_ranger( range_max=10.0, camera_height=0.34, ): + + """ Usage: pr = ParkRanger( real_odometry, world_name, @@ -673,10 +671,11 @@ def park_ranger( range_max, camera_height, ) + """ if __name__ == "__main__": try: park_ranger() - except: + except(AttributeError, ValueError, IndexError): pass diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py index e70926b5..f7b3f2ac 100755 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/utility_functions.py @@ -1,11 +1,9 @@ #!/usr/bin/env python import rospy -import time import math from geometry_msgs.msg import Twist -from std_msgs.msg import Float32 from geometry_msgs.msg import Pose import nav_functions as nf @@ -54,7 +52,7 @@ def set_back_arm_angle(world_state, ros_util, target_angle): def self_check(world_state, ros_util): - """ Check for unfavorable states in the system + """ Check for unfavorable states in the system and handle or quit gracefully. """ if ros_util.auto_function_command == 32 or ros_util.auto_function_command == 0: @@ -114,7 +112,7 @@ def turn(new_heading, direction, world_state, ros_util): # Cap our minimum velocity at 1/10 our max velocity. turn_velocity = max(turn_velocity, ros_util.max_angular_velocity / 10) - if direction is "right": + if direction == "right": turn_velocity *= -1 twist_message = Twist() @@ -176,7 +174,7 @@ def move(dist, world_state, ros_util, direction="forward"): # Cap our minimum velocity at 1/10 our max velocity. move_velocity = max(move_velocity, ros_util.max_linear_velocity / 10) - if direction is "backward": + if direction == "backward": move_velocity *= -1 twist_message = Twist() @@ -251,7 +249,7 @@ def self_right_from_side(world_state, ros_util): """ Flip EZ-RASSOR over from its side. """ rospy.loginfo("Starting auto self-right...") - while world_state.on_side != False: + while world_state.on_side is not False: ros_util.publish_actions("stop", 0, 1, 0, 0) ros_util.publish_actions("stop", 1, 0, 0, 0) diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py index 7978e7ea..d7d1f3fb 100644 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py @@ -46,7 +46,7 @@ def get_rover_status(rover_num): return response except rospy.ServiceException as e: - print "Service call failed: %s" % e + print("Service call failed: {}".format(e)) def preempt_rover_path(rover_num): @@ -69,4 +69,4 @@ def preempt_rover_path(rover_num): return response except rospy.ServiceException as e: - print "Service call failed: %s" % e + print("Service call failed: {}".format(e)) diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py index dfcc01ca..63fed232 100755 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/waypoint_client.py @@ -7,8 +7,7 @@ from path_planner import PathPlanner from swarm_utils import euclidean2D -from ezrassor_swarm_control.msg import * -from geometry_msgs.msg import Point +from ezrassor_swarm_control.msg import waypointAction, waypointGoal from ezrassor_swarm_control.msg import Path from ezrassor_swarm_control.srv import PreemptPath, PreemptPathResponse diff --git a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py index 3999a289..913d280b 100644 --- a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py +++ b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/controller_server.py @@ -5,7 +5,6 @@ Written by Tiger Sachse. Inspired by Camilo Lozano. """ -import time import json import rospy import threading diff --git a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py index 0f7a1b65..70aefcdb 100644 --- a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py +++ b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/joy_translator.py @@ -147,7 +147,7 @@ def start_node(): pub_auto_toggles = rospy.Publisher( publish_topic_auto_toggles, Int8, queue_size=10 ) - rate = rospy.Rate(60) + rospy.Rate(60) rospy.Subscriber( TOPIC, Joy, diff --git a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py index d46ff198..1bf39a0f 100755 --- a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py +++ b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/topic_switch.py @@ -12,7 +12,7 @@ class OverrideStatus: def __init__(self, override_topic): """Initialize this override status with a subscriber. - + This class possibly doesn't need the lock, but I left it just in case. """ self.__status = False diff --git a/packages/communication/ezrassor_topic_switch/test/test_topic_switch b/packages/communication/ezrassor_topic_switch/test/test_topic_switch index 1ebed2af..7863d2cb 100755 --- a/packages/communication/ezrassor_topic_switch/test/test_topic_switch +++ b/packages/communication/ezrassor_topic_switch/test/test_topic_switch @@ -30,7 +30,7 @@ class TestTopicSwitch(unittest.TestCase): """Initialize this test suite with several publishers and other variables.""" super(TestTopicSwitch, self).__init__(*arguments, **keyword_arguments) rospy.init_node(DEFAULT_TEST_NAME) - + # Read in all necessary ROS parameters. primary_topic = rospy.get_param( rospy.get_name() + "/primary_topic", diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py index 084f9e61..e4944df6 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_arms_driver.py @@ -3,7 +3,6 @@ Written by Harrison Black. """ import rospy -import std_msgs from std_msgs.msg import Float32, Float64 NODE = "sim_arms_driver" @@ -19,14 +18,14 @@ def handle_front_arm_movements(data): - """Move the front arm of the robot per - the commands encoded in the instruction. + """Move the front arm of the robot per + the commands encoded in the instruction. """ pub_FA.publish(data.data * MAX_ARM_SPEED) def handle_back_arm_movements(data): - """Move the back arm of the robot per + """Move the back arm of the robot per the commands encoded in the instruction. """ pub_BA.publish(data.data * MAX_ARM_SPEED) diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py index 36be475f..dd6bcba9 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_drums_driver.py @@ -5,7 +5,6 @@ Written by Harrison Black. """ import rospy -import std_msgs from std_msgs.msg import Float32, Float64 NODE = "sim_drums_driver" @@ -23,15 +22,15 @@ def handle_front_drum_movements(data): - """Move the front drum of the robot per - the commands encoded in the instruction. + """Move the front drum of the robot per + the commands encoded in the instruction. """ pub_FA.publish(data.data * MAX_DRUM_SPEED) def handle_back_drum_movements(data): - """Move the back drum of the robot per - the commands encoded in the instruction. + """Move the back drum of the robot per + the commands encoded in the instruction. """ pub_BA.publish(data.data * MAX_DRUM_SPEED) diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py index 74d9691e..d5f817ee 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/sim_wheels_driver.py @@ -5,7 +5,6 @@ Written by Harrison Black and Shelby Basco. """ import rospy -from std_msgs.msg import Int16, Float64 from geometry_msgs.msg import Twist NODE = "sim_wheels_driver" From 5300da42adc44a77efb7667835aee7a8095b64b1 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 14:57:45 -0400 Subject: [PATCH 12/17] Removing imports from __init__.py --- .github/workflows/dev.yml | 2 +- develop.sh | 2 +- .../source/ezrassor_autonomous_control/__init__.py | 7 ------- .../source/ezrassor_swarm_control/__init__.py | 3 --- .../source/ezrassor_swarm_control/swarm_control.py | 1 - .../source/ezrassor_swarm_control/swarm_utils.py | 2 -- .../source/ezrassor_controller_server/__init__.py | 1 - .../source/ezrassor_joy_translator/__init__.py | 1 - .../source/ezrassor_topic_switch/__init__.py | 1 - .../source/ezrassor_sim_control/__init__.py | 3 --- 10 files changed, 2 insertions(+), 21 deletions(-) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index 2439e431..d2c60769 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -28,4 +28,4 @@ jobs: - uses: actions/checkout@v2 - uses: TrueBrain/actions-flake8@master with: - ignore: E501, W503 + ignore: "E501, W503" diff --git a/develop.sh b/develop.sh index e7281045..94f54ec7 100755 --- a/develop.sh +++ b/develop.sh @@ -4,7 +4,7 @@ USER_SHELLS="bash zsh" PACKAGES_DIR="packages" -WORKSPACE_DIR="$HOME/ez-rassor_ws" +WORKSPACE_DIR="$HOME/ezrassor_ws" WORKSPACE_SOURCE_DIR="$WORKSPACE_DIR/src" WORKSPACE_DEVEL_DIR="$WORKSPACE_DIR/devel" PACKAGE_XML_FILE="package.xml" diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py index f5a827f1..e69de29b 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py @@ -1,7 +0,0 @@ -import autonomous_control -import ai_objects -import nav_functions -import utility_functions -import obstacle_detection -import auto_functions -import park_ranger diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py index f7e64ad0..e69de29b 100755 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py @@ -1,3 +0,0 @@ -import swarm_control -import path_planner -import waypoint_client diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py index 8e15a5f2..32489ac8 100755 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_control.py @@ -1,5 +1,4 @@ import rospy -from std_msgs.msg import Int8 from geometry_msgs.msg import Point from constants import commands diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py index d7d1f3fb..7158e76a 100644 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/swarm_utils.py @@ -4,8 +4,6 @@ import math import numpy as np -from geometry_msgs.msg import Point - from ezrassor_swarm_control.srv import GetRoverStatus, PreemptPath diff --git a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py index 896482d4..59c03619 100644 --- a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py +++ b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py @@ -2,4 +2,3 @@ Written by Tiger Sachse. """ -import controller_server diff --git a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py index 9df92bff..59c03619 100644 --- a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py +++ b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py @@ -2,4 +2,3 @@ Written by Tiger Sachse. """ -import joy_translator diff --git a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py index 9238ab54..59c03619 100755 --- a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py +++ b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py @@ -2,4 +2,3 @@ Written by Tiger Sachse. """ -import topic_switch diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py index 8831a1cb..e69de29b 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py @@ -1,3 +0,0 @@ -import sim_arms_driver -import sim_drums_driver -import sim_wheels_driver From aeecf7b4af91b4382fee95fbb406a79a1425c056 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 15:08:47 -0400 Subject: [PATCH 13/17] Updating flake ignore --- .github/workflows/dev.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index d2c60769..9f3f00de 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -28,4 +28,4 @@ jobs: - uses: actions/checkout@v2 - uses: TrueBrain/actions-flake8@master with: - ignore: "E501, W503" + ignore: E501,W503 From 0c3b19ba8088f6143b9a3dcb8f1a7d474f5f3a90 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 15:17:12 -0400 Subject: [PATCH 14/17] Updating branches for ci --- .github/workflows/dev.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index 9f3f00de..a6607c4a 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -3,7 +3,8 @@ name: dev on: push: branches: - - github-actions + - dev + - staging jobs: From d789d8aaf4d910267a2a679545b5f2952b4f1448 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 16:33:05 -0400 Subject: [PATCH 15/17] Restoring imports; adding flake8 ignore for __init__.py --- .../source/ezrassor_autonomous_control/__init__.py | 9 +++++++++ .../source/ezrassor_swarm_control/__init__.py | 5 +++++ .../source/ezrassor_controller_server/__init__.py | 5 ++++- .../source/ezrassor_joy_translator/__init__.py | 3 +++ .../source/ezrassor_topic_switch/__init__.py | 3 +++ .../source/ezrassor_sim_control/__init__.py | 5 +++++ 6 files changed, 29 insertions(+), 1 deletion(-) diff --git a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py index e69de29b..3e93a07d 100644 --- a/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py +++ b/packages/autonomy/ezrassor_autonomous_control/source/ezrassor_autonomous_control/__init__.py @@ -0,0 +1,9 @@ +# flake8: noqa + +import autonomous_control +import ai_objects +import nav_functions +import utility_functions +import obstacle_detection +import auto_functions +import park_ranger \ No newline at end of file diff --git a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py index e69de29b..e9a9ee40 100755 --- a/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py +++ b/packages/autonomy/ezrassor_swarm_control/source/ezrassor_swarm_control/__init__.py @@ -0,0 +1,5 @@ +# flake8: noqa + +import swarm_control +import path_planner +import waypoint_client \ No newline at end of file diff --git a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py index 59c03619..74fc7d9b 100644 --- a/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py +++ b/packages/communication/ezrassor_controller_server/source/ezrassor_controller_server/__init__.py @@ -1,4 +1,7 @@ +# flake8: noqa + """Initialize this directory as a Python module. Written by Tiger Sachse. -""" +""" +import controller_server \ No newline at end of file diff --git a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py index 59c03619..449cf15f 100644 --- a/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py +++ b/packages/communication/ezrassor_joy_translator/source/ezrassor_joy_translator/__init__.py @@ -1,4 +1,7 @@ +# flake8: noqa + """Initialize this directory as a Python module. Written by Tiger Sachse. """ +import joy_translator \ No newline at end of file diff --git a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py index 59c03619..37a37f59 100755 --- a/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py +++ b/packages/communication/ezrassor_topic_switch/source/ezrassor_topic_switch/__init__.py @@ -1,4 +1,7 @@ +# flake8: noqa + """Initialize this directory as a Python module. Written by Tiger Sachse. """ +import topic_switch \ No newline at end of file diff --git a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py index e69de29b..9f9d9768 100755 --- a/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py +++ b/packages/simulation/ezrassor_sim_control/source/ezrassor_sim_control/__init__.py @@ -0,0 +1,5 @@ +# flake8: noqa + +import sim_arms_driver +import sim_drums_driver +import sim_wheels_driver From 83f0ef61df3f2958f0849ecf7f1ef8d2b1f75105 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 16:35:26 -0400 Subject: [PATCH 16/17] Updating actions for pull requests --- .github/workflows/dev.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/dev.yml b/.github/workflows/dev.yml index a6607c4a..f2de33c0 100644 --- a/.github/workflows/dev.yml +++ b/.github/workflows/dev.yml @@ -5,6 +5,10 @@ on: branches: - dev - staging + pull_request: + branches: + - dev + - staging jobs: From 0ed732628385a758e0f91a345e55511108ee8eb2 Mon Sep 17 00:00:00 2001 From: CSharpRon Date: Thu, 23 Jul 2020 21:58:29 -0400 Subject: [PATCH 17/17] Adding additional changes from review comments --- dem_scripts/auto_gen_wrld/model_create.py | 19 +++++-------------- develop.sh | 14 +++++--------- docs/CONTRIBUTING.rst | 3 ++- docs/README.rst | 4 ++-- .../test/test_controller_server | 10 +++++----- 5 files changed, 19 insertions(+), 31 deletions(-) diff --git a/dem_scripts/auto_gen_wrld/model_create.py b/dem_scripts/auto_gen_wrld/model_create.py index 3799477d..59aa934c 100755 --- a/dem_scripts/auto_gen_wrld/model_create.py +++ b/dem_scripts/auto_gen_wrld/model_create.py @@ -2,10 +2,9 @@ from lxml import etree import sys -""" Given a uri tag, modify model_name and dem_name parts of the path """ - def replace_ref(tag, model_name, dem_name): + """Given a uri tag, modify model_name and dem_name parts of the path.""" first = True tag_text = tag.text tag_text = tag_text.split("/") @@ -22,10 +21,8 @@ def replace_ref(tag, model_name, dem_name): tag.text = new_tag_text -""" Given a uri tag, modify model_name part of the path """ - - def replace_diff_mod(tag, model_name): + """Given a uri tag, modify model_name part of the path.""" first = True tag_text = tag.text tag_text = tag_text.split("/") @@ -42,24 +39,18 @@ def replace_diff_mod(tag, model_name): tag.text = new_tag_text -""" Update a tag's name attribute with model_name """ - - def replace_top_model_name(tag, model_name): + """Update a tag's name attribute with model_name.""" tag.attrib["name"] = model_name -""" Update a tag's text field with string """ - - def replace_text_field(tag, string): + """Update a tag's text field with string.""" tag.text = string -""" Fill in model.sdf template """ - - def model_trav(path_to_file, model_name, dem_name, w, h, squish_factor): + """Fill in model.sdf template.""" tree = etree.parse(path_to_file) diff --git a/develop.sh b/develop.sh index 94f54ec7..3e256e0d 100755 --- a/develop.sh +++ b/develop.sh @@ -46,7 +46,7 @@ source_setups_in_directory() { # Set up the development environment. setup_environment() { - # Setup will run fresh every time + # Setup will run fresh every time. rm -r -f "$WORKSPACE_DIR" mkdir -p "$WORKSPACE_SOURCE_DIR" @@ -182,20 +182,16 @@ kill_ros() { test_packages() { cd "$WORKSPACE_DIR" - # This command will run the tests but return a status of 0 + # This command will run the tests but return a status of 0. catkin_make run_tests - # This command will return a status code of 0 or 1 depending on if the previous tests succeeded + # This command will return a status code of 0 or 1 depending on if the previous tests succeeded. (catkin_test_results) local result=$? - # After we return to the main directory, return the status code from the test results + # After we return to the main directory, return the status code from the test results. cd - > /dev/null 2>&1 - if [ $result -ne 0 ]; then - return 1 - else - return 0 - fi + [ $result -eq 0 ] } # Change the version number of all the packages. diff --git a/docs/CONTRIBUTING.rst b/docs/CONTRIBUTING.rst index aafa0489..31a9b34f 100644 --- a/docs/CONTRIBUTING.rst +++ b/docs/CONTRIBUTING.rst @@ -4,7 +4,7 @@ This project is free and open-source under the `MIT license`_. Anyone can fork t DEVELOPMENT INSTRUCTIONS ---- -Before you begin developing, you must first install ROS and the ROS build tools. Clone the repository with the following command: +Before you begin developing, you must first install ROS and the ROS build tools (these instructions are in the `readme`_). Clone the repository with the following command: :: git clone https://github.com/FlaSpaceInst/EZ-RASSOR.git cd EZ-RASSOR @@ -75,3 +75,4 @@ Here are some example commands to get started. .. _`authors`: https://github.com/FlaSpaceInst/NASA-E-RASSOR-Team/blob/master/docs/README.rst#authors .. _`README`: README.rst#INSTALLATION .. _`the script`: ../develop.sh +.. _`readme`: https://github.com/FlaSpaceInst/EZ-RASSOR/#installation-prerequisites diff --git a/docs/README.rst b/docs/README.rst index e20ff3fe..f2b8c685 100644 --- a/docs/README.rst +++ b/docs/README.rst @@ -58,7 +58,7 @@ Make sure you have already run the setup command at least once: sh develop.sh setup -Then, you can call the relink function and use -o to pass in the package name(s) you would like to install +Then, you can call the relink function and use ``-o`` to pass in the package name(s) you would like to install: .. code-block:: bash @@ -66,7 +66,7 @@ Then, you can call the relink function and use -o to pass in the package name(s) sh develop.sh build sh develop.sh install -Alternatively, you can also call the relink function and use the -e flag to make the script install all *but* the specified package(s): +Alternatively, you can also call the relink function and use the ``-e`` flag to make the script install all *but* the specified package(s): .. code-block:: bash diff --git a/packages/communication/ezrassor_controller_server/test/test_controller_server b/packages/communication/ezrassor_controller_server/test/test_controller_server index 3244a4ca..b09eb009 100755 --- a/packages/communication/ezrassor_controller_server/test/test_controller_server +++ b/packages/communication/ezrassor_controller_server/test/test_controller_server @@ -24,7 +24,7 @@ PACKAGE_NAME = "ezrassor_controller_server" DEFAULT_TEST_NAME = "test_controller_server" AUTONOMOUS_TOGGLES_TOPIC = "autonomous_toggles" TARGET_COORDINATES_TOPIC = "target_coordinates" -POST_HEADER = {"content-type" : "application/json"} +POST_HEADER = {"content-type": "application/json"} WHEEL_INSTRUCTION_STRINGS = ("forward", "backward", "left", "right") @@ -54,7 +54,7 @@ class Instruction(): def is_none(self): """Check to see if this instruction's content is None.""" self.__content_lock.acquire() - is_none = self.__content == None + is_none = self.__content is None self.__content_lock.release() return is_none @@ -177,7 +177,7 @@ class TestControllerServer(unittest.TestCase): for instruction in FLOAT_INSTRUCTIONS: for iteration in range(ITERATION_RANGE): speed = self.__get_random_speed() - self.__post_and_sleep({instruction.name : speed}) + self.__post_and_sleep({instruction.name: speed}) self.assertFalse(instruction.is_none()) self.assertAlmostEqual(instruction.get().data, speed, places=4) @@ -197,8 +197,8 @@ class TestControllerServer(unittest.TestCase): AUTONOMOUS_TOGGLES_INTEGERS, ) instruction["target_coordinate"] = { - "x" : random.randrange(COORDINATE_MIN, COORDINATE_MAX), - "y" : random.randrange(COORDINATE_MIN, COORDINATE_MAX), + "x": random.randrange(COORDINATE_MIN, COORDINATE_MAX), + "y": random.randrange(COORDINATE_MIN, COORDINATE_MAX), } # Randomly remove one key so that the complex instruction is not