diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml index cdbb846794..c8f5361792 100644 --- a/.github/workflows/ci_pr.yml +++ b/.github/workflows/ci_pr.yml @@ -7,38 +7,6 @@ on: jobs: - build-xenial: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - - name: Checkout submodule - run: git submodule update --init --depth 1 description/media - - - name: Build, test - run: ./scripts/docker/build.sh --xenial --remote - astrobee test_astrobee - - build-bionic: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - - name: Checkout submodule - run: git submodule update --init --depth 1 description/media - - - name: Build, test - run: ./scripts/docker/build.sh --bionic --remote - astrobee test_astrobee - build-focal: runs-on: ubuntu-20.04 diff --git a/.github/workflows/ci_push.yml b/.github/workflows/ci_push.yml index 8d0314f897..f6ed14c486 100644 --- a/.github/workflows/ci_push.yml +++ b/.github/workflows/ci_push.yml @@ -8,78 +8,6 @@ on: jobs: - build-xenial: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - - - name: Checkout submodule - run: git submodule update --init --depth 1 description/media - - - name: Log in to registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin - - - name: Build, test, push docker - run: > - revision="latest"; - build_arg="astrobee"; - push_arg=""; - branch=${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}; - if [ "${{ github.repository_owner }}" == "nasa" ]; then - if [ $branch == "develop" ]; then - push_arg="push_astrobee"; - fi; - if [ $branch == "master" ]; then - export VERSION=`grep -w -m 1 "Release" RELEASE.md | awk '{print $3}'`; - revision="v${VERSION}"; - build_arg="astrobee_base astrobee"; - push_arg="push_astrobee_base push_astrobee"; - fi; - fi; - command="./scripts/docker/build.sh --xenial --remote --revision ${revision} \ - --owner ${{ github.repository_owner }} \ - ${build_arg} test_astrobee ${push_arg}"; - echo "$command"; - eval "$command"; - - build-bionic: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - - - name: Checkout submodule - run: git submodule update --init --depth 1 description/media - - - name: Log in to registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin - - - name: Build, test, push docker - run: > - revision="latest"; - build_arg="astrobee"; - push_arg=""; - branch=${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}; - if [ "${{ github.repository_owner }}" == "nasa" ]; then - if [ $branch == "develop" ]; then - push_arg="push_astrobee"; - fi; - if [ $branch == "master" ]; then - export VERSION=`grep -w -m 1 "Release" RELEASE.md | awk '{print $3}'`; - revision="v${VERSION}"; - build_arg="astrobee_base astrobee"; - push_arg="push_astrobee_base push_astrobee"; - fi; - fi; - command="./scripts/docker/build.sh --bionic --remote --revision ${revision} \ - --owner ${{ github.repository_owner }} \ - ${build_arg} test_astrobee ${push_arg}"; - echo "$command"; - eval "$command"; - build-focal: runs-on: ubuntu-20.04 diff --git a/.github/workflows/docker_push_latest_base.yml b/.github/workflows/docker_push_latest_base.yml index 21d54b6e4c..a178bfa702 100644 --- a/.github/workflows/docker_push_latest_base.yml +++ b/.github/workflows/docker_push_latest_base.yml @@ -8,42 +8,6 @@ on: ['workflow_dispatch'] jobs: - build-xenial: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - - - name: Checkout submodule - run: git submodule update --init --depth 1 description/media - - - name: Log in to registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin - - - name: Build base, build, test, push base docker - run: ./scripts/docker/build.sh --xenial - --owner ${{ github.repository_owner }} - astrobee_base astrobee test_astrobee push_astrobee_base - - build-bionic: - - runs-on: ubuntu-20.04 - - steps: - - uses: actions/checkout@v3 - - - name: Checkout submodule - run: git submodule update --init --depth 1 description/media - - - name: Log in to registry - run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin - - - name: Build base, build, test, push base docker - run: ./scripts/docker/build.sh --bionic - --owner ${{ github.repository_owner }} - astrobee_base astrobee test_astrobee push_astrobee_base - build-focal: runs-on: ubuntu-20.04 diff --git a/.github/workflows/msgs_jar.yaml b/.github/workflows/msgs_jar.yaml index d090339dba..ba9cdc8694 100644 --- a/.github/workflows/msgs_jar.yaml +++ b/.github/workflows/msgs_jar.yaml @@ -11,19 +11,12 @@ jobs: steps: - uses: actions/checkout@v3 - - name: Build image astrobee/astrobee:msgs-ubuntu16.04 + - name: Build image astrobee/astrobee:msgs-ubuntu20.04 run: docker build . -f ./scripts/docker/astrobee_msgs.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - -t astrobee/astrobee:msgs-ubuntu16.04 + -t astrobee/astrobee:msgs-ubuntu20.04 - - name: Build image astrobee/astrobee:latest-msgs-jar-ubuntu16.04 + - name: Build image astrobee/astrobee:latest-msgs-jar-ubuntu20.04 run: docker build . -f ./scripts/docker/build_msgs_jar.Dockerfile - --build-arg UBUNTU_VERSION=16.04 - --build-arg ROS_VERSION=kinetic - --build-arg PYTHON='' - --build-arg REPO=astrobee -t ghcr.io/${{ github.repository_owner }}/astrobee:latest-msgs-jar - name: Copy jar files diff --git a/INSTALL.md b/INSTALL.md index 6787eb800a..61f3a99066 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -2,13 +2,15 @@ ## System requirements -Ubuntu 20.04 is the preferred host OS for most Astrobee developers to use. +Ubuntu 20.04 is currently the only supported platform. -Here are the available host OS options with development roadmap details (use 64-bit PC (AMD64) desktop image): -- [Ubuntu 20.04](http://releases.ubuntu.com/20.04): This is the preferred host OS for most Astrobee developers to use. The Astrobee Facility team is currently preparing to upgrade the robots on ISS from Ubuntu 16.04 to Ubuntu 20.04, but we aren't yet ready to announce a deployment date for that upgrade. -- [Ubuntu 18.04](http://releases.ubuntu.com/18.04): We are not aware of any current robot users that still need Ubuntu 18.04 support, and expect to discontinue support in the near future. New users should not select this host OS. -- [Ubuntu 16.04](http://releases.ubuntu.com/16.04): The Astrobee robot hardware on ISS currently runs Ubuntu 16.04. Only developers with NASA internal access can cross-compile software to run on the robot, and must use 16.04 for that. Most developers shouldn't need to work with 16.04, especially when just getting started. Support will eventually be discontinued after the robot hardware on ISS is upgraded to Ubuntu 20.04. -(Ubuntu 22.04 not supported) +Here are the currently available host OS options with development roadmap details (use 64-bit PC (AMD64) desktop image): +- [Ubuntu 20.04](http://releases.ubuntu.com/20.04): This is currently the only supported platform. The Astrobee hardware on ISS has been running Ubuntu 20.04 since it was upgraded during the "Crew-Minimal S14" activity on December 19, 2023. + +Specifically not supported: +- ~~[Ubuntu 16.04](http://releases.ubuntu.com/16.04)~~: No longer supported. The Astrobee robot hardware ran Ubuntu 16.04 from its launch in 2019 until it was upgraded to run Ubuntu 20.04. Ubuntu 16.04 support was discontinued in February 2024. Ending Ubuntu 16.04 support removed important limitations. For example, going forward, Astrobee's software will no longer need to be backward-compatible with Python 2 and OpenCV 3. +- ~~[Ubuntu 18.04](http://releases.ubuntu.com/18.04)~~: Ubuntu 18.04 support as a software development platform was discontinued as of November 2023. (It was never supported on the robot hardware.) +- ~~[Ubuntu 22.04](http://releases.ubuntu.com/22.04)~~: There is currently no plan for Ubuntu 22.04 support on the Astrobee roadmap. However, note that Astrobee ROS2 support, when it eventually becomes available, is currently expected to use the ROS2 Humble Hawksbill distribution that normally runs on 22.04, but backported to run on 20.04 (the last Ubuntu version supported by ROS1). This will facilitate migrating from ROS1 to ROS2 without requiring a simultaneous Ubuntu distribution upgrade. Graphical interfaces will perform best if your host OS is running natively (not in a virtual machine). diff --git a/RELEASE.md b/RELEASE.md index 4fefde59e7..dea20defdf 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,5 +1,11 @@ # Releases +# Release 0.17.2 + + * Adding Generic comms bridge + * Multiple bug fixes + * Ubuntu 20 only supported now + # Release 0.17.1 * Multiple bug fixes diff --git a/astrobee.doxyfile b/astrobee.doxyfile index 6cc696edf4..7bb0ffe858 100644 --- a/astrobee.doxyfile +++ b/astrobee.doxyfile @@ -39,7 +39,7 @@ PROJECT_NAME = "NASA Astrobee Robot Software" # control system is used. -PROJECT_NUMBER = 0.17.1 +PROJECT_NUMBER = 0.17.2 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/astrobee/CMakeLists.txt b/astrobee/CMakeLists.txt index 7e4c3d7e02..e47ad4b0b5 100644 --- a/astrobee/CMakeLists.txt +++ b/astrobee/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(astrobee) -set(ASTROBEE_VERSION 0.17.1) +set(ASTROBEE_VERSION 0.17.2) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config new file mode 100644 index 0000000000..4662f05c56 --- /dev/null +++ b/astrobee/config/communications/comms_bridge.config @@ -0,0 +1,105 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +require "context" + +-- Enable DDS communications on start -- +-- If false, a trigger call will be needed to start communications -- +initialize_dds_on_start = false + +-- Request an advertisement info message if a content message was received +-- before the advertisement info message in case the advertisement message +-- never comes. +-- DDS has been setup such that this shouldn't be needed and it can cause more +-- information to be sent then needed. By default, it will be set to false +-- but if communications on the ISS are not working as expected, it can be +-- enabled. +enable_advertisement_info_request = false + +-- The verbosity is used to control the output of the underlying ros +-- publisher/subscriber. Level 2 is the most verbose and will output all of the +-- output including debug statements. Level 1 will output all output except +-- debug statements and level 0 will only output warnings and errors. Since +-- all output statements go directly to the ros log, it is safest to keep it +-- set to 2. +verbose = 2 + +-- ad2pub_delay is a time amount in seconds. The bridge publisher must receive +-- the adverttisement info message for a topic before it can publish a content +-- message for that topic. If the bridge publisher receives the content message +-- before the advertisement info, it will save the content message if the +-- ad2pub_delay is greater than 0. If the bridge publisher receives the +-- advertisement info message within the ad2pub_delay time, it will process +-- and publish the content message. If the comms bridge seems to be dropping +-- the first few messages, try increasing this value. +ad2pub_delay = 3.0 + +links = { + -- Logically, there could be up to three two-way links between the three robots. In practice, we + -- will probably only have one link. + { + -- A single link entry has required fields "from" and "to" that specify the robot roles involved + -- in the link. + -- This link will only work on the ISS since it uses the names of the + -- robots on the ISS. Please add more links if Queen goes back to the ISS. + from = "Bumble", -- manager + to = "Honey", -- actor + + -- The link entry has three optional fields: relay_forward (messages to be relayed only in the + -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), + -- and relay_both (to be relayed in both directions). Providing all three fields gives the user + -- full directional control while minimizing repetition and copy/paste errors. + + -- Each topic entry can contain an input topic and an output topic. The + -- in topic is the topic being published on the robot sending the data and + -- must be specified. The optional out topic is the name of the topic a + -- user wants the data published on in the receiving robot. If the + -- out topic is not specified, the comms bridge will set it to be the name + -- of the robot sending the data combined with the in topic name. For + -- instance, if the from robot was Bumble and the to robot was Honey and + -- one of the in topics in the relay forward list was "mgt/ack", then it + -- would be published on Honey on topic "bumble/mgt/ack". + -- Please note that only one unique in topic can exist in the relay forward + -- and relay both lists and the relay backward and relay both lists. It is + -- fine to have the same in topic in the relay forward and relay backward + -- lists. + + relay_forward = { + }, + relay_backward = { + }, + relay_both = { + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, + }, + }, + { + -- This link will only work in the granite lab since it uses the names of + -- the robots in the granite lab. + from = "Bsharp", -- manager + to = "Wannabee", -- actor + + relay_forward = { + }, + relay_backward = { + }, + relay_both = { + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, + } + } +} diff --git a/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS new file mode 100644 index 0000000000..dc71e38323 --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS @@ -0,0 +1,6 @@ +;; NDDS_DISCOVERY_PEERS - Discovery Configuration File +;; This file is used to configure the dds portion of the comms bridge +; EVERY LINE IN THIS FILE MUST END IN A COMMENT. Even blank lines. +; +;; Add the IP addresses of the robots to communicate with +127.0.0.1 ; Local host is added by default diff --git a/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml new file mode 100644 index 0000000000..27bb1ff25f --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml @@ -0,0 +1,994 @@ + + + + + + + + + + + + + + + 50, 48, 49, 49, 48, 54,0x32,0x39,0x2e,0x30,0x39,0x34,0x33,0x30,0x30,0x2e,0x48,0x55,0x2d,0x41,0x52,0x43 + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + MASK_DEFAULT + + + UDPv4 + + + + + + true + + + + 4096 + + + 0 + 0 + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController.scheduling_policy + DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.max_tokens + 12 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_added_per_period + 4 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_leaked_per_period + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.bytes_per_token + 1250 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.sec + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.nanosec + 5000000 + + + + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 1250 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65535 + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 65535 + + + + + dds.transport.shmem.builtin.received_message_count_max + 512 + + + dds.transport.shmem.builtin.receive_buffer_size + 1048576 + + + + + dds.transport.UDPv4.builtin.parent.deny_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + dds.transport.UDPv4.builtin.parent.deny_multicast_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + + + + + + + + + + 0 + + + + + + ALIVE_THEN_DISPOSED_INSTANCE_REPLACEMENT + + + ASYNCHRONOUS_PUBLISH_MODE_QOS + + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + KEEP_ALL_HISTORY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_ALL_HISTORY_QOS + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 20 + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + KEEP_LAST_HISTORY_QOS + 1 + + + 512 + 4 + 128 + 128 + 512 + 380 + + + + 32 + + 0 + 500000000 + + 8 + true + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + KEEP_LAST_HISTORY_QOS + 100 + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 1000 + + + + + KEEP_LAST_HISTORY_QOS + 200 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + 100 + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + + + + + + + 50 + + + + + + + + + + + + + + + + 1000 + + + + + + + + 1000 + + + + + + + + + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController + + + KEEP_LAST_HISTORY_QOS + 1 + + + 32 + 32 + 1 + 1 + 32 + + + + + 4096 + + + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + + + + + + + + + + + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + + + + 30000 + 64 + 2048 + + + + UDPv4 | SHMEM + + + Monitoring UI Application + + + 65530 + + + + + 100 + 0 + + + 10 + 0 + + LIVELINESS_BASED_REMOTE_PARTICIPANT_PURGE + + 10 + 0 + + 1 + + 2 + 0 + + + 2 + 0 + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + + + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 1048576 + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 65530 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65530 + + + + + dds.transport.shmem.builtin.parent.message_size_max + 65530 + + + dds.transport.shmem.builtin.receive_buffer_size + 65530 + + + dds.transport.shmem.builtin.received_message_count_max + 32 + + + + + + + + + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 2 + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + diff --git a/astrobee/config/management/cpu_mem_monitor.config b/astrobee/config/management/cpu_mem_monitor.config index 3bdae428a1..512be2f818 100644 --- a/astrobee/config/management/cpu_mem_monitor.config +++ b/astrobee/config/management/cpu_mem_monitor.config @@ -18,7 +18,6 @@ -- frequency to check and publish cpu stats llp = { - update_pid_hz = 0.1, update_freq_hz = 1, temperature_scale = 0.001, cpu_avg_load_limit = 100, @@ -40,7 +39,6 @@ llp = { } mlp = { - update_pid_hz = 0.1, update_freq_hz = 1, temperature_scale = 1.0, cpu_avg_load_limit = 100, diff --git a/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index d1f0f7cf1d..aab193af50 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -110,8 +110,8 @@ subsystems={ }}, }}, {name="communication", nodes={ - {name="astrobee_astrobee_bridge", faults={ - {id=51, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Atrobee to Astrobee Bridge", heartbeat={timeout_sec=1.1, misses=1.0}}, + {name="comms_bridge", faults={ + {id=51, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Comms Bridge", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="dds_ros_bridge", faults={ {id=33, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, diff --git a/astrobee/config/management/sys_monitor.config b/astrobee/config/management/sys_monitor.config index cb0cc55ec4..050fffb979 100644 --- a/astrobee/config/management/sys_monitor.config +++ b/astrobee/config/management/sys_monitor.config @@ -41,8 +41,8 @@ time_drift_thres_sec = 0.25 nodelet_info={ {name="access_control", manager="mlp_management", type="access_control/AccessControl"}, {name="arm", manager="mlp_arm", type="arm/ArmNodelet"}, - {name="astrobee_astrobee_bridge", manager="mlp_multibridge", type="dds_ros_bridge/AstrobeeAstrobeeBridge"}, {name="choreographer", manager="mlp_mobility", type="choreographer/ChoreographerNodelet"}, + {name="comms_bridge", manager="mlp_multibridge", type="comms_bridge/CommsBridgeNodelet"}, {name="ctl", manager="llp_gnc", type="ctl/ctl"}, {name="data_bagger", manager="mlp_recording", type="data_bagger/DataBagger"}, {name="dds_ros_bridge", manager="mlp_communications", type="dds_ros_bridge/DdsRosBridge"}, diff --git a/astrobee/config/robots/honey.config b/astrobee/config/robots/honey.config index fc8b814061..5c22798c94 100644 --- a/astrobee/config/robots/honey.config +++ b/astrobee/config/robots/honey.config @@ -46,14 +46,14 @@ robot_geometry = { robot_camera_calibrations = { nav_cam = { - distortion_coeff = 0.99872, + distortion_coeff = 0.998757, intrinsic_matrix = { - 607.64218, 0.0, 624.85552, - 0.0, 606.53899, 514.57737, + 610.12594, 0.0, 628.35064, + 0.0, 610.12594, 514.89592, 0.0, 0.0, 1.0 }, - gain=100, - exposure=150 + gain=50, + exposure=175 }, dock_cam = { distortion_coeff = 1.00882, diff --git a/astrobee/config/robots/sim_pub.config b/astrobee/config/robots/sim_pub.config index a0ad5be8a3..e47e2f5f6e 100644 --- a/astrobee/config/robots/sim_pub.config +++ b/astrobee/config/robots/sim_pub.config @@ -16,4 +16,4 @@ -- under the License. require "robots/sim" -nodes_not_running = {"dds_ros_bridge", "astrobee_astrobee_bridge", "light_flow"} +nodes_not_running = {"dds_ros_bridge", "comms_bridge", "light_flow"} diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 096cafee64..37b473170a 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -287,8 +287,8 @@ - - + + diff --git a/astrobee/launch/sim.launch b/astrobee/launch/sim.launch index 9701e3e1ca..414f0be244 100644 --- a/astrobee/launch/sim.launch +++ b/astrobee/launch/sim.launch @@ -165,6 +165,9 @@ + + + @@ -188,6 +191,9 @@ + + + @@ -211,6 +217,9 @@ + + + @@ -234,9 +243,10 @@ + + + - - diff --git a/astrobee/scripts/cpu_print_version.sh b/astrobee/scripts/cpu_print_version.sh index 190127cd6c..ed0a17a6a8 100755 --- a/astrobee/scripts/cpu_print_version.sh +++ b/astrobee/scripts/cpu_print_version.sh @@ -9,7 +9,7 @@ function deb_version { fi } -source /opt/ros/kinetic/setup.bash +source /opt/ros/noetic/setup.bash date echo "Kernel: $(uname -r)" lsb_release -r diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt new file mode 100644 index 0000000000..a9f6aaf995 --- /dev/null +++ b/communications/comms_bridge/CMakeLists.txt @@ -0,0 +1,141 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + + +cmake_minimum_required(VERSION 3.0) +project(comms_bridge) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +if (USE_DDS) +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + config_reader + ff_util + ff_msgs + topic_tools + dds_msgs +) + + +# System dependencies are found with CMake's conventions +# find_package(Eigen3 REQUIRED) +find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + +if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) +else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) +endif (USE_CTC) + +set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# find Qt version according to OS +find_program(LSB_RELEASE_EXEC lsb_release) +execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) +else () + find_package(Qt4 4.6.0 REQUIRED QtXml) +endif () + +find_package(Miro REQUIRED) +find_package(RtiDds REQUIRED) +find_package(Soracore REQUIRED) + +catkin_package( + LIBRARIES comms_bridge + CATKIN_DEPENDS message_runtime std_msgs nodelet config_reader ff_util ff_msgs dds_msgs +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR} + ${QT_INCLUDE_DIR} + ${QT_INCLUDE_DIR}/Qt + ${Boost_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cpp" +) + +# Declare C++ libraries +add_library(comms_bridge + ${cc_files} +) +target_compile_definitions(comms_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) +add_dependencies(comms_bridge ${catkin_EXPORTED_TARGETS}) +target_link_libraries(comms_bridge rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + + +#add_library(comms_bridge_pub +# src/bridge_publisher.cpp +# src/dds_ros_bridge_publisher.cpp +# src/bridge_publisher_nodelet.cpp +#) +#add_dependencies(comms_bridge_pub ${catkin_EXPORTED_TARGETS} ) +#target_link_libraries(comms_bridge_pub +# rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) + + +############# +## Install ## +############# +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) diff --git a/communications/comms_bridge/include/comms_bridge/bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h new file mode 100644 index 0000000000..d8a4202697 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h @@ -0,0 +1,138 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ +#define COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ + +/* + Virtual base class for the subscriber (input side) of a generic ROS bridge + Actual implementation depends on an inheriting class + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef std::shared_ptr SubscriberPtr; +typedef std::shared_ptr PublisherPtr; +typedef std::chrono::time_point timepoint_t; + +class BridgePublisher { + public: + virtual ~BridgePublisher(); + + // 0 => none, > 0 => progressively more + void setVerbosity(unsigned int verbosity); + + protected: + // a yet-unknown message type will have null string values + class AdvertisementInfo { + public: + // whehter republisher should advertise this as a latching ROS topic + bool latching; + + // ROS message data type name, eg "std_msgs/Int32" + std::string data_type; + + // ROS message md5sum of type, eg "da5909fbe378aeaf85e547e830cc1bb7" + std::string md5_sum; + + // ROS message definition, eg as one finds in a .msg definition file + std::string definition; + }; + + class ContentInfo { + public: + // the md5sum from the original advertisement + std::string type_md5_sum; + + // opaque serialized message data + std::vector data; + }; + + class RelayTopicInfo { + public: + RelayTopicInfo() : out_topic(""), advertised(false), relay_seqnum(0) {} + + ros::Publisher publisher; // our subscriber on this input topic + std::string out_topic; // topic name to republish on + bool advertised; // whether we have sent type info to other side + unsigned int relay_seqnum; // counter of messages relayed on this topic + + // Will be filled in only on receipt of advertisement from our + // bridger subscriber counterpart + AdvertisementInfo ad_info; + + std::queue + waiting_msgs; // unpublished messages because type yet unknown or we're still delaying after advertising + timepoint_t delay_until; // time to wait until relaying any data, after having advertised + }; + + /**************************************************************************/ + + // This base class should only be extended, not itself instantiated + explicit BridgePublisher(double ad2pub_delay); + + // Should be called by implementing class when it has received a topic's + // AdvertisementInfo + // Must be called with mutex held + bool advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info); + + // Should be called by an implementing class when it receives a serialized + // message over its particular conduit + // Must be called with mutex held + bool relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info); + + // worker thread spawned at construction + void drainThread(); + + // does actual work of publishing a message (eg called by relayMessage) + // Must be called with mutex held + bool relayContent(RelayTopicInfo& topic_info, const ContentInfo& content_info); + + // Publishes messages that were enqueued for a given topic + // Must be called with mutex held + void drainWaitingQueue(RelayTopicInfo& topic_info); + void drainWaitingQueue(std::string const& output_topic); + + unsigned int m_verbose_; + + double m_ad2pub_delay_; // seconds + + std::mutex m_mutex_; // serializes access to below data structures + std::shared_ptr worker_thread_; + std::map m_relay_topics_; // keyed by input topic + std::priority_queue> m_drain_queue_; + std::condition_variable m_drain_cv_; + // stats: + unsigned int m_n_relayed_; +}; + +#endif // COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h new file mode 100644 index 0000000000..158d85376a --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h @@ -0,0 +1,129 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ +#define COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ + +/* + Virtual base class for the subscriber (input side) of a generic ROS bridge + Actual implementation depends on an inheriting class + */ + +#include + +#include +#include + +#include +#include +#include +#include + +typedef std::shared_ptr SubscriberPtr; +typedef std::shared_ptr PublisherPtr; + +class BridgeSubscriber { + public: + BridgeSubscriber(); + virtual ~BridgeSubscriber(); + + // Add a relay between this input topic on the subscriber side and this + // output topic on the republisher side + // All relays must have a unique input topic and unique output topic + // Other topologies (merging topics or duplicating topics) should be + // implemented by some other node on top of this one + bool addTopic(std::string const& in_topic, std::string const& out_topic); + + // 0 => none, > 0 => progressively more + void setVerbosity(unsigned int verbosity); + + protected: + class AdvertisementInfo { + public: + // whehter republisher should advertise this as a latching ROS topic + bool latching; + + // ROS message data type name, eg "std_msgs/Int32" + std::string data_type; + + // ROS message md5sum of type, eg "da5909fbe378aeaf85e547e830cc1bb7" + std::string md5_sum; + + // ROS message definition, eg as one finds in a .msg definition file + std::string definition; + }; + + class RelayTopicInfo { + public: + RelayTopicInfo() : out_topic(""), advertised(false), relay_seqnum(0) {} + + SubscriberPtr sub; // our subscriber on this input topic + std::string out_topic; // topic name to republish on + bool advertised; // whether we have sent type info to other side + unsigned int relay_seqnum; // counter of messages relayed on this topic + + // not filled in until advertiseTopic() is called + AdvertisementInfo ad_info; + }; + + class ContentInfo { + public: + // the md5sum from the original advertisement + std::string type_md5_sum; + + // length in bytes of the serialized data + size_t data_size; + + // opaque serialized message data + const uint8_t* data; + }; + + // Called via addTopic() + // Notifies implementation that a bridge between these two topics is needed + // No information about the message type etc. is yet available + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) = 0; + + // Called upon receipt of the first message on a subscribed topic + // Notifies implementation to advertise the topic with this type info + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info) = 0; + + // Called on receipt of any message on a subscribed topic + // Notifies implementation to relay this message content to the output topic + // Note any pointers within info struct are valid only during this call + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) = 0; + + /**************************************************************************/ + + SubscriberPtr rosSubscribe(std::string const& topic); + + void handleRelayedMessage(const ros::MessageEvent& msg_event, + std::string const& topic, SubscriberPtr sub); + + unsigned int m_verbose_; + + std::mutex m_mutex_; // serializes access to below data structures + uint8_t* m_msgbuffer_; // data serialization scratch + std::map m_relay_topics_; // keyed by input topic + // stats: + unsigned int m_n_relayed_; +}; + +#endif // COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h new file mode 100644 index 0000000000..29df797e86 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ + +#include +#include +#include + +#include +#include +#include + +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +// default time to delay between advertisement and publishing on that topic [sec] +#define DEFAULT_ADVERTISE_TO_PUB_DELAY 3.0 + +namespace ff { + +class GenericRapidMsgRosPub : public BridgePublisher { + public: + explicit GenericRapidMsgRosPub(double ad2pub_delay = DEFAULT_ADVERTISE_TO_PUB_DELAY); + virtual ~GenericRapidMsgRosPub(); + + void InitializeDDS(std::vector const& connections, bool enable_advertisement_info_request); + + void ConvertAdvertisementInfo( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); + void ConvertContent(rapid::ext::astrobee::GenericCommsContent const* data, + std::string const& connecting_robot); + + void RequestAdvertisementInfo(std::string const& output_topic, + std::string const& connecting_robot); + + private: + bool dds_initialized_, enable_advertisement_info_request_; + + std::map robot_connections_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h new file mode 100644 index 0000000000..4aef42a757 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstantsSupport.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +namespace ff { + +class GenericRapidPub { + public: + explicit GenericRapidPub(std::string const& robot_name); + ~GenericRapidPub(); + + template + void CopyString(const int max_size, + std::string src, + T &dest, + std::string const& data_name, + std::string const& topic); + + void SendAdvertisementInfo(std::string const& output_topic, + bool latching, + std::string const& data_type, + std::string const& md5_sum, + std::string definition); + + void SendContent(std::string const& output_topic, + std::string const& md5_sum, + uint8_t const* data, + const size_t data_size, + const int seq_num); + + private: + using AdvertisementInfoSupplier = + kn::DdsTypedSupplier; + using AdvertisementInfoSupplierPtr = + std::unique_ptr; + AdvertisementInfoSupplierPtr advertisement_info_supplier_; + + using ContentSupplier = + kn::DdsTypedSupplier; + using ContentSupplierPtr = std::unique_ptr; + ContentSupplierPtr content_supplier_; + + unsigned int advertisement_info_seq_; +}; + +typedef std::shared_ptr GenericRapidPubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h new file mode 100644 index 0000000000..52fa3e5f4e --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ + +#include +#include +#include +#include + +#include "ros/ros.h" + +#include "knDds/DdsEventLoop.h" + +#include "knShare/Time.h" + +namespace ff { + +/** + * @brief base class for rapid subscriber to ros publisher + * @details base class for rapid subscriber to ros publisher. + * A kn::DdsEventLoop is run within its own thread of execution. + * Child classes must connect requested message and callback + * to m_ddsEventLoop and call startThread() + */ +class GenericRapidSub { + protected: + GenericRapidSub(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition); + + ~GenericRapidSub(); + + /** + * Will start thread exection by calling threadExec() + */ + virtual void StartThread(); + + std::string subscribe_topic_; + std::string subscriber_partition_; + + std::atomic alive_; + std::thread thread_; + kn::DdsEventLoop dds_event_loop_; + + private: + /** + * Function to execute within seperate thread + * process DdsEventLoop at 10Hz + */ + virtual void ThreadExec(); +}; + +typedef std::shared_ptr GenericRapidSubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h new file mode 100644 index 0000000000..803f835d9f --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ + +/* This is a specialization of ROSBridgeSubscriber using a DDS conduit +*/ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "dds_msgs/GenericCommsRequestSupport.h" + +namespace ff { + +class GenericROSSubRapidPub : public BridgeSubscriber { + public: + GenericROSSubRapidPub(); + ~GenericROSSubRapidPub(); + + void AddTopics(std::map>> const& link_entries); + + void InitializeDDS(std::vector const& connections); + + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, + const RelayTopicInfo& info); + + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info); + + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, + ContentInfo const& content_info); + + void ConvertRequest(rapid::ext::astrobee::GenericCommsRequest const* data, + std::string const& connecting_robot); + + private: + bool dds_initialized_; + + std::map>> topic_mapping_; + std::map robot_connections_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_pub_request.h b/communications/comms_bridge/include/comms_bridge/rapid_pub_request.h new file mode 100644 index 0000000000..9a8f841f6c --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_pub_request.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_PUB_REQUEST_H_ +#define COMMS_BRIDGE_RAPID_PUB_REQUEST_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstantsSupport.h" +#include "dds_msgs/GenericCommsRequestSupport.h" + +namespace ff { + +class RapidPubRequest { + public: + explicit RapidPubRequest(std::string const& robot_name); + ~RapidPubRequest(); + + void SendRequest(std::string const& output_topic); + + private: + using RequestSupplier = + kn::DdsTypedSupplier; + using RequestSupplierPtr = std::unique_ptr; + RequestSupplierPtr request_supplier_; + + unsigned int request_seq_; +}; + +typedef std::shared_ptr RapidPubRequestPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_PUB_REQUEST_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_sub_advertisement_info.h b/communications/comms_bridge/include/comms_bridge/rapid_sub_advertisement_info.h new file mode 100644 index 0000000000..a566c7ee4a --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_sub_advertisement_info.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_SUB_ADVERTISEMENT_INFO_H_ +#define COMMS_BRIDGE_RAPID_SUB_ADVERTISEMENT_INFO_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" + +namespace ff { + +class RapidSubAdvertisementInfo : public GenericRapidSub { + public: + RapidSubAdvertisementInfo(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub); + + /** + * Call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsAdvertisementInfo const* + ad_info); + private: + GenericRapidMsgRosPub* ros_pub_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_SUB_ADVERTISEMENT_INFO_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_sub_content.h b/communications/comms_bridge/include/comms_bridge/rapid_sub_content.h new file mode 100644 index 0000000000..5ea483a8ee --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_sub_content.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_SUB_CONTENT_H_ +#define COMMS_BRIDGE_RAPID_SUB_CONTENT_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +namespace ff { + +class RapidSubContent : public GenericRapidSub { + public: + RapidSubContent(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub); + + /** + * Call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsContent const* content); + + private: + GenericRapidMsgRosPub* ros_pub_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_SUB_CONTENT_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_sub_request.h b/communications/comms_bridge/include/comms_bridge/rapid_sub_request.h new file mode 100644 index 0000000000..d15f557ef8 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_sub_request.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_SUB_REQUEST_H_ +#define COMMS_BRIDGE_RAPID_SUB_REQUEST_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsRequestSupport.h" + +namespace ff { + +class RapidSubRequest : public GenericRapidSub { + public: + RapidSubRequest(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericROSSubRapidPub* ros_sub_rapid_pub); + + /** + * Call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsRequest const* request); + + private: + GenericROSSubRapidPub* ros_sub_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_SUB_REQUEST_H_ diff --git a/communications/comms_bridge/include/comms_bridge/util.h b/communications/comms_bridge/include/comms_bridge/util.h new file mode 100644 index 0000000000..51c07eda52 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/util.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_UTIL_H_ +#define COMMS_BRIDGE_UTIL_H_ + +#include + +#include +#include + +#include + +#include + +#include + +#include + +namespace comms_util { + + // Time-related helpers + ::ros::Time RapidTime2RosTime(const int64_t dds_time); + + int64_t RosTime2RapidTime(::ros::Time const& ros_time); + + // Header-related helpers + void RosHeader2Rapid(::std_msgs::Header const& ros_hdr, + ::rapid::Header* rapid_hdr, + int status = 0, + int serial = -1); + + void RapidHeader2Ros(::rapid::Header const& rapid_hdr, + ::std_msgs::Header* ros_hdr, + std::string const& frame_id = "world"); + +} // end namespace comms_util + +#endif // COMMS_BRIDGE_UTIL_H_ diff --git a/communications/comms_bridge/launch/comms_bridge.launch b/communications/comms_bridge/launch/comms_bridge.launch new file mode 100644 index 0000000000..07ec93abbf --- /dev/null +++ b/communications/comms_bridge/launch/comms_bridge.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/communications/comms_bridge/nodelet_plugins.xml b/communications/comms_bridge/nodelet_plugins.xml new file mode 100644 index 0000000000..f30810f612 --- /dev/null +++ b/communications/comms_bridge/nodelet_plugins.xml @@ -0,0 +1,8 @@ + + + + Nodelet for the comms bridge + + + diff --git a/communications/comms_bridge/package.xml b/communications/comms_bridge/package.xml new file mode 100644 index 0000000000..df3076380b --- /dev/null +++ b/communications/comms_bridge/package.xml @@ -0,0 +1,45 @@ + + + comms_bridge + 0.0.0 + + The comms_bridge package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_util + ff_msgs + topic_tools + config_reader + dds_msgs + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_util + ff_msgs + topic_tools + config_reader + dds_msgs + + + + diff --git a/communications/comms_bridge/readme.md b/communications/comms_bridge/readme.md new file mode 100644 index 0000000000..9f89f2b31c --- /dev/null +++ b/communications/comms_bridge/readme.md @@ -0,0 +1,52 @@ +\page comms_bridge DDS Generic Comms Bridge + +This is a bidirectional bridge to communicate between different astrobees or ros +clients using DDS to relay the topics. Supporting an arbitrary number of in-out +topics, relaying of ROS messages from one topic to another, without any +specification of the message types, nor even the requirement that the types be +available to the nodes at compile-time. + +## Basic architecture + +- The subscriber class (A) subscribes to a specified list of topics +- Upon receipt of the first message on a given forwarded topic, A sends an advertisement message to a metadata "advertisement" topic +- Receiving the metadata advertisement, the republisher class (B) itself advertises the output (republished) topic +- The first and all subsequent messages A receives are serialized and published to a metadata "content" topic +- A delay is implemented in B between its advertising and subsequent republishing of any messages to practically avoid the potential race condition of a subscriber on that end missing the first message as it connects to B only upon seeing the advertisement +- Otherwise, B deserializes all messages received on the "content" metadata topic, looks up the topic in previously received "advertisement" metadata, and republishes the message on the output topic +- In this implementation, the metadata advertisement info and content are converted to and passed over dds. + +## Enabling DDS Communication + +The generic comms bridge runs by default when the astrobee flight starts up. By +default, dds publishing and subscribing is disabled since astrobee to astrobee +communication is not desired for every activity. There are 2 ways to enable it. +One can either change the 'initialize_dds_on_start' parameter in the comms bridge +config file to true before starting flight software or send an "enable astrobee +intercomms" command to astrobee after the flight software has started. + +## Specifying Topics + +To add topics to be passed between robots, please see the comms bridge config +file. The comments in the config file should explain how to add topics. + +## DDS Specifics + +### Discovery Peers File + +In order to have two or more Astrobees communicate, one must add the ip +addresses to the "NDDS_DISCOVERY_PEERS" file in the "dds_generic_comms" folder. +Be sure to add a semi-colon (;) to the end of every line added. + +### QoS Profile + +By default, DDS properties are used to ensure the advertisement info messages +are reliably deliveried to the republisher class even if the subscriber or +republisher are restarted. Currently, DDS is configured to work if there are +no more than 20 topics being passed between robots. If more topics are required, +please change the message depth in the "RAPID_QOS_PROFILES.xml" file in the +"dds_generic_comms" folder. The depth for the "RapidReliableDurableQos" will +need to be changed for both the "datareader_qos" and the "datawriter_qos". This +should be on or around lines 206 and 218 in the file. You will also need to +change the value check in the "comms_bridge_nodelet.cpp" file on or around +line 307. diff --git a/communications/comms_bridge/src/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp new file mode 100644 index 0000000000..61c4dfe743 --- /dev/null +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -0,0 +1,245 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/bridge_publisher.h" + +#include +#include +#include +#include +#include + +// ROS internal publisher message queue size +#define PUBLISHER_QUEUE_SIZE 10 +// max number of messages to store because we're not ready to republish them +#define MAX_WAITING_QUEUE_SIZE 100 +// default time to delay between advertising and publishing on that topic [sec] +#define DEFAULT_ADVERTISE_DELAY 3.0 + +using ros::Publisher; +using ros::Publisher; +using topic_tools::ShapeShifter; + +BridgePublisher::BridgePublisher(double ad2pub_delay) : m_ad2pub_delay_(ad2pub_delay) { + m_n_relayed_ = 0; + m_verbose_ = 0; + + worker_thread_.reset(new std::thread(&BridgePublisher::drainThread, this)); +} + +BridgePublisher::~BridgePublisher() { + worker_thread_->join(); +} + +void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } + +bool BridgePublisher::advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info) { + if (m_verbose_) + ROS_INFO("Advertising for topic %s\n", output_topic.c_str()); + + std::map::iterator iter = m_relay_topics_.find(output_topic); + if (iter == m_relay_topics_.end()) { + if (m_verbose_) + ROS_INFO(" topic is yet-unknown\n"); + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + iter = m_relay_topics_.emplace(output_topic, topic_info).first; + } + + RelayTopicInfo &topic_info = iter->second; + + if (topic_info.advertised) { + // we've already advertised, nothing to do + if (topic_info.ad_info.md5_sum != ad_info.md5_sum || + topic_info.ad_info.data_type != ad_info.data_type || + topic_info.ad_info.definition != ad_info.definition) + ROS_ERROR("Received advertisement with differing type info from previous advertisement, messages will be lost"); + return false; + } else { + if (topic_info.ad_info.md5_sum != "" && topic_info.ad_info.md5_sum != ad_info.md5_sum) + ROS_WARN( + "Advertisement received for topic %s with differing md5sum from prior content, prior messages will be lost", + output_topic.c_str()); + + // we haven't seen an ad for this topic yet, populate everything + topic_info.ad_info = ad_info; + + ros::NodeHandle nh; + ShapeShifter shifter; + // will always succeed, even if inconsistent type info + shifter.morph(ad_info.md5_sum, ad_info.data_type, ad_info.definition, ""); + topic_info.publisher = shifter.advertise(nh, output_topic, PUBLISHER_QUEUE_SIZE, ad_info.latching); + + topic_info.advertised = true; + + const timepoint_t now = std::chrono::steady_clock::now(); + topic_info.delay_until = now + std::chrono::microseconds((unsigned)(m_ad2pub_delay_ * 1e6)); + + if (m_ad2pub_delay_ > 0) { + m_drain_queue_.push(std::make_pair(topic_info.delay_until, output_topic)); + m_drain_cv_.notify_all(); + } else { + // no post-advertise delay + + // send out any old messages first + drainWaitingQueue(topic_info); + } + } + + return true; +} + +bool BridgePublisher::relayContent(RelayTopicInfo& topic_info, const ContentInfo& content_info) { + ShapeShifter shifter; + + // IStreams don't really modify their input but the implementation is + // lazily general so we have to be ugly + ros::serialization::IStream stream(const_cast(content_info.data.data()), content_info.data.size()); + try { + stream.next(shifter); + } + catch (ros::Exception &e) { + ROS_ERROR("Deserialization error (%s), losing message", e.what()); + return false; + } + + // Output error if inconsistent type info + if (topic_info.ad_info.md5_sum != content_info.type_md5_sum) { + ROS_ERROR("Content md5 sum doesn't match the advertiment info md5 sum. This could cause issues!"); + } + + // will always succeed, even if inconsistent type info + shifter.morph(topic_info.ad_info.md5_sum, topic_info.ad_info.data_type, topic_info.ad_info.definition, ""); + + try { + topic_info.publisher.publish(shifter); + } + catch (ros::Exception &e) { + ROS_ERROR("Error publishing morphed message (%s), losing message", e.what()); + return false; + } + + topic_info.relay_seqnum++; + m_n_relayed_++; + + if (m_verbose_) + ROS_INFO("Sent message on topic %s\n", topic_info.out_topic.c_str()); + + return true; +} + +bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info) { + if (m_verbose_) + ROS_INFO("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); + + if (!topic_info.advertised) { + if (m_verbose_) + ROS_INFO(" topic is yet-unknown, enqueuing message for now\n"); + + topic_info.waiting_msgs.emplace(content_info); + + return true; + } + + if (topic_info.ad_info.md5_sum != content_info.type_md5_sum) + ROS_WARN( + "Message received for topic %s with differing md5sum from prior advertiesment, future messages will be lost", + topic_info.out_topic.c_str()); + + const timepoint_t now = std::chrono::steady_clock::now(); + + if (!topic_info.advertised || now < topic_info.delay_until) { + // we still don't have type information for this message, or + // we're still delaying after advertising + // so just enqueue messages + + if (m_verbose_) + ROS_INFO(" topic is not %s, enqueuing message for now\n", + (!topic_info.advertised ? "yet advertised" : "yet ready")); + + // limit the number of waiting messages, tossing the oldest ones + while (topic_info.waiting_msgs.size() >= MAX_WAITING_QUEUE_SIZE) + topic_info.waiting_msgs.pop(); + + topic_info.waiting_msgs.emplace(content_info); + + if (m_verbose_ > 1) + ROS_INFO(" %zu messages now waiting\n", topic_info.waiting_msgs.size()); + + return true; + } + + // send out any old messages first + drainWaitingQueue(topic_info); + + // send out this message + return relayContent(topic_info, content_info); +} + +void BridgePublisher::drainThread() { + std::unique_lock lk(m_mutex_); + + if (m_verbose_) + ROS_INFO("Started drain thread\n"); + + while (1) { + while (m_drain_queue_.empty()) + m_drain_cv_.wait(lk); + + std::pair entry = m_drain_queue_.top(); + m_drain_queue_.pop(); + + lk.unlock(); + + timepoint_t t = entry.first; + + if (m_verbose_ > 1) + ROS_INFO( + "draining %s in %0.3f seconds\n", entry.second.c_str(), + std::chrono::duration((t - std::chrono::steady_clock::now())).count()); + std::this_thread::sleep_until(t); + + if (m_verbose_ > 1) + ROS_INFO("draining %s now\n", entry.second.c_str()); + + lk.lock(); + + drainWaitingQueue(entry.second); + } +} + +// called with mutex held +void BridgePublisher::drainWaitingQueue(RelayTopicInfo& topic_info) { + if (m_verbose_ && topic_info.waiting_msgs.size() > 0) + ROS_INFO("Draining queue of %zu waiting messages\n", topic_info.waiting_msgs.size()); + while (topic_info.waiting_msgs.size() > 0) { + const ContentInfo old_msg = topic_info.waiting_msgs.front(); + relayContent(topic_info, old_msg); + topic_info.waiting_msgs.pop(); + } +} + +// called with mutex held +void BridgePublisher::drainWaitingQueue(std::string const& output_topic) { + std::map::iterator iter = m_relay_topics_.find(output_topic); + assert(iter != m_relay_topics_.end()); + + RelayTopicInfo &topic_info = iter->second; + + drainWaitingQueue(topic_info); +} diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp new file mode 100644 index 0000000000..d35987c1eb --- /dev/null +++ b/communications/comms_bridge/src/bridge_subscriber.cpp @@ -0,0 +1,187 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include "comms_bridge/bridge_subscriber.h" + +// ROS internal subscriber message queue size +#define SUBSCRIBER_QUEUE_SIZE 10 +// max serialized message size +#define ROS_BRIDGE_MAX_MSG_SIZE (10*1024*1024) + +using ros::Subscriber; +using ros::Publisher; +using topic_tools::ShapeShifter; + +BridgeSubscriber::BridgeSubscriber() { + m_n_relayed_ = 0; + m_verbose_ = 0; + + m_msgbuffer_ = new uint8_t[ROS_BRIDGE_MAX_MSG_SIZE]; +} + +BridgeSubscriber::~BridgeSubscriber() { delete[] m_msgbuffer_; } + +void BridgeSubscriber::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } + +void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent& msg_event, + std::string const& topic, SubscriberPtr sub) { + ShapeShifter::ConstPtr ptr = msg_event.getConstMessage(); + + boost::shared_ptr const& connection_header = + msg_event.getConnectionHeaderPtr(); + + if (m_verbose_) { + ROS_INFO("got data on %s:\n", topic.c_str()); + if (m_verbose_ > 1) { + ROS_INFO(" datatype: \"%s\"\n", ptr->getDataType().c_str()); + ROS_INFO(" md5: \"%s\"\n", ptr->getMD5Sum().c_str()); + } + if (m_verbose_ > 2) + ROS_INFO(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); + + if (m_verbose_ > 2) { + ROS_INFO(" conn header:\n"); + for (ros::M_string::const_iterator iter = connection_header->begin(); + iter != connection_header->end(); + iter++) + ROS_INFO(" %s: %s\n", iter->first.c_str(), iter->second.c_str()); + } + } + + std::unique_lock lock(m_mutex_); + + std::map::iterator iter = m_relay_topics_.find(topic); + if (iter == m_relay_topics_.end()) { + ROS_INFO("Received message on non-relayed topic %s\n", topic.c_str()); + return; + } + + RelayTopicInfo &topic_info = iter->second; + + if (!topic_info.advertised) { + if (m_verbose_) + ROS_INFO(" sending advertisement\n"); + + bool latching = false; + if (connection_header) { + ros::M_string::const_iterator iter = connection_header->find("latching"); + if (iter != connection_header->end() && iter->second == "1") + latching = true; + } + + AdvertisementInfo ad_info; + ad_info.latching = latching; + ad_info.data_type = ptr->getDataType(); + ad_info.md5_sum = ptr->getMD5Sum(); + ad_info.definition = ptr->getMessageDefinition(); + topic_info.ad_info = ad_info; + + advertiseTopic(topic_info); + topic_info.advertised = true; + + // ROS "latches" the msg type md5sum for us, only accepting same in future + // so no need for us to save and check that it doesn't change + } + + ros::serialization::OStream stream(m_msgbuffer_, ROS_BRIDGE_MAX_MSG_SIZE); + stream.next(*ptr); // serializes just the message contents + ssize_t serialized_size = ROS_BRIDGE_MAX_MSG_SIZE - stream.getLength(); + if (m_verbose_ > 2) + ROS_INFO(" serialized size = %zd\n", serialized_size); + if (serialized_size <= 0) { + ROS_ERROR("Serialization buffer size deficient, discarding message"); + return; + } + + ContentInfo content_info; + content_info.type_md5_sum = ptr->getMD5Sum(); + content_info.data_size = (size_t)serialized_size; + content_info.data = m_msgbuffer_; + relayMessage(topic_info, content_info); + topic_info.relay_seqnum++; + m_n_relayed_++; + + lock.release()->unlock(); + // now done with any access to topic info + + if (m_verbose_) + fflush(stdout); +} + +// called with mutex held +SubscriberPtr BridgeSubscriber::rosSubscribe(std::string const& topic) { + ros::NodeHandle nh; + + SubscriberPtr ptr = std::make_shared(); + + ros::SubscribeOptions opts; + opts.topic = topic; + opts.queue_size = SUBSCRIBER_QUEUE_SIZE; + opts.md5sum = ros::message_traits::md5sum(); + opts.datatype = ros::message_traits::datatype(); + opts.helper = boost::make_shared &> >( + std::bind(&BridgeSubscriber::handleRelayedMessage, this, std::placeholders::_1, topic, ptr)); + *ptr = nh.subscribe(opts); + + if (m_verbose_) + ROS_INFO("Subscribed to topic %s\n", topic.c_str()); + + return ptr; +} + +bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& out_topic) { + const std::lock_guard lock(m_mutex_); + + // Enforce that all relays have a unique input topic + if (m_relay_topics_.find(in_topic) != m_relay_topics_.end()) { + if (m_verbose_) + ROS_ERROR("Already subscribed to relay from topic %s\n", in_topic.c_str()); + return false; + } + + // Enforce that all relays have a unique output topic + // The republishing side will already be checking for this but may have no way + // to communicate that to us, so we check too + std::map::iterator iter = m_relay_topics_.begin(); + while (iter != m_relay_topics_.end()) { + if (iter->second.out_topic == out_topic) { + if (m_verbose_) + ROS_ERROR("Already relaying to topic %s\n", out_topic.c_str()); + return false; + } + iter++; + } + + RelayTopicInfo topic_info; + topic_info.out_topic = out_topic; + + topic_info.sub = rosSubscribe(in_topic); + // handleRelayedMessage() may immediately start getting called + + m_relay_topics_[in_topic] = topic_info; + + // let implementation know + subscribeTopic(in_topic, topic_info); + + return true; +} diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp new file mode 100644 index 0000000000..2887553208 --- /dev/null +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -0,0 +1,410 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Standard ROS includes +#include +#include +#include + +// FSW shared libraries +#include + +// FSW nodelet +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "comms_bridge/generic_rapid_msg_ros_pub.h" +#include "comms_bridge/generic_rapid_sub.h" +#include "comms_bridge/generic_ros_sub_rapid_pub.h" +#include "comms_bridge/rapid_sub_advertisement_info.h" +#include "comms_bridge/rapid_sub_content.h" +#include "comms_bridge/rapid_sub_request.h" + +#include "dds_msgs/AstrobeeConstants.h" + +#include "ff_msgs/ResponseOnly.h" + +// SoraCore +#include "knDds/DdsSupport.h" +#include "knDds/DdsEntitiesFactory.h" +#include "knDds/DdsEntitiesFactorySvc.h" +#include "knDds/DdsTypedSupplier.h" + +// miro +#include "miro/Configuration.h" +#include "miro/Robot.h" +#include "miro/Log.h" + +namespace kn { + class DdsEntitiesFactorySvc; +} // end namespace kn + +namespace comms_bridge { + +class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { + public: + CommsBridgeNodelet() : ff_util::FreeFlyerNodelet("comms_bridge"), + dds_initialized_(false), + initialize_dds_on_start_(false), + enable_advertisement_info_request_(false) {} + + virtual ~CommsBridgeNodelet() {} + + protected: + virtual void Initialize(ros::NodeHandle* nh) { + // Need to get robot name which is in the lua config files, add files, read + // files, and get robot name but don't get the rest of the config parameters + // since these params are used to create the classes that use rapid dds. Need + // to set up Miro/DDs before reading the parameters. + + config_params_.AddFile("communications/comms_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("BridgeSubscriberNodelet: Error reading config files."); + exit(EXIT_FAILURE); + return; + } + + if (!config_params_.GetStr("agent_name", &agent_name_)) { + ROS_FATAL("BridgeSubscriberNodelet: Could not read robot name."); + exit(EXIT_FAILURE); + return; + } + + // In simulation, the namespace is usually set to the robot name so we need to + // check if we are in simulation and get the right name + if (agent_name_ == "sim" || agent_name_ == "simulator") { + // The platform name should be the simulated robot name + agent_name_ = GetPlatform(); + + // If there is not robot name, set it to a default name so that we can + // connect to the bridge + if (agent_name_ == "") { + agent_name_ = "Bumble"; + } else { + // Make sure that first letter of robot name is capitialized. GDS only + // recognizes capitialized robot names. + agent_name_[0] = toupper(agent_name_[0]); + } + } + ROS_INFO_STREAM("Comms Bridge Nodelet: agent name " << agent_name_); + + ros_sub_ = std::make_shared(); + + int fake_argc = 1; + + // Make path to QOS and NDDS files + std::string config_path = ff_common::GetConfigDir(); + config_path += "/communications/dds_generic_comms/"; + + // Create fake argv containing only the participant name + // Participant name needs to be unique so combine robot name with timestamp + ros::Time time = ros::Time::now(); + participant_name_ = agent_name_ + std::to_string(time.sec) + std::string("-comms-bridge"); + char **fake_argv = new char*[1]; + fake_argv[0] = new char[(participant_name_.size() + 1)]; + std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT + + /* fake miro log into thinking we have no arguments */ + Miro::Log::init(fake_argc, fake_argv); + Miro::Log::level(9); + + /* fake miro configuration into thinking we have no arguments */ + Miro::Configuration::init(fake_argc, fake_argv); + + Miro::RobotParameters *robot_params = Miro::RobotParameters::instance(); + + kn::DdsEntitiesFactorySvcParameters *dds_params = + kn::DdsEntitiesFactorySvcParameters::instance(); + + /* get the defaults for *all the things!* */ + Miro::ConfigDocument *config = Miro::Configuration::document(); + config->setSection("Robot"); + config->getParameters("Miro::RobotParameters", *robot_params); + config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); + + robot_params->name = agent_name_; + robot_params->namingContextName = robot_params->name; + + ROS_INFO("Agent name %s and participant name %s\n", agent_name_.c_str(), participant_name_.c_str()); + + // Set values for default punlisher and subscriber + dds_params->publishers[0].name = agent_name_; + dds_params->publishers[0].partition = agent_name_; + dds_params->publishers[0].participant = participant_name_; + dds_params->subscribers[0].participant = participant_name_; + + // Clear config files so that dds only looks for the files we add + dds_params->participants[0].discoveryPeersFiles.clear(); + dds_params->configFiles.clear(); + + dds_params->participants[0].name = participant_name_; + dds_params->participants[0].participantName = participant_name_; + dds_params->participants[0].domainId = 38; + dds_params->participants[0].discoveryPeersFiles.push_back( + (config_path + "NDDS_DISCOVERY_PEERS")); + dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); + + std::string local_subscriber = Miro::RobotParameters::instance()->name.c_str(); + + if (!ReadParams()) { + exit(EXIT_FAILURE); + return; + } + + // Register the connections into the parameters so they can be used later + for (int i = 0; i < rapid_connections_.size(); i++) { + // This shouldn't be needed but check just in case + if (local_subscriber != rapid_connections_[i]) { + kn::DdsNodeParameters subscriber; + subscriber.name = rapid_connections_[i]; + subscriber.partition = rapid_connections_[i]; + subscriber.participant = participant_name_; + dds_params->subscribers.push_back(subscriber); + } + } + + /** + * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory + * which will create all objects: + * Participants DdsDomainParticipantRepository::instance() + * Publishers DdsPublisherRespository::instance() + * Subscribers DdsSubscriberRepository::instance() + * Topics + * and store in relevant repository + * based on DdsEntitiesFactoryParameters + */ + dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); + dds_entities_factory_->init(dds_params); + + dds_initialize_srv_ = nh->advertiseService( + SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS, + &CommsBridgeNodelet::StartDDS, + this); + + if (initialize_dds_on_start_) { + InitializeDDS(); + } + } + + bool StartDDS(ff_msgs::ResponseOnly::Request& req, + ff_msgs::ResponseOnly::Response& res) { + if (!dds_initialized_) { + InitializeDDS(); + } + res.success = true; + return true; + } + + void InitializeDDS() { + std::string connection, dds_topic_name; + ff::GenericRapidSubPtr rapid_sub; + ros_sub_->InitializeDDS(rapid_connections_); + if (enable_advertisement_info_request_) { + ros_pub_->InitializeDDS(rapid_connections_, + enable_advertisement_info_request_); + } + for (size_t i = 0; i < rapid_connections_.size(); i++) { + // Lower case the external agent name to use it like a namespace + connection = rapid_connections_[i]; + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; + ROS_INFO("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); + rapid_sub = std::make_shared( + "AstrobeeGenericCommsAdvertisementInfoProfile", + dds_topic_name, + connection, + ros_pub_.get()); + rapid_subs_.push_back(rapid_sub); + + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; + ROS_INFO("Comms Bridge: DDS Sub DDS content topic name: %s\n", + dds_topic_name.c_str()); + rapid_sub = std::make_shared( + "AstrobeeGenericCommsContentProfile", + dds_topic_name, + connection, + ros_pub_.get()); + rapid_subs_.push_back(rapid_sub); + + if (enable_advertisement_info_request_) { + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_REQUEST_TOPIC; + ROS_INFO("Comms Bridge: DDS Sub DDS request topic name: %s\n", + dds_topic_name.c_str()); + rapid_sub = std::make_shared( + "AstrobeeGenericCommsRequestProfile", + dds_topic_name, + connection, + ros_sub_.get()); + rapid_subs_.push_back(rapid_sub); + } + } + ros_sub_->AddTopics(link_entries_); + dds_initialized_ = true; + } + + bool ReadParams() { + double ad2pub_delay = 0; + if (!config_params_.GetReal("ad2pub_delay", &ad2pub_delay) || + ad2pub_delay <= 0) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read/or invalid ad2pub_delay. Setting to 3."); + ad2pub_delay = 3; + } + ros_pub_ = std::make_shared(ad2pub_delay); + + unsigned int verbose = 2; + if (!config_params_.GetUInt("verbose", &verbose)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read verbosity level. Setting to 2 (info?)."); + } + ros_sub_->setVerbosity(verbose); + ros_pub_->setVerbosity(verbose); + + initialize_dds_on_start_ = false; + if (!config_params_.GetBool("initialize_dds_on_start", + &initialize_dds_on_start_)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read initialize dds on start. Setting to false."); + } + + enable_advertisement_info_request_ = false; + if (!config_params_.GetBool("enable_advertisement_info_request", + &enable_advertisement_info_request_)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read enable advertisement info request. Setting to false."); + } + + // Load shared topic groups + config_reader::ConfigReader::Table links, link; + if (!config_params_.GetTable("links", &links)) { + ROS_FATAL("Comms Bridge Nodelet: Links not specified!"); + return false; + } + + std::string ns = std::string("/") + agent_name_ + "/"; + ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case + int num_topics = 0; + ROS_INFO_STREAM("Read Params numebr of links: " << links.GetSize()); + for (int i = 1; i <= links.GetSize(); i++) { + if (!links.GetTable(i, &link)) { + NODELET_ERROR("Comms Bridge Nodelet: Could read link table row %i", i); + continue; + } + std::string config_agent, connection_agent; + num_topics = 0; + ROS_INFO_STREAM("Link " << i << " from " << link.GetStr("from", &config_agent)); + ROS_INFO_STREAM("Link " << i << " to " << link.GetStr("to", &config_agent)); + if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { + if (AddRapidConnections(link, "to", connection_agent)) { + AddTableToSubs(link, "relay_forward", ns, connection_agent, num_topics); + AddTableToSubs(link, "relay_both", ns, connection_agent, num_topics); + } + } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { + if (AddRapidConnections(link, "from", connection_agent)) { + AddTableToSubs(link, "relay_backward", ns, connection_agent, num_topics); + AddTableToSubs(link, "relay_both", ns, connection_agent, num_topics); + } + } + + // Check to make sure the number of topics added doesn't exceed the + // the number of messages dds reliably delivers + if (num_topics > 20) { + ROS_ERROR("Comms bridge: Num of added topics is greater than the number of topics dds will reliably deliver."); + } + } + return true; + } + + bool AddRapidConnections(config_reader::ConfigReader::Table &link_table, + std::string const& direction, + std::string &connection) { + if (!link_table.GetStr(direction.c_str(), &connection)) { + NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction.c_str()); + return false; + } + + // This should be very quick since we shouldn't have more than 2 connections + bool found = false; + for (size_t i = 0; i < rapid_connections_.size() && !found; i++) { + if (connection == rapid_connections_[i]) { + found = true; + } + } + + if (!found) { + rapid_connections_.push_back(connection); + } + return true; + } + + void AddTableToSubs(config_reader::ConfigReader::Table &link_table, + std::string table_name, + std::string const& current_robot_ns, + std::string const& connection_robot, + int &num_topics) { + config_reader::ConfigReader::Table relay_table, relay_item; + std::string in_topic, out_topic; + if (link_table.GetTable(table_name.c_str(), &relay_table)) { + num_topics += relay_table.GetSize(); + for (size_t i = 1; i <= relay_table.GetSize(); i++) { + relay_table.GetTable(i, &relay_item); + if (!relay_item.GetStr("in_topic", &in_topic)) { + NODELET_ERROR("Comms Bridge Nodelet: In topic not specified!"); + continue; + } + + if (!relay_item.GetStr("out_topic", &out_topic)) { + out_topic = current_robot_ns + in_topic; + } + + // Save all output topics under the same in topic since we don't want + // to subscribe to the same topic multiple times + link_entries_[in_topic].push_back(std::make_pair(connection_robot, + out_topic)); + } + } + } + + private: + bool initialize_dds_on_start_, dds_initialized_; + bool enable_advertisement_info_request_; + config_reader::ConfigReader config_params_; + + std::vector rapid_subs_; + std::vector rapid_connections_; + + std::shared_ptr dds_entities_factory_; + std::shared_ptr ros_pub_; + std::shared_ptr ros_sub_; + + std::string agent_name_, participant_name_; + std::map>> link_entries_; + ros::ServiceServer dds_initialize_srv_; +}; + +PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) + +} // namespace comms_bridge diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp new file mode 100644 index 0000000000..9a7b4389c3 --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -0,0 +1,141 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include +#include +#include + +namespace ff { + +GenericRapidMsgRosPub::GenericRapidMsgRosPub(double ad2pub_delay) : + BridgePublisher(ad2pub_delay), + dds_initialized_(false), + enable_advertisement_info_request_(false) { +} + +GenericRapidMsgRosPub::~GenericRapidMsgRosPub() {} + +void GenericRapidMsgRosPub::InitializeDDS(std::vector const& connections, + bool enable_advertisement_info_request) { + std::string robot_name; + for (size_t i = 0; i < connections.size(); ++i) { + robot_name = connections[i]; + RapidPubRequestPtr rapid_pub = std::make_shared(robot_name); + robot_connections_[robot_name] = rapid_pub; + } + + enable_advertisement_info_request_ = enable_advertisement_info_request; + + dds_initialized_ = true; +} + +// Handle Ad Message +void GenericRapidMsgRosPub::ConvertAdvertisementInfo( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_INFO("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + output_topic.c_str()); + + AdvertisementInfo ad_info; + ad_info.latching = data->latching; + ad_info.data_type = data->dataType; + ad_info.md5_sum = data->md5Sum; + ad_info.definition = data->msgDefinition; + ROS_INFO_STREAM("ad_info latching: " << data->latching); + ROS_INFO_STREAM("ad_info dataType: " << data->dataType); + ROS_INFO_STREAM("ad_info md5Sum: " << data->md5Sum); + ROS_INFO_STREAM("ad_info msgDefinition: " << data->msgDefinition); + + if (!advertiseTopic(output_topic, ad_info)) { + ROS_INFO("Comms Bridge Nodelet: Received more than one advertisement info message for topic: %s\n", + output_topic.c_str()); + } +} + +// Handle content message +void GenericRapidMsgRosPub::ConvertContent( + rapid::ext::astrobee::GenericCommsContent const* data, + std::string const& connecting_robot) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_INFO("Comms Bridge Nodelet: Received content message for topic %s\n", + output_topic.c_str()); + + std::map::iterator iter = m_relay_topics_.find(output_topic); + if (iter == m_relay_topics_.end()) { + ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n", + output_topic.c_str()); + + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + topic_info.ad_info.md5_sum = data->md5Sum; + iter = m_relay_topics_.emplace(output_topic, topic_info).first; + + RequestAdvertisementInfo(output_topic, connecting_robot); + } + + RelayTopicInfo &topic_info = iter->second; + + ContentInfo content_info; + content_info.type_md5_sum = data->md5Sum; + + unsigned char* buf = data->data.get_contiguous_buffer(); + for (size_t i = 0; i < data->data.length(); i++) { + content_info.data.push_back(buf[i]); + } + + ROS_INFO("Comms Bridge Nodelet: Calling relay message for topic %s\n", + output_topic.c_str()); + + if (!relayMessage(topic_info, content_info)) { + ROS_ERROR("Comms Bridge Nodelet: Error relaying message for topic %s\n", + output_topic.c_str()); + } +} + +void GenericRapidMsgRosPub::RequestAdvertisementInfo( + std::string const& output_topic, + std::string const& connecting_robot) { + if (enable_advertisement_info_request_) { + if (dds_initialized_) { + // Check robot connection exists + if (robot_connections_.find(connecting_robot) != + robot_connections_.end()) { + ROS_INFO("Sending request for advertisement info for topic %s!\n", + output_topic.c_str()); + robot_connections_[connecting_robot]->SendRequest(output_topic); + } else { + ROS_ERROR("Comms Bridge: No connection for %s in rapid msg ros pub.\n", + connecting_robot.c_str()); + } + } else { + ROS_ERROR("Comms Bridge: DDS is not initialized for sending advertisement info request.\n"); + } + } else { + ROS_INFO("Comms Bridge: Advertisement info request not enabled.\n"); + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_rapid_pub.cpp b/communications/comms_bridge/src/generic_rapid_pub.cpp new file mode 100644 index 0000000000..036e7e20b8 --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_pub.cpp @@ -0,0 +1,168 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_rapid_pub.h" + +#include + +namespace ff { + +GenericRapidPub::GenericRapidPub(std::string const& robot_name) : + advertisement_info_seq_(0) { + std::string dds_topic_name; + dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; + ROS_INFO("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); + advertisement_info_supplier_.reset( + new GenericRapidPub::AdvertisementInfoSupplier( + dds_topic_name, "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); + + dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; + ROS_INFO("Comms Bridge: DDS Publisher DDS content topic name: %s\n", + dds_topic_name.c_str()); + content_supplier_.reset(new GenericRapidPub::ContentSupplier( + dds_topic_name, "", "AstrobeeGenericCommsContentProfile", "")); + + rapid::RapidHelper::initHeader(advertisement_info_supplier_->event().hdr); + rapid::RapidHelper::initHeader(content_supplier_->event().hdr); +} + +GenericRapidPub::~GenericRapidPub() {} + +template +void GenericRapidPub::CopyString(const int max_size, + std::string src, + T &dest, + std::string const& data_name, + std::string const& topic) { + unsigned int size = src.size(); + if (size > (max_size - 1)) { + ROS_ERROR("Comms Bridge: %s for topic %s is greater than %i characters. Please change idl.", + data_name.c_str(), topic.c_str(), max_size); + size = (max_size - 1); + } + memset(dest, 0, max_size); + strncpy(dest, src.data(), size); + dest[size] = '\0'; +} + +void GenericRapidPub::SendAdvertisementInfo(std::string const& output_topic, + bool latching, + std::string const& data_type, + std::string const& md5_sum, + std::string definition) { + rapid::ext::astrobee::GenericCommsAdvertisementInfo &msg = + advertisement_info_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = ++advertisement_info_seq_; + + // Currrently the output topic can only be 128 characters long + CopyString(128, + output_topic, + msg.outputTopic, + "Out topic", + output_topic); + + msg.latching = latching; + + // Currently the data type can only be 128 characters long + CopyString(128, + data_type, + msg.dataType, + "Data type", + output_topic); + + // Currently the md5 sum can only be 64 characters long + CopyString(64, + md5_sum, + msg.md5Sum, + "Md5 sum", + output_topic); + + // Remove legal statement(s) + std::string::size_type begin = definition.find("# Copyright (c) 20"); + std::string::size_type end; + while (begin != definition.npos) { + end = definition.find("# under the License."); + end += 20; + definition.erase(begin, (end - begin)); + begin = definition.find("# Copyright (c) 20"); + } + + // Currently the ROS message definition can only by 8192 characters long + ROS_INFO("Definition is %i long\n", definition.size()); + CopyString(8192, + definition, + msg.msgDefinition, + "Message definition", + output_topic); + + // Send message + advertisement_info_supplier_->sendEvent(); +} + +void GenericRapidPub::SendContent(std::string const& output_topic, + std::string const& md5_sum, + uint8_t const* data, + const size_t data_size, + const int seq_num) { + unsigned int size; + rapid::ext::astrobee::GenericCommsContent &msg = content_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = seq_num; + + // Currently the output topic can only be 128 characters long + CopyString(128, + output_topic, + msg.outputTopic, + "Out topic", + output_topic); + + // Currently the md5 sum can only be 64 characters long + CopyString(64, + md5_sum, + msg.md5Sum, + "Md5 sum", + output_topic); + + // Currently the content can only be 128K bytes long + size = data_size; + if (size > 131071) { + ROS_ERROR("Comms Bridge: The message data for topic %s is greater than 131072 . Please change idl.", + output_topic); + size = 131071; + } + + msg.data.ensure_length(size, (size + 1)); + unsigned char *buf = msg.data.get_contiguous_buffer(); + if (buf == NULL) { + ROS_ERROR("DDS: RTI didn't give a contiguous buffer for the content data!"); + return; + } + std::memset(buf, 0, (size + 1)); + std::memmove(buf, data, size); + + // Send message + content_supplier_->sendEvent(); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_rapid_sub.cpp b/communications/comms_bridge/src/generic_rapid_sub.cpp new file mode 100644 index 0000000000..ad41b7d35b --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_sub.cpp @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_rapid_sub.h" + +#include + +namespace ff { + +GenericRapidSub::GenericRapidSub(const std::string& entity_name, const std::string& subscribe_topic, + const std::string& subscriber_partition) + : dds_event_loop_(entity_name), + subscribe_topic_(subscribe_topic), + subscriber_partition_(subscriber_partition), + alive_(true) {} + +GenericRapidSub::~GenericRapidSub() { + alive_ = false; // Notify thread to exit + thread_.join(); +} + +void GenericRapidSub::StartThread() { + // Start joinable thread + thread_ = std::thread(&GenericRapidSub::ThreadExec, this); +} + +void GenericRapidSub::ThreadExec() { + while (alive_) { + // process events at 10hz + dds_event_loop_.processEvents(kn::milliseconds(100)); + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp new file mode 100644 index 0000000000..6e1db13f92 --- /dev/null +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -0,0 +1,202 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_ros_sub_rapid_pub.h" + +#include +#include +#include +#include + +namespace ff { + +GenericROSSubRapidPub::GenericROSSubRapidPub() : dds_initialized_(false) {} + +GenericROSSubRapidPub::~GenericROSSubRapidPub() {} + +void GenericROSSubRapidPub::AddTopics( + std::map>> const& link_entries) { + std::string in_topic, primary_out_topic; + // Make sure dds is initialized before adding topics + if (dds_initialized_) { + for (auto it = link_entries.begin(); it != link_entries.end(); ++it) { + in_topic = it->first; + // Use the first out_topic we read in as the out topic the base class uses + primary_out_topic = it->second[0].second; + // Save all robot/out topic pairs so that the bridge can pass the correct + // advertisement info and content message to each roboot that needs it + topic_mapping_[primary_out_topic] = it->second; + // Add topic to base class + ROS_INFO("Adding topic %s to base class.", in_topic.c_str()); + addTopic(in_topic, primary_out_topic); + } + } else { + ROS_ERROR("Comms Bridge: Cannot add topics until dds is initialized.\n"); + } +} + +void GenericROSSubRapidPub::InitializeDDS(std::vector const& connections) { + std::string robot_name; + for (size_t i = 0; i < connections.size(); ++i) { + robot_name = connections[i]; + GenericRapidPubPtr rapid_pub = std::make_shared(robot_name); + robot_connections_[robot_name] = rapid_pub; + } + + dds_initialized_ = true; +} + +// Called with the mutex held +void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, + const RelayTopicInfo& info) { + // this is just the base subscriber letting us know it's adding a topic + // nothing more we need to do +} + +// Called with the mutex held +void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { + const AdvertisementInfo &info = relay_info.ad_info; + std::string out_topic = relay_info.out_topic, robot_name, robot_out_topic; + + ROS_INFO("Received ros advertise topic for topic %s\n", out_topic.c_str()); + + // Make sure we recognize the topic + if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in advertise topic.\n", + out_topic.c_str()); + return; + } + + for (size_t i = 0; i < topic_mapping_[out_topic].size(); ++i) { + robot_name = topic_mapping_[out_topic][i].first; + robot_out_topic = topic_mapping_[out_topic][i].second; + + ROS_INFO("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->SendAdvertisementInfo(out_topic, + info.latching, + info.data_type, + info.md5_sum, + info.definition); + } +} + +// Called with the mutex held +void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, + ContentInfo const& content_info) { + std::string out_topic = topic_info.out_topic, robot_name, robot_out_topic; + unsigned int size; + ROS_INFO("Received ros content message for topic %s\n", out_topic.c_str()); + + // Make sure we recognize the topic + if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in relay message.\n", + out_topic.c_str()); + return; + } + + for (size_t i = 0; i < topic_mapping_[out_topic].size(); ++i) { + robot_name = topic_mapping_[out_topic][i].first; + robot_out_topic = topic_mapping_[out_topic][i].second; + + ROS_INFO("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->SendContent(out_topic, + content_info.type_md5_sum, + content_info.data, + content_info.data_size, + topic_info.relay_seqnum); + } +} + +void GenericROSSubRapidPub::ConvertRequest( + rapid::ext::astrobee::GenericCommsRequest const* data, + std::string const& connecting_robot) { + const std::lock_guard lock(m_mutex_); + + std::string out_topic, robot_out_topic = data->outputTopic; + bool found = false; + + // This is the output topic on the robot and may not match the keyed output + // topic so we need to find the keyed one + // First check if it is the keyed topic + auto search = topic_mapping_.find(robot_out_topic); + if (search != topic_mapping_.end()) { + out_topic = robot_out_topic; + found = true; + } else { + // If it is not the keyed topic, try to find it. + for (auto it = topic_mapping_.begin(); it != topic_mapping_.end() && !found; it++) { + for (size_t i = 0; it->second.size() && !found; i++) { + if (robot_out_topic == it->second[i].second) { + out_topic = it->first; + found = true; + } + } + } + } + + // Make sure we found the keyed topic + if (!found) { + ROS_ERROR("Received request for topic %s but it wasn't added to the ros sub rapid pub.\n", + robot_out_topic.c_str()); + return; + } + + std::map::iterator iter = m_relay_topics_.begin(); + while (iter != m_relay_topics_.end()) { + if (iter->second.out_topic == out_topic) + break; + iter++; + } + + if (iter == m_relay_topics_.end()) { + ROS_ERROR("Received request for topic %s but it wasn't added to the bridge subscriber.\n", + out_topic.c_str()); + return; + } + + ROS_INFO("Received reset for topic %s\n", out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(connecting_robot) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s\n", connecting_robot.c_str()); + return; + } + + const AdvertisementInfo &info = iter->second.ad_info; + robot_connections_[connecting_robot]->SendAdvertisementInfo(robot_out_topic, + info.latching, + info.data_type, + info.md5_sum, + info.definition); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_pub_request.cpp b/communications/comms_bridge/src/rapid_pub_request.cpp new file mode 100644 index 0000000000..396acd6b5f --- /dev/null +++ b/communications/comms_bridge/src/rapid_pub_request.cpp @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_pub_request.h" + +#include + +namespace ff { + +RapidPubRequest::RapidPubRequest(std::string const& robot_name) : + request_seq_(0) { + std::string dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_REQUEST_TOPIC; + ROS_INFO("Comms Bridge: DDS Publisher DDS request topic name: %s\n", + dds_topic_name.c_str()); + request_supplier_.reset(new RapidPubRequest::RequestSupplier( + dds_topic_name, "", "AstrobeeGenericCommsRequestProfile", "")); + + rapid::RapidHelper::initHeader(request_supplier_->event().hdr); +} + +RapidPubRequest::~RapidPubRequest() {} + +void RapidPubRequest::SendRequest(std::string const& output_topic) { + rapid::ext::astrobee::GenericCommsRequest &msg = request_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = ++request_seq_; + + // Currently the output topic can only be 128 characters long + unsigned int size = output_topic.size(); + if (size > 127) { + ROS_ERROR("Comms Bridge: Out topic for topic %s is greater than 128 characters. Please change idl.", + output_topic.c_str()); + size = 127; + } + memset(msg.outputTopic, 0, 128); + strncpy(msg.outputTopic, output_topic.data(), size); + msg.outputTopic[size] = '\0'; + + // Send message + request_supplier_->sendEvent(); +} + + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_sub_advertisement_info.cpp b/communications/comms_bridge/src/rapid_sub_advertisement_info.cpp new file mode 100644 index 0000000000..9bdf7833a7 --- /dev/null +++ b/communications/comms_bridge/src/rapid_sub_advertisement_info.cpp @@ -0,0 +1,57 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_sub_advertisement_info.h" + +#include + +namespace ff { + +RapidSubAdvertisementInfo::RapidSubAdvertisementInfo( + const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : GenericRapidSub(entity_name, subscribe_topic, subscriber_partition), + ros_pub_(rapid_msg_ros_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect( + this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidSubAdvertisementInfo exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidSubAdvertisementInfo exception unknown"); + throw; + } + + // Start thread + StartThread(); +} + +void RapidSubAdvertisementInfo::operator() ( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* ad_info) { + ros_pub_->ConvertAdvertisementInfo(ad_info); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_sub_content.cpp b/communications/comms_bridge/src/rapid_sub_content.cpp new file mode 100644 index 0000000000..1b3f255e4d --- /dev/null +++ b/communications/comms_bridge/src/rapid_sub_content.cpp @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_sub_content.h" + +#include + +namespace ff { + +RapidSubContent::RapidSubContent(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : GenericRapidSub(entity_name, subscribe_topic, subscriber_partition), + ros_pub_(rapid_msg_ros_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidSubContent exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidSubContent exception unknown"); + throw; + } + + // Start thread + StartThread(); +} + +void RapidSubContent::operator() ( + rapid::ext::astrobee::GenericCommsContent const* content) { + ros_pub_->ConvertContent(content, subscriber_partition_); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_sub_request.cpp b/communications/comms_bridge/src/rapid_sub_request.cpp new file mode 100644 index 0000000000..8817f2c1d5 --- /dev/null +++ b/communications/comms_bridge/src/rapid_sub_request.cpp @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_sub_request.h" + +#include + +namespace ff { + +RapidSubRequest::RapidSubRequest(const std::string& entity_name, + const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericROSSubRapidPub* ros_sub_rapid_pub) + : GenericRapidSub(entity_name, subscribe_topic, subscriber_partition), + ros_sub_(ros_sub_rapid_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidSubRequest exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidSubRequest exception unknown"); + throw; + } + + // Start thread + StartThread(); +} + +void RapidSubRequest::operator() ( + rapid::ext::astrobee::GenericCommsRequest const* request) { + ros_sub_->ConvertRequest(request, subscriber_partition_); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/util.cpp b/communications/comms_bridge/src/util.cpp new file mode 100644 index 0000000000..d90f9bd01c --- /dev/null +++ b/communications/comms_bridge/src/util.cpp @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include + +ros::Time comms_util::RapidTime2RosTime(const int64_t dds_time) { + ros::Time t; + + // DDS time is in microseconds, we need nanoseconds + t.fromNSec(static_cast(dds_time) * 1000); + + return t; +} + +int64_t comms_util::RosTime2RapidTime(ros::Time const& ros_time) { + return static_cast(ros_time.toNSec() / 1000ULL); +} + +void comms_util::RosHeader2Rapid(std_msgs::Header const& ros_hdr, + rapid::Header* rapid_hdr, + int status, + int serial) { + rapid::RapidHelper::initHeader(*rapid_hdr); + if (serial >= 0) { + rapid_hdr->serial = serial; + } + rapid_hdr->statusCode = status; + rapid_hdr->timeStamp = comms_util::RosTime2RapidTime(ros_hdr.stamp); +} + +void comms_util::RapidHeader2Ros(rapid::Header const& rapid_hdr, std_msgs::Header* ros_hdr, + std::string const& frame_id) { + ros_hdr->frame_id = frame_id; + ros_hdr->stamp = comms_util::RapidTime2RosTime(rapid_hdr.timeStamp); +} diff --git a/communications/dds_msgs/idl/AstrobeeConstants.idl b/communications/dds_msgs/idl/AstrobeeConstants.idl index cbe7caaed9..456e726ad6 100644 --- a/communications/dds_msgs/idl/AstrobeeConstants.idl +++ b/communications/dds_msgs/idl/AstrobeeConstants.idl @@ -34,6 +34,9 @@ module rapid { const String64 EPS_STATE_TOPIC = "astrobee_eps_state"; const String64 FAULT_CONFIG_TOPIC = "astrobee_fault_config"; const String64 FAULT_STATE_TOPIC = "astrobee_fault_state"; + const String64 GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC = "astrobee_generic_comms_advertisement_info"; + const String64 GENERIC_COMMS_CONTENT_TOPIC = "astrobee_generic_comms_content"; + const String64 GENERIC_COMMS_REQUEST_TOPIC = "astrobee_generic_comms_request"; const String64 GNC_CONTROL_STATE_TOPIC = "astrobee_gnc_control_state"; const String64 GNC_FAM_CMD_STATE_TOPIC = "astrobee_gnc_fam_cmd_state"; const String64 GUEST_SCIENCE_CONFIG_TOPIC = "astrobee_guest_science_config"; diff --git a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl new file mode 100644 index 0000000000..4267c03a31 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl @@ -0,0 +1,46 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef string<8192> String8K; + + //@copy-c-declaration class GenericCommsAdvertisementInfoTypeSupport; + //@copy-c-declaration class GenericCommsAdvertisementInfoDataWriter; + //@copy-c-declaration class GenericCommsAdvertisementInfoDataReader; + //@copy-c-declaration struct GenericCommsAdvertisementInfoSeq; + + //@copy-declaration /** + //@copy-declaration * Information of the topic type for generic comms. + //@copy-declaration */ + valuetype GenericCommsAdvertisementInfo : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsAdvertisementInfoTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoDataReader DataReader; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsAdvertisementInfo Type; + + //@copy-declaration /** Topic on which to republish */ + public String128 outputTopic; + + //@copy-declaration /** Whether republisher should advertise topic as latching */ + public boolean latching; + + //@copy-declaration /** ROS message data type name */ + public String128 dataType; + + //@copy-declaration /** ROS message md5sum of type */ + public String64 md5Sum; + + //@copy-declaration /** ROS message definition */ + public String8K msgDefinition; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsContent.idl b/communications/dds_msgs/idl/GenericCommsContent.idl new file mode 100644 index 0000000000..5ab9cd5a20 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsContent.idl @@ -0,0 +1,39 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsContentTypeSupport; + //@copy-c-declaration class GenericCommsContentDataWriter; + //@copy-c-declaration class GenericCommsContentDataReader; + //@copy-c-declaration struct GenericCommsContentSeq; + + //@copy-declaration /** + //@copy-declaration * Content of a generic comms ROS message + //@copy-declaration */ + valuetype GenericCommsContent : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsContentTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsContentDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsContentDataReader DataReader; + //@copy-c-declaration typedef GenericCommsContentSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsContent Type; + + //@copy-declaration /** Topic on which to republish */ + public String128 outputTopic; + + //@copy-declaration /** The md5 sum of msg type repeated from the original advertisement */ + public String64 md5Sum; + + //@copy-declaration /** Serialized content of the message */ + public OctetSequence128K data; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsRequest.idl b/communications/dds_msgs/idl/GenericCommsRequest.idl new file mode 100644 index 0000000000..c3929eb328 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsRequest.idl @@ -0,0 +1,33 @@ +/* + * Copy 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsRequestTypeSupport; + //@copy-c-declaration class GenericCommsRequestDataWriter; + //@copy-c-declaration class GenericCommsRequestDataReader; + //@copy-c-declaration struct GenericCommsRequestSeq; + + //@copy-declaration /** + //@copy-declaration * Request topic for the generic comms + //@copy-declaration */ + valuetype GenericCommsRequest : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsRequestTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsRequestDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsRequestDataReader DataReader; + //@copy-c-declaration typedef GenericCommsRequestSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsRequest Type; + + //@copy-declaration /** Topic for which metadata retransmission is requested */ + public String128 outputTopic; + }; + }; + }; +}; diff --git a/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc b/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc index 9cbfd3b547..ee87fe924b 100644 --- a/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc +++ b/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc @@ -83,7 +83,7 @@ void AstrobeeAstrobeeBridge::Initialize(ros::NodeHandle *nh) { std::string config_path = ff_common::GetConfigDir(); config_path += "/communications/dds_intercomms/"; - // Create fake argv containing only the particaptant name + // Create fake argv containing only the participant name // Participant name needs to uniue so combine robot name with timestamp ros::Time time = ros::Time::now(); participant_name_ = agent_name_ + std::to_string(time.sec) diff --git a/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg new file mode 100644 index 0000000000..f0d49d208b --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg @@ -0,0 +1,17 @@ +# timestamp in header should be time of whatever triggered the advertisement +std_msgs/Header header + +#topic on which to republish +string output_topic + +#whether should be republished as a latching topic +bool latching + +#standard ROS meanings for message descriptions +# eg std_msgs/Int32 +string data_type +# eg da5909fbe378aeaf85e547e830cc1bb7 +string md5_sum +# eg a definition such as this file +string definition + diff --git a/communications/ff_msgs/msg/GenericCommsContent.msg b/communications/ff_msgs/msg/GenericCommsContent.msg new file mode 100644 index 0000000000..c768922328 --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsContent.msg @@ -0,0 +1,13 @@ +# timestamp in header should be time of receipt of original message +std_msgs/Header header + +#topic on which to republish +string output_topic + +#md5sum of msg type, repeated from advertisement +string type_md5_sum + +#serialized content of the message +#definition, etc. assumed to be known from preceding advertisement +uint8[] msg + diff --git a/communications/ff_msgs/msg/GenericCommsReset.msg b/communications/ff_msgs/msg/GenericCommsReset.msg new file mode 100644 index 0000000000..397561bffd --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsReset.msg @@ -0,0 +1,7 @@ +# timestamp in header should be time of whatever triggered the reset +std_msgs/Header header + +#topic for which metadata retransmission is requested +string topic + + diff --git a/communications/ff_msgs/srv/SetExposure.srv b/communications/ff_msgs/srv/SetExposure.srv new file mode 100644 index 0000000000..301f3f94b2 --- /dev/null +++ b/communications/ff_msgs/srv/SetExposure.srv @@ -0,0 +1,23 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. # +# +# Service for setting camera exposure + +uint16 exposure +--- +bool success +string status_message diff --git a/communications/readme.md b/communications/readme.md index 81177cbefd..970753285b 100644 --- a/communications/readme.md +++ b/communications/readme.md @@ -1,3 +1,4 @@ \page comms Communications \subpage dds_ros_bridge +\subpage comms_bridge diff --git a/debian/astrobee-comms.files b/debian/astrobee-comms.files index 074fa03445..2a62ae9d47 100644 --- a/debian/astrobee-comms.files +++ b/debian/astrobee-comms.files @@ -1,3 +1,5 @@ opt/astrobee/lib/libdds_ros_bridge.so +opt/astrobee/lib/libcomms_bridge.so opt/astrobee/lib/librapidExtAstrobee.so opt/astrobee/share/dds_ros_bridge +opt/astrobee/share/comms_bridge diff --git a/debian/changelog b/debian/changelog index 40598cae8e..41f219a17f 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,11 @@ +astrobee (0.17.2) UNRELEASED; urgency=medium + + * Adding Generic comms bridge + * Multiple bug fixes + * Ubuntu 20 only supported now + + -- Astrobee Flight Software Mon, 12 Feb 2024 12:33:07 -0800 + astrobee (0.17.1) testing; urgency=medium * Multiple bug fixes diff --git a/description/description/urdf/model_carriage.urdf.xacro b/description/description/urdf/model_carriage.urdf.xacro index 34cae175f8..42bf9d8a4e 100644 --- a/description/description/urdf/model_carriage.urdf.xacro +++ b/description/description/urdf/model_carriage.urdf.xacro @@ -25,13 +25,13 @@ - + - + @@ -49,7 +49,7 @@ - + 100000.0 100000.0 0.0 diff --git a/doc/general_documentation/INSTALL.md b/doc/general_documentation/INSTALL.md index f220e976ae..86418be858 100644 --- a/doc/general_documentation/INSTALL.md +++ b/doc/general_documentation/INSTALL.md @@ -107,7 +107,7 @@ instead: ./src/scripts/configure.sh -l -F -D -p $INSTALL_PATH -w $WORKSPACE_PATH -*Note: If a workspace is specified but not an explicit install distectory, +*Note: If a workspace is specified but not an explicit install directory, install location will be $WORKSPACE_PATH/install.* *Note: Make sure you use the -F and -D flags. If these flags are not used, the diff --git a/doc/general_documentation/NASA_INSTALL.md b/doc/general_documentation/NASA_INSTALL.md index 32265036b6..a1a707c2be 100644 --- a/doc/general_documentation/NASA_INSTALL.md +++ b/doc/general_documentation/NASA_INSTALL.md @@ -34,7 +34,7 @@ your life greatly. Verify that you are in this situation with the command below should succeed (certificate will be added later; remove the Release.gpg file after being fetched). - wget -v --no-check-certificate http://astrobee.ndc.nasa.gov/software/dists/xenial/Release.gpg + wget -v --no-check-certificate https://astrobee.ndc.nasa.gov/software_new/dists/focal/Release.gpg Before running the scripts in `scripts/setup` below, set this variable: @@ -155,7 +155,7 @@ instead: ./scripts/configure.sh -l -p $INSTALL_PATH -w $WORKSPACE_PATH -*Note: If a workspace is specified but not an explicit install distectory, +*Note: If a workspace is specified but not an explicit install directory, install location will be $WORKSPACE_PATH/install.* @@ -183,7 +183,7 @@ For more information on running the simulator and moving the robot, please see t ## Cross-compile - Running the code on a real robot -In order to do this, you will need to followe the cross-compile build +In order to do this, you will need to follow the cross-compile build instructions. ### Cross-compile setup @@ -202,11 +202,18 @@ Next, download the cross toolchain and install the chroot: mkdir -p $ARMHF_TOOLCHAIN cd $HOME/arm_cross - $ASTROBEE_WS/src/submodules/platform/fetch_toolchain.sh - $ASTROBEE_WS/src/submodules/platform/rootfs/make_chroot.sh xenial dev $ARMHF_CHROOT_DIR -*Note: The last script shown above needs the packages `qemu-user-static` (not -`qemu-arm-static`) and `multistrap` to be installed (can be installed through apt).* + DIST=$(. /etc/os-release && echo $UBUNTU_CODENAME) + if [ ${DIST} == "kinetic" ]; then + # Use pre-built toolchain for 16.04 + $ASTROBEE_WS/src/submodules/platform/fetch_toolchain.sh + else + # Use packaged toolchain for 20.04 + sudo apt install -y gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf + fi + sudo apt install -y qemu-user-static multistrap + + $ASTROBEE_WS/src/submodules/platform/rootfs/make_chroot.sh ${DIST} dev $ARMHF_CHROOT_DIR ### Cross-compile build diff --git a/doc/general_documentation/code_style.md b/doc/general_documentation/code_style.md index 1e8d8f8dfd..d33529d38b 100644 --- a/doc/general_documentation/code_style.md +++ b/doc/general_documentation/code_style.md @@ -1,3 +1,3 @@ \page astrobee-code-style Astrobee code style -The Astrobee code uses [cpplint](https://en.wikipedia.org/wiki/Cpplint) to enforce a consistent and uniform coding style. Code which fails this tool cannot be committed with Git. It is suggested that the [clang-format-8](https://launchpad.net/ubuntu/bionic/+package/clang-format-8) program be installed on your machine. The *git commit* command will invoke this tool which will fix most (but not all) of the formatting errors using as a guide the ``.clang-format`` style file at the base of the Astrobee repository. +The Astrobee code uses [cpplint](https://en.wikipedia.org/wiki/Cpplint) to enforce a consistent and uniform coding style. Code which fails this tool cannot be committed with Git. It is suggested that the [clang-format-8](https://launchpad.net/ubuntu/focal/+package/clang-format-8) program be installed on your machine. The *git commit* command will invoke this tool which will fix most (but not all) of the formatting errors using as a guide the ``.clang-format`` style file at the base of the Astrobee repository. diff --git a/hardware/is_camera/include/is_camera/camera.h b/hardware/is_camera/include/is_camera/camera.h index 881cc6dd76..b65d4b589f 100644 --- a/hardware/is_camera/include/is_camera/camera.h +++ b/hardware/is_camera/include/is_camera/camera.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -79,6 +80,7 @@ class CameraNodelet : public ff_util::FreeFlyerNodelet { void EnableBayer(bool enable); void AutoExposure(); size_t getNumBayerSubscribers(); + bool SetExposure(ff_msgs::SetExposure::Request &req, ff_msgs::SetExposure::Response &res); sensor_msgs::CameraInfo info_msg_; @@ -93,6 +95,7 @@ class CameraNodelet : public ff_util::FreeFlyerNodelet { ros::Publisher info_pub_; ros::Publisher bayer_pub_; ros::Publisher pub_exposure_; + ros::ServiceServer srv_exposure_; std::shared_ptr v4l_; config_reader::ConfigReader config_; diff --git a/hardware/is_camera/src/camera.cc b/hardware/is_camera/src/camera.cc index a9e0667749..46f4071cad 100644 --- a/hardware/is_camera/src/camera.cc +++ b/hardware/is_camera/src/camera.cc @@ -225,9 +225,11 @@ namespace is_camera { false, true); pub_ = nh->advertise(camera_topic_, 1); - info_pub_ = nh->advertise(camera_topic_ + "/camera_info", 1); + info_pub_ = nh->advertise(camera_topic_ + TOPIC_HARDWARE_CAM_INFO, 1); pub_exposure_ = nh->advertise(camera_topic_ + "_ctrl", 1); + srv_exposure_ = nh->advertiseService(camera_topic_ + SERVICE_SET_EXPOSURE, &CameraNodelet::SetExposure, this); + // Allocate space for our output msg buffer for (size_t i = 0; i < kImageMsgBuffer; i++) { img_msg_buffer_[i].reset(new sensor_msgs::Image()); @@ -243,6 +245,26 @@ namespace is_camera { thread_ = std::thread(&CameraNodelet::PublishLoop, this); } + // Set the exposure + bool CameraNodelet::SetExposure(ff_msgs::SetExposure::Request &req, + ff_msgs::SetExposure::Response &res) { + if (thread_running_ && !auto_exposure_) { + // Set exposure + camera_exposure_ = req.exposure; + v4l_->SetParameters(camera_gain_, camera_exposure_); + // Success! + res.success = true; + res.status_message = "Success"; + } else { + // Failed + res.success = false; + res.status_message = "Failed! Either thread not running (" + std::to_string(thread_running_) + + ") or auto-exposure on (" + std::to_string(auto_exposure_) + ")"; + } + + return true; + } + size_t CameraNodelet::getNumBayerSubscribers(void) { if (bayer_enable_) { return bayer_pub_.getNumSubscribers(); @@ -273,7 +295,7 @@ namespace is_camera { } if (!camera.GetInt("exposure", &camera_exposure_)) { - FF_FATAL("Gain not specified."); + FF_FATAL("Exposure not specified."); exit(EXIT_FAILURE); } diff --git a/hardware/pico_driver/scripts/debug_pico_utils.py b/hardware/pico_driver/scripts/debug_pico_utils.py index 1848530e78..59c5da902b 100755 --- a/hardware/pico_driver/scripts/debug_pico_utils.py +++ b/hardware/pico_driver/scripts/debug_pico_utils.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/hardware/pico_driver/scripts/pico_check_split_extended.py b/hardware/pico_driver/scripts/pico_check_split_extended.py index 90253453fb..82ef553d54 100755 --- a/hardware/pico_driver/scripts/pico_check_split_extended.py +++ b/hardware/pico_driver/scripts/pico_check_split_extended.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/hardware/pico_driver/scripts/pico_split_extended.py b/hardware/pico_driver/scripts/pico_split_extended.py index 3a095e77a4..39fd0f380e 100755 --- a/hardware/pico_driver/scripts/pico_split_extended.py +++ b/hardware/pico_driver/scripts/pico_split_extended.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/hardware/pico_driver/scripts/pico_utils.py b/hardware/pico_driver/scripts/pico_utils.py index c80845579e..32931d24d7 100644 --- a/hardware/pico_driver/scripts/pico_utils.py +++ b/hardware/pico_driver/scripts/pico_utils.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/hardware/pico_driver/scripts/pico_write_xyz_coeff.py b/hardware/pico_driver/scripts/pico_write_xyz_coeff.py index aad7716301..c7416fc580 100755 --- a/hardware/pico_driver/scripts/pico_write_xyz_coeff.py +++ b/hardware/pico_driver/scripts/pico_write_xyz_coeff.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/localization/camera/src/camera_model.cc b/localization/camera/src/camera_model.cc index 274323eb60..a2c998b321 100644 --- a/localization/camera/src/camera_model.cc +++ b/localization/camera/src/camera_model.cc @@ -70,12 +70,12 @@ void CameraModel::SetTransform(const Eigen::Affine3d & cam_t_global) { double CameraModel::GetFovX(void) const { // This is an approximation since it doesn't take in account lens distortion - return atan(1.0 / (params_.GetFocalVector()[0] * params_.GetDistortedHalfSize()[0])) * 2; + return atan(params_.GetDistortedHalfSize()[0] / params_.GetFocalVector()[0] ) * 2; } double CameraModel::GetFovY(void) const { // This is an approximation since it doesn't take in account lens distortion - return atan(1.0 / (params_.GetFocalVector()[1] * params_.GetDistortedHalfSize()[1])) * 2; + return atan(params_.GetDistortedHalfSize()[1] / params_.GetFocalVector()[1] ) * 2; } const camera::CameraParameters& CameraModel::GetParameters() const { diff --git a/localization/localization_common/scripts/localization_common/utilities.py b/localization/localization_common/scripts/localization_common/utilities.py index 8c5ec28463..17708cc26f 100644 --- a/localization/localization_common/scripts/localization_common/utilities.py +++ b/localization/localization_common/scripts/localization_common/utilities.py @@ -20,8 +20,6 @@ import os import subprocess -import pandas as pd - # Forward errors so we can recover failures # even when running commands through multiprocessing @@ -68,6 +66,9 @@ def create_directory(directory=None): def load_dataframe(files): + # Importing the module only within this function + import pandas as pd + dataframes = [pd.read_csv(file) for file in files] dataframe = pd.concat(dataframes) return dataframe diff --git a/localization/sparse_mapping/tools/generate_hugin.py b/localization/sparse_mapping/tools/generate_hugin.py index ef4196186a..ace166e91d 100755 --- a/localization/sparse_mapping/tools/generate_hugin.py +++ b/localization/sparse_mapping/tools/generate_hugin.py @@ -144,10 +144,8 @@ def main(): srcImage.setFilename(img) p.addImage(srcImage) - # make a c++ std::ofstream to write to - ofs = ofstream(output_hugin) # write the modified panorama to that stream - p.writeData(ofs) + p.WritePTOFile(output_hugin) # done with it del ofs diff --git a/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h b/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h index 22acee377e..6f6d5197ce 100644 --- a/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h +++ b/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h @@ -33,6 +33,7 @@ #include #include +#include #include #include @@ -74,7 +75,7 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { private: // Get the PIDs of the nodes to monitor - void GetPIDs(ros::TimerEvent const &te); + void GetPIDs(); // Assert CPU loads and report if too high void AssertCPULoadHighFaultCallback(ros::TimerEvent const& te); @@ -119,18 +120,19 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { virt_percentage; }; + std::shared_timed_mutex pid_lock_; + config_reader::ConfigReader config_params_; ros::Publisher cpu_state_pub_; // Cpu stats publisher ros::Publisher mem_state_pub_; // Memory stats publisher ros::Timer reload_params_timer_; // Ckeck if parameters were updated - ros::Timer pid_timer_; // Update PIDs ros::Timer stats_timer_; // Update stats ros::Timer assert_cpu_load_fault_timer_; // Check cpu load limits ros::Timer clear_cpu_load_fault_timer_; // Clear cpu fault timer ros::Timer assert_mem_load_fault_timer_; // Check memory load limits ros::Timer clear_mem_load_fault_timer_; // Clear memory fault timer int pub_queue_size_; // Monitor publishing queue size - double update_freq_hz_, update_pid_hz_; // Publishing and PID update frequency + double update_freq_hz_; // Publishing update frequency struct sysinfo mem_info_; // Scope memory info from sysinfo unsigned int ncpus_; // Number of cpu's diff --git a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc index 12c5881e2e..1338960b8a 100644 --- a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc +++ b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc @@ -84,13 +84,6 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { true, false); - // Timer for updating PID values of interest nodes - pid_timer_ = nh->createTimer(ros::Duration(1 / update_pid_hz_), - &CpuMemMonitor::GetPIDs, - this, - false, - true); - // Timer for checking cpu and memory stats. Timer is not one shot and start it right away stats_timer_ = nh->createTimer(ros::Duration(1 / update_freq_hz_), &CpuMemMonitor::PublishStatsCallback, @@ -155,6 +148,9 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { // Initialize the memory state message mem_state_msg_.name = monitor_host_; mem_state_msg_.nodes.resize(nodes_pid_.size()); + + std::thread pid_thread(&cpu_mem_monitor::CpuMemMonitor::GetPIDs, this); + pid_thread.detach(); } bool CpuMemMonitor::ReadParams() { @@ -171,15 +167,6 @@ bool CpuMemMonitor::ReadParams() { config_reader::ConfigReader::Table processor_config(&config_params_, processor_name_.c_str()); - // get udpate pid frequency - if (!processor_config.GetPosReal("update_pid_hz", &update_pid_hz_)) { - err_msg = "CPU monitor: Update PID frequency not specified for " + - processor_name_; - FF_ERROR(err_msg); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); - return false; - } - // get udpate stats frequency if (!processor_config.GetPosReal("update_freq_hz", &update_freq_hz_)) { err_msg = "CPU monitor: Update frequency not specified for " + @@ -255,16 +242,18 @@ bool CpuMemMonitor::ReadParams() { FF_ERROR(err_msg); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); } - for (int i = 0; i < nodes.GetSize(); i++) { - config_reader::ConfigReader::Table node; - if (!nodes.GetTable(i + 1, &node)) { - FF_ERROR("Could not get node table"); - return false; - } - std::string name; - if (node.GetStr("name", &name)) { - NODELET_DEBUG_STREAM("Read node " << name); - nodes_pid_.insert(std::pair(name, 0)); + if (nodes_pid_.size() == 0) { // modifying nodes while running not supported + for (int i = 0; i < nodes.GetSize(); i++) { + config_reader::ConfigReader::Table node; + if (!nodes.GetTable(i + 1, &node)) { + FF_ERROR("Could not get node table"); + return false; + } + std::string name; + if (node.GetStr("name", &name)) { + NODELET_DEBUG_STREAM("Read node " << name); + nodes_pid_.insert(std::pair(name, 0)); + } } } return true; @@ -308,7 +297,7 @@ void CpuMemMonitor::ClearMemLoadHighFaultCallback(ros::TimerEvent const& te) { load_fault_state_ = CLEARED; } -void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { +void CpuMemMonitor::GetPIDs() { // Go through all the node list and get the PID XmlRpc::XmlRpcValue args, result, payload; std::map::iterator it; @@ -321,11 +310,13 @@ void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { args[0] = ros::this_node::getName(); args[1] = it->first; if (!ros::master::execute("lookupNode", args, result, payload, true)) { + std::unique_lock lock(pid_lock_); it->second = -1; continue; } std::string node_host = getHostfromURI(result[2]); if (node_host.empty()) { + std::unique_lock lock(pid_lock_); it->second = -1; continue; } @@ -333,6 +324,7 @@ void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { // If it is not in the same cpu if (node_host != monitor_host_) { // Insert it on the list + std::unique_lock lock(pid_lock_); it->second = -1; std::string err_msg = "CPU Memory Monitor: Specified node " + it->first + "in" + monitor_host_ + " and not in the same cpu as manager " + monitor_host_ + "."; @@ -346,6 +338,7 @@ void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { FILE* pipe = popen(("rosnode info " + it->first + " 2>/dev/null | grep Pid| cut -d' ' -f2").c_str(), "r"); if (!pipe) { + std::unique_lock lock(pid_lock_); it->second = -1; std::string err_msg = "CPU Memory Monitor: Could not open rosnode process for node " + it->first; FF_ERROR(err_msg); @@ -357,6 +350,7 @@ void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { if (pid.empty()) { // Node not found + std::unique_lock lock(pid_lock_); it->second = -1; std::string err_msg = "CPU Memory Monitor: Specified node " + it->first + "does not have a PID."; @@ -365,6 +359,7 @@ void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { } pclose(pipe); // Insert it on the list + std::unique_lock lock(pid_lock_); it->second = std::stoi(pid); } } @@ -484,6 +479,7 @@ int CpuMemMonitor::CollectCPUStats() { std::map::iterator it; std::map::iterator it_load; int i; + std::shared_lock lock(pid_lock_); for ( i = 0, it = nodes_pid_.begin(); it != nodes_pid_.end(); i++, it++ ) { // Look if PID is invalid if (it->second <= 0) @@ -625,6 +621,7 @@ int CpuMemMonitor::CollectMemStats() { // Go through all the node list and check memory usage int i = -1; std::map::iterator it; + std::shared_lock lock(pid_lock_); for ( it = nodes_pid_.begin(); it != nodes_pid_.end(); it++ ) { // Increment counter i++; diff --git a/management/executive/tools/plan_pub.cc b/management/executive/tools/plan_pub.cc index a263e73f7d..5f6a2c2777 100644 --- a/management/executive/tools/plan_pub.cc +++ b/management/executive/tools/plan_pub.cc @@ -42,6 +42,8 @@ #include #include +#define MAX_COUNT 6 + namespace fs = boost::filesystem; namespace io = boost::iostreams; @@ -57,6 +59,8 @@ bool ValidateCompression(const char* name, std::string const &value) { DEFINE_string(compression, "none", "Type of compression [none, deflate, gzip]"); +DEFINE_string(ns, "", "Robot namespace"); +DEFINE_bool(remote, false, "Whether target command is remote robot"); constexpr uintmax_t kMaxSize = 128 * 1024; @@ -75,7 +79,10 @@ void on_cf_ack(ff_msgs::CompressedFileAckConstPtr const& cf_ack) { // compressed file ack is latched so we need to check the timestamp to make // sure this plan is being acked // ROS_WARN_STREAM(plan_pub_time << " : " << cf_ack->header.stamp); - if (plan_pub_time <= cf_ack->header.stamp) { + + // If remote and in the granite lab, the clocks of the robots might not be + // properly synchronized because we do it manually + if (plan_pub_time <= cf_ack->header.stamp + ros::Duration(5.0)) { ROS_INFO("Compressed file ack is valid! Sending set plan!"); ff_msgs::CommandStamped cmd; cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_PLAN; @@ -90,7 +97,10 @@ void on_plan_status(ff_msgs::PlanStatusStamped::ConstPtr const& ps) { // plan status is latched so we need to check the timestamp to make sure this // plan is loaded // ROS_WARN_STREAM(plan_pub_time << " : " << ps->header.stamp); - if (plan_pub_time <= ps->header.stamp) { + + // If remote and in the granite lab, the clocks of the robots might not be + // properly synchronized because we do it manually + if (plan_pub_time <= ps->header.stamp + ros::Duration(5.0)) { ff_msgs::CommandStamped cmd; cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_RUN_PLAN; cmd.subsys_name = "Astrobee"; @@ -104,7 +114,7 @@ void on_plan_status(ff_msgs::PlanStatusStamped::ConstPtr const& ps) { int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); ros::init(argc, argv, "plan_pub"); - ros::NodeHandle n; + ros::NodeHandle n(std::string("/") + FLAGS_ns); ros::Time::waitForValid(); @@ -172,11 +182,19 @@ int main(int argc, char** argv) { ros::Subscriber plan_status_sub = n.subscribe(TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, 10, &on_plan_status); - while (cf_ack_sub.getNumPublishers() == 0 || + // Timeout if it can't find anything in 3s or if remote + // If remote, this wait allows enough spin + int count = 0; + while ((cf_ack_sub.getNumPublishers() == 0 || plan_status_sub.getNumPublishers() == 0 || - command_pub.getNumSubscribers() == 0) { + command_pub.getNumSubscribers() == 0) && count < MAX_COUNT) { + count++; ros::Duration(0.5).sleep(); } + if (count == MAX_COUNT && !FLAGS_remote) { + ROS_ERROR("Could not connect"); + return 1; + } ros::Publisher plan_pub = n.advertise(sub_topic_plan, 10, diff --git a/management/executive/tools/teleop_tool.cc b/management/executive/tools/teleop_tool.cc index f38bffe202..d0759dfe32 100644 --- a/management/executive/tools/teleop_tool.cc +++ b/management/executive/tools/teleop_tool.cc @@ -55,6 +55,7 @@ DEFINE_bool(undock, false, "Send undock command"); DEFINE_bool(relative, false, "Position is relative to current point"); DEFINE_bool(reset_bias, false, "Send initialize bias command"); DEFINE_bool(reset_ekf, false, "Send reset ekf command"); +DEFINE_bool(remote, false, "Whether target command is remote robot"); DEFINE_double(accel, -1.0, "Desired acceleration"); DEFINE_double(alpha, -1.0, "Desired angular acceleration"); @@ -586,6 +587,7 @@ void AckCallback(ff_msgs::AckStampedConstPtr const& ack) { std::cout << "\n" << ack->cmd_id << " command failed! " << ack->message; std::cout << "\n"; ros::shutdown(); + exit(1); return; } if (Finished()) { @@ -736,7 +738,7 @@ int main(int argc, char** argv) { // Hacky time out int count = 0; - while (ack_sub.getNumPublishers() == 0) { + while (ack_sub.getNumPublishers() == 0 && !FLAGS_remote) { ros::Duration(0.2).sleep(); // Only wait 2 seconds if (count == 9) { @@ -748,7 +750,7 @@ int main(int argc, char** argv) { } // If the user wants to get pose or move, get the current pose of the robot - if (FLAGS_get_pose || FLAGS_move) { + if (FLAGS_get_pose || (FLAGS_move && !FLAGS_remote)) { std::string ns = FLAGS_ns; // Wait for transform listener to start up ros::Duration(1.0).sleep(); @@ -802,7 +804,7 @@ int main(int argc, char** argv) { dock_sub = nh.subscribe(topic_name, 10, &DockFeedbackCallback); // Hacky time out int dock_count = 0; - while (dock_sub.getNumPublishers() == 0) { + while (dock_sub.getNumPublishers() == 0 && !FLAGS_remote) { ros::Duration(0.2).sleep(); // Only wait 2 seconds if (dock_count == 9) { @@ -814,6 +816,16 @@ int main(int argc, char** argv) { } } + // If remote, spin for seconds + if (FLAGS_remote) { + ros::Rate loop_rate(10); + ros::Time start_time = ros::Time::now(); + + // Spin for 3 seconds + while (ros::Time::now() - start_time < ros::Duration(3.0)) + loop_rate.sleep(); + } + if (!SendNextCommand()) { return 1; } diff --git a/scripts/calibrate/intrinsics_calibrate.py b/scripts/calibrate/intrinsics_calibrate.py index 0b64a302db..f5ad81aa39 100755 --- a/scripts/calibrate/intrinsics_calibrate.py +++ b/scripts/calibrate/intrinsics_calibrate.py @@ -222,7 +222,7 @@ def calibrate_camera( return None return ( - bag_dir + "/camchain-" + bag_name + ".yaml" + bag_dir + "/" + bag_name + "-camchain.yaml" ) # The file where kalibr writes its output diff --git a/scripts/calibrate/readme.md b/scripts/calibrate/readme.md index 2e4193d7d3..ce4a0970c1 100644 --- a/scripts/calibrate/readme.md +++ b/scripts/calibrate/readme.md @@ -7,52 +7,17 @@ This folder contains various scripts for calibration. - Build and install the Astrobee code on the robot. - Install Kalibr on your computer. -## Installation instructions for Kalibr for Ubuntu 18.04 +## Installation instructions for Kalibr for Ubuntu 20.04 - sudo apt install python-rosinstall ipython python-software-properties \ - python-git ipython python-catkin-tools - sudo apt install libopencv-dev ros-melodic-vision-opencv +Install Kalibr following the instructions at: -# Install pip and use it to install python packages +https://github.com/ethz-asl/kalibr -We assume that Python 2 is used. - - curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py - sudo python get-pip.py - sudo -H pip install testresources - sudo -H pip install python-igraph==0.8 --upgrade - sudo -H pip install numpy==1.15.0 opencv-python==4.2.0.32 - -If necessary, pip and the other packages can be installed in user -space. The PYTHONPATH may need to be set for such packages. -The pip flags --user, --force, and --verbose may be useful. - -Kalibr uses Python 2, and many packages are transitioning to Python 3, -so some effort may be needed to install the Python 2 dependencies. - - # Build using catkin - export KALIBR_WS=$HOME/source/kalibr_workspace - mkdir -p $KALIBR_WS/src - cd $KALIBR_WS - source /opt/ros/melodic/setup.bash - catkin init - catkin config --extend /opt/ros/melodic - catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release - cd $KALIBR_WS/src - git clone git@github.com:oleg-alexandrov/Kalibr.git - -If the above cloning operation fails, it is likely due to some ssh settings on your machine not being as GitHub expects. Then try: +However, check out our fork which supports the assymetric aprilgrid +installed on Astrobee's dock: git clone https://github.com/oleg-alexandrov/Kalibr.git -In either case, continue as follows: - - # This line takes care of catkin not finding numpy. - ln -s /usr/local/lib/python2.7/dist-packages/numpy/core/include/numpy \ - $KALIBR_WS/src/Kalibr/Schweizer-Messer/numpy_eigen/include - - catkin build -DCMAKE_BUILD_TYPE=Release -j4 - # Prepare the environment. This is expected for all the steps below. Run, for example: diff --git a/scripts/configure.sh b/scripts/configure.sh index 7ae438818d..51c3f29b5e 100755 --- a/scripts/configure.sh +++ b/scripts/configure.sh @@ -266,8 +266,6 @@ ff_path=`canonicalize ${rootpath}` DIST=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` if [ "$DIST" = "xenial" ]; then ros_version=kinetic -elif [ "$DIST" = "bionic" ]; then - ros_version=melodic elif [ "$DIST" = "focal" ]; then ros_version=noetic fi diff --git a/scripts/docker/astrobee_msgs.Dockerfile b/scripts/docker/astrobee_msgs.Dockerfile index bd0df08cf9..9111c03337 100644 --- a/scripts/docker/astrobee_msgs.Dockerfile +++ b/scripts/docker/astrobee_msgs.Dockerfile @@ -19,11 +19,11 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 FROM ubuntu:${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON="3" # try to suppress certain warnings during apt-get calls ARG DEBIAN_FRONTEND=noninteractive diff --git a/scripts/docker/build.sh b/scripts/docker/build.sh index 36558549c4..6adabd7317 100755 --- a/scripts/docker/build.sh +++ b/scripts/docker/build.sh @@ -23,7 +23,6 @@ usage: build.sh [-h] [-x] [-b] [-f] [-r] [-o ] [-d] -h or --help: Print this help -x or --xenial: Build Ubuntu 16.04 docker images --b or --bionic: Build Ubuntu 18.04 docker images -f or --focal: Build Ubuntu 20.04 docker images -r or --remote: Build first target on top of a pre-built remote image -o or --owner: Set ghcr.io owner for push action (default: nasa) @@ -55,8 +54,8 @@ usage() # Parse options 1 (validate and normalize with getopt) ###################################################################### -shortopts="h,x,b,f,r,o:,v:,d" -longopts="help,xenial,bionic,focal,remote,owner:,revision:,dry-run" +shortopts="h,x,f,r,o:,v:,d" +longopts="help,xenial,focal,remote,owner:,revision:,dry-run" opts=$(getopt -a -n build.sh --options "$shortops" --longoptions "$longopts" -- "$@") if [ $? -ne 0 ]; then echo @@ -91,8 +90,6 @@ while [ "$1" != "" ]; do ;; --xenial ) os="xenial" ;; - --bionic ) os="bionic" - ;; --focal ) os="focal" ;; --remote ) remote="true" @@ -158,11 +155,7 @@ UBUNTU_VERSION=16.04 ROS_VERSION=kinetic PYTHON='' -if [ "$os" = "bionic" ]; then - UBUNTU_VERSION=18.04 - ROS_VERSION=melodic - PYTHON='' -elif [ "$os" = "focal" ]; then +if [ "$os" = "focal" ]; then UBUNTU_VERSION=20.04 ROS_VERSION=noetic PYTHON='3' @@ -209,6 +202,7 @@ build () { --build-arg UBUNTU_VERSION=${UBUNTU_VERSION} \ --build-arg ROS_VERSION=${ROS_VERSION} \ --build-arg PYTHON=${PYTHON} \ + --build-arg REVISION=${tag_revision} \ $remote_args \ -t astrobee/astrobee:${tag_revision}${tag_stage}ubuntu${UBUNTU_VERSION} { set +x; } 2>/dev/null @@ -245,7 +239,7 @@ cd "${checkout_dir}" { set +x; } 2>/dev/null if [ "$build_astrobee_base" = "true" ]; then - build astrobee_base "${revision}-" "base-" + build astrobee_base "latest-" "base-" fi if [ "$build_astrobee" = "true" ]; then @@ -253,7 +247,7 @@ if [ "$build_astrobee" = "true" ]; then fi if [ "$build_test_astrobee" = "true" ]; then - build test_astrobee "" "test-" + build test_astrobee "${revision}-" "test-" fi if [ "$astrobee_quick" = "true" ]; then diff --git a/scripts/docker/build_msgs_jar.Dockerfile b/scripts/docker/build_msgs_jar.Dockerfile index 7f0b3fd5d6..8457522a9d 100644 --- a/scripts/docker/build_msgs_jar.Dockerfile +++ b/scripts/docker/build_msgs_jar.Dockerfile @@ -21,18 +21,22 @@ ARG REMOTE=astrobee ARG REPO=astrobee -FROM ${REMOTE}/${REPO}:msgs-ubuntu16.04 +FROM ${REMOTE}/${REPO}:msgs-ubuntu20.04 -RUN apt-get update && apt-get install -y \ - unzip \ - libc6-dev-i386 \ - lib32z1 \ - python-wstool \ - openjdk-8-jdk \ - ros-kinetic-rosjava \ - && rm -rf /var/lib/apt/lists/* +COPY scripts/setup/debians/rosjava /src/msgs/src/scripts -# Compile msg jar files, genjava_message_artifacts only works with bash +# make sure there is a python binary +RUN update-alternatives --install /usr/bin/python python /usr/bin/python3 1 + +# install dependencies for rosjava build script +RUN apt-get update && apt-get install -y devscripts equivs + +# build rosjava debians +RUN cd /src/msgs/src/scripts \ + && /bin/bash build_rosjava_debians.sh --install-with-deps \ + && rm -rf /var/lib/apt/lists/* + +# compile msg jar files, genjava_message_artifacts only works with bash RUN ["/bin/bash", "-c", "cd /src/msgs \ && catkin config \ && catkin build \ diff --git a/scripts/docker/readme.md b/scripts/docker/readme.md index d4c2738963..13a47a4fe6 100644 --- a/scripts/docker/readme.md +++ b/scripts/docker/readme.md @@ -145,7 +145,7 @@ All pre-built remote images are available on [GitHub here](https://github.com/na By default, the build script will automatically detect your host's Ubuntu OS version and configure the Docker image to use the same version using `Dockerfile` `ARGS`. -However, there is no requirement for the host OS and the Docker image OS to match. You can override the default and select a specific Docker image Ubuntu version by specifying `--xenial`, `--bionic`, or `--focal` for Ubuntu 16.04, 18.04, or 20.04 docker images, respectively. +However, there is no requirement for the host OS and the Docker image OS to match. You can override the default and select a specific Docker image Ubuntu version by specifying `--xenial` or `--focal` for Ubuntu 16.04 or 20.04 docker images, respectively. For more information about all the build arguments: @@ -155,14 +155,13 @@ For more information about all the build arguments: The `build.sh` script normally manages these `Dockerfile` `ARGS` but you can set them yourself if you run `docker build` manually: -- `UBUNTU_VERSION` - The version of Ubuntu to use. Valid values are "16.04", "18.04", and "20.04". +- `UBUNTU_VERSION` - The version of Ubuntu to use. Valid values are "16.04" and "20.04". - `ROS_VERSION` - The version of ROS to use. Valid values are "kinetic", "melodic", and "noetic". - `PYTHON` - The version of Python to use. Valid values are "" (an empty string representing Python 2) and "3". Constraints: - If `UBUNTU_VERSION` is `"16.04"`, `ROS_VERSION` and `PYTHON` must be `"kinetic"` and `""` respectively. -- If `UBUNTU_VERSION` is `"18.04"`, `ROS_VERSION` and `PYTHON` must be `"melodic"` and `""` respectively. -- If `UBUNTU_VERSION` is `"20.04"`, `ROS_VERSION` and `PYTHON` must be `"neotic"` and `"3"` respectively. +- If `UBUNTU_VERSION` is `"20.04"`, `ROS_VERSION` and `PYTHON` must be `"noetic"` and `"3"` respectively. The Docker files also accept args to use local or container registry images. @@ -223,8 +222,8 @@ argument parsing more predictable.) As with `build.sh`, by default, the docker image OS version will be configured to match your host's OS version, but you can override that -by specifying the `--xenial`, `--bionic`, or `--focal` option for -Ubuntu 16.04, 18.04, or 20.04 docker images, respectively. +by specifying the `--xenial` or `--focal` option for +Ubuntu 16.04 or 20.04 docker images, respectively. Use `--remote` to fetch and run a pre-built Astrobee docker image. (Omit `--remote` to run using a docker image built locally by @@ -260,7 +259,7 @@ The package argument is optional. The default is to build/test all packages. If debugging a CI failure that is specific to a particular OS version, -remember to pass `run.sh` the `--xenial`, `--bionic`, or `--focal` +remember to pass `run.sh` the `--xenial` or `--focal` option to select the right OS version to replicate the failure. Note: integration tests that use Gazebo simulation will be silently diff --git a/scripts/docker/run.sh b/scripts/docker/run.sh index 37e2fc2c45..e6641d143f 100755 --- a/scripts/docker/run.sh +++ b/scripts/docker/run.sh @@ -24,7 +24,6 @@ usage: run.sh [-h] [-x] [-b] [-f] [-r] [-n] [-m] [-d] -h or --help: Print this help -x or --xenial: Use Ubuntu 16.04 docker image --b or --bionic: Use Ubuntu 18.04 docker image -f or --focal: Use Ubuntu 20.04 docker image -r or --remote: Fetch pre-built remote docker image (vs. local image built by build.sh) -n or --no-display: Don't set up X forwarding. (For headless environment.) @@ -57,8 +56,8 @@ usage() # Parse options 1 (validate and normalize with getopt) ###################################################################### -shortopts="h,x,b,f,r,n,g:,m,d,i:,a:" -longopts="help,xenial,bionic,focal,remote,no-display,gpu:,mount,dry-run,image:,args:" +shortopts="h,x,f,r,n,g:,m,d,i:,a:" +longopts="help,xenial,focal,remote,no-display,gpu:,mount,dry-run,image:,args:" opts=$(getopt -a -n run.sh --options "$shortopts" --longoptions "$longopts" -- "$@") if [ $? -ne 0 ]; then echo @@ -95,8 +94,6 @@ while [ "$1" != "" ]; do ;; -x | --xenial ) os="xenial" ;; - -b | --bionic ) os="bionic" - ;; -f | --focal ) os="focal" ;; -r | --remote ) tagrepo="ghcr.io/nasa" @@ -153,8 +150,6 @@ fi if [ "$os" = "xenial" ]; then tag=astrobee:latest-${image}ubuntu16.04 -elif [ "$os" = "bionic" ]; then - tag=astrobee:latest-${image}ubuntu18.04 elif [ "$os" = "focal" ]; then tag=astrobee:latest-${image}ubuntu20.04 fi diff --git a/scripts/docker/test_astrobee.Dockerfile b/scripts/docker/test_astrobee.Dockerfile index 1b4addac8e..6f1ae31a22 100644 --- a/scripts/docker/test_astrobee.Dockerfile +++ b/scripts/docker/test_astrobee.Dockerfile @@ -1,9 +1,10 @@ -# This will test an Astrobee bionic docker container. +# This will test an Astrobee docker container. # You must set the docker context to be the repository root directory ARG UBUNTU_VERSION=16.04 ARG REMOTE=astrobee -FROM ${REMOTE}/astrobee:latest-ubuntu${UBUNTU_VERSION} +ARG REVISION=latest- +FROM ${REMOTE}/astrobee:${REVISION}ubuntu${UBUNTU_VERSION} # Run tests. See also ../run_tests.sh for explanation. RUN cd /src/astrobee \ diff --git a/scripts/git/cpplint.py b/scripts/git/cpplint.py index 300489ed0c..f5ff8045cd 100755 --- a/scripts/git/cpplint.py +++ b/scripts/git/cpplint.py @@ -6660,8 +6660,8 @@ def FlagCxx11Features(filename, clean_lines, linenum, error): if include and include.group(1) in ( "cfenv", "fenv.h", - "condition_variable", - "future", + # "condition_variable", + # "future", "ratio", "regex", "system_error", diff --git a/scripts/git/pre-commit.linter_python b/scripts/git/pre-commit.linter_python index db90dac612..2c2f3bbf31 100755 --- a/scripts/git/pre-commit.linter_python +++ b/scripts/git/pre-commit.linter_python @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -71,12 +71,18 @@ echo " Analysing python code style with 'isort'." # could happen for example if the .isort.cfg src_paths list gets out # of date. -if $(isort . --extend-skip cmake --profile black --diff --check-only --quiet >/dev/null); then +isort_args=( + "--extend-skip=cmake" + "--extend-skip=submodules" + "--extend-skip=debian" + "--profile=black" +) +if $(isort . "${isort_args[@]}" --diff --check-only --quiet >/dev/null); then echo "Linter checks using 'isort' passed." else echo "Errors detected with 'isort'. Fixing them. Try to add and commit your files again." - isort . --extend-skip cmake --profile black >/dev/null - failed_lint=true + isort . "${isort_args[@]}" >/dev/null + failed_lint=true fi # Cancel commit if linter failed diff --git a/scripts/setup/debians/build_debians.sh b/scripts/setup/debians/build_debians.sh index 15a4b7c122..b93eadb293 100755 --- a/scripts/setup/debians/build_debians.sh +++ b/scripts/setup/debians/build_debians.sh @@ -49,9 +49,6 @@ while [[ $# -gt 0 ]]; do # Packages to find and build dependencies sudo apt-get install -y devscripts equivs libproj-dev || die # Dependencies for jps3d - if [[ $dist == "bionic" ]]; then - sudo apt-get install -y libvtk6.3 libboost-filesystem1.62.0 libboost-system1.62.0 || die - fi if [[ $dist == "focal" ]]; then sudo apt-get install -y libvtk7.1p libboost-filesystem1.71.0 libboost-system1.71.0 || die fi @@ -60,8 +57,8 @@ while [[ $# -gt 0 ]]; do shift done -# Build opencv if ubuntu 18 or 20 -[[ "$dist" =~ ^bionic|focal$ ]] && build_list+=( opencv ) +# Build opencv if ubuntu 20 +[[ "$dist" =~ ^focal$ ]] && build_list+=( opencv ) # Add public debians to build list build_list+=( alvar dlib dbow2 gtsam decomputil jps3d openmvg ) @@ -90,4 +87,4 @@ do if $install_debians ; then sudo dpkg -i *${pkg}*.deb || die "Failed to install $pkg" fi -done \ No newline at end of file +done diff --git a/scripts/setup/debians/build_install_debians.sh b/scripts/setup/debians/build_install_debians.sh index 514f878f7f..06288969a1 100755 --- a/scripts/setup/debians/build_install_debians.sh +++ b/scripts/setup/debians/build_install_debians.sh @@ -33,17 +33,10 @@ case $dist in xenial) echo "Ubuntu 16 detected" ;; - bionic|focal) - build_list+=( opencv ) - ;;& - bionic) - echo "Ubuntu 18 detected" - # jps3d deps - sudo apt-get install -y libvtk6.3 libboost-filesystem1.62.0 libboost-system1.62.0 - ;; focal) echo "Ubuntu 20 detected" - #jps3d deps + build_list+=( opencv ) + # jps3d deps sudo apt-get install -y libvtk7.1p libboost-filesystem1.71.0 libboost-system1.71.0 ;; *) diff --git a/scripts/setup/debians/mirror/create_repo.sh b/scripts/setup/debians/mirror/create_repo.sh index ad94039f58..290c7f2998 100755 --- a/scripts/setup/debians/mirror/create_repo.sh +++ b/scripts/setup/debians/mirror/create_repo.sh @@ -29,4 +29,3 @@ $APTLY mirror create -architectures=armhf xenial-security http://ports.ubuntu.co $APTLY mirror create -architectures=armhf ros http://packages.ros.org/ros/ubuntu xenial main $APTLY repo create astrobee -$APTLY repo create bionic-astrobee diff --git a/scripts/setup/debians/mirror/publish_snapshot.sh b/scripts/setup/debians/mirror/publish_snapshot.sh index 7541c59987..74a7dfc635 100755 --- a/scripts/setup/debians/mirror/publish_snapshot.sh +++ b/scripts/setup/debians/mirror/publish_snapshot.sh @@ -21,14 +21,10 @@ $APTLY snapshot create ros_$1 from mirror ros || exit $APTLY snapshot merge -latest $1 astrobee_$1 main_$1 security_$1 updates_$1 ros_$1 || exit -$APTLY snapshot create bionic-astrobee_$1 from repo bionic-astrobee || exit - # need to delete old version first $APTLY publish drop xenial -$APTLY publish drop bionic $APTLY publish snapshot -gpg-key=33C0A17A -distribution="xenial" $1 || exit 0 -$APTLY publish snapshot -gpg-key=33C0A17A -distribution="bionic" bionic-astrobee_$1 || exit 1 PUBLISH_DIR=`$APTLY config show | grep "rootDir" | sed -r 's|^[^:]+: "([^"]+)",$|\1|'`/public diff --git a/scripts/setup/debians/mirror/update_astrobee_debians.sh b/scripts/setup/debians/mirror/update_astrobee_debians.sh index 408ad3dba6..acfafd6b9e 100755 --- a/scripts/setup/debians/mirror/update_astrobee_debians.sh +++ b/scripts/setup/debians/mirror/update_astrobee_debians.sh @@ -4,11 +4,8 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" APTLY="$DIR/aptly/aptly -config=$DIR/aptly.conf" DEBLOC=${ASTROBEE_DEBIAN_DIR:-/home/p-free-flyer/free-flyer/FSW/ars_debs/dists/xenial/main} -BIONIC_DEBLOC=${ASTROBEE_BIONIC_DEBIAN_DIR:-/home/p-free-flyer/free-flyer/FSW/ars_debs/dists/bionic} # add our debians $APTLY repo add astrobee $DEBLOC/binary-armhf/*.deb $APTLY repo add astrobee $DEBLOC/binary-amd64/*.deb $APTLY repo add astrobee $DEBLOC/source/*.dsc -$APTLY repo add bionic-astrobee $BIONIC_DEBLOC/*.deb -$APTLY repo add bionic-astrobee $BIONIC_DEBLOC/*.dsc diff --git a/scripts/setup/debians/rosjava/.gitignore b/scripts/setup/debians/rosjava/.gitignore new file mode 100644 index 0000000000..27736c0d69 --- /dev/null +++ b/scripts/setup/debians/rosjava/.gitignore @@ -0,0 +1,12 @@ +*.build +*.changes +*.dsc +*.tar.xz +*.tar.gz +*.deb +*.ddeb +*.buildinfo +ros-noetic-rosjava-build-tools +ros-noetic-rosjava-bootstrap +ros-noetic-genjava +ros-noetic-rosjava-messages diff --git a/scripts/setup/debians/rosjava/bootstrap/changelog b/scripts/setup/debians/rosjava/bootstrap/changelog new file mode 100644 index 0000000000..651cb4957c --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/changelog @@ -0,0 +1,5 @@ +ros-noetic-rosjava-bootstrap (0.3.3-1astrobeefocal) focal; urgency=high + + * Add Python3 basic support + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/bootstrap/compat b/scripts/setup/debians/rosjava/bootstrap/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/bootstrap/control b/scripts/setup/debians/rosjava/bootstrap/control new file mode 100644 index 0000000000..4b61941a65 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/control @@ -0,0 +1,12 @@ +Source: ros-noetic-rosjava-bootstrap +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ros-noetic-catkin, ros-noetic-rosjava-build-tools +Homepage: http://ros.org/wiki/rosjava_bootstrap +Standards-Version: 3.9.2 + +Package: ros-noetic-rosjava-bootstrap +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-rosjava-build-tools +Description: Bootstrap utilities for rosjava builds. diff --git a/scripts/setup/debians/rosjava/bootstrap/rules b/scripts/setup/debians/rosjava/bootstrap/rules new file mode 100755 index 0000000000..cebf5a8ec7 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-rosjava-bootstrap//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/bootstrap/source/format b/scripts/setup/debians/rosjava/bootstrap/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/rosjava/bootstrap/source/options b/scripts/setup/debians/rosjava/bootstrap/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/rosjava/bootstrap/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/rosjava/build-tools/changelog b/scripts/setup/debians/rosjava/build-tools/changelog new file mode 100644 index 0000000000..da3394c467 --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/changelog @@ -0,0 +1,5 @@ +ros-noetic-rosjava-build-tools (0.3.3-1astrobeefocal) focal; urgency=high + + * Add Python3 basic support + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 diff --git a/scripts/setup/debians/rosjava/build-tools/compat b/scripts/setup/debians/rosjava/build-tools/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/build-tools/control b/scripts/setup/debians/rosjava/build-tools/control new file mode 100644 index 0000000000..584edbee6f --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/control @@ -0,0 +1,12 @@ +Source: ros-noetic-rosjava-build-tools +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ant, openjdk-8-jdk, ros-noetic-catkin +Homepage: http://ros.org/wiki/rosjava_build_tools +Standards-Version: 3.9.2 + +Package: ros-noetic-rosjava-build-tools +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ant, openjdk-8-jdk, ros-noetic-catkin +Description: Simple tools and catkin modules for rosjava development. diff --git a/scripts/setup/debians/rosjava/build-tools/patches/python3_compat.patch b/scripts/setup/debians/rosjava/build-tools/patches/python3_compat.patch new file mode 100644 index 0000000000..1420cf5ced --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/patches/python3_compat.patch @@ -0,0 +1,121 @@ +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/__init__.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/__init__.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/__init__.py +@@ -4,10 +4,11 @@ + # Imports + ############################################################################## + +-import console +-from create_package import init_android_package, init_rosjava_package +-from create_android_project import create_android_project +-from create_rosjava_project import create_rosjava_project, create_rosjava_msg_project, create_rosjava_library_project +-from utils import which +-from release import scrape_for_release_message_packages +-import catkin ++from rosjava_build_tools import console ++from rosjava_build_tools.create_package import init_android_package, init_rosjava_package ++from rosjava_build_tools.create_android_project import create_android_project ++from rosjava_build_tools.create_rosjava_project import create_rosjava_project, create_rosjava_msg_project, create_rosjava_library_project ++from rosjava_build_tools.utils import which ++from rosjava_build_tools.release import scrape_for_release_message_packages ++from rosjava_build_tools import catkin ++ +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_android_project.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/create_android_project.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_android_project.py +@@ -11,11 +11,10 @@ import sys + import argparse + import subprocess + import shutil +-import exceptions + + # local imports +-import utils +-import console ++from rosjava_build_tools import utils ++from rosjava_build_tools import console + + ############################################################################## + # Methods +@@ -72,9 +71,9 @@ def actually_create_android_project(pack + except subprocess.CalledProcessError: + print("Error") + raise subprocess.CalledProcessError("failed to create android project.") +- except exceptions.OSError as e: ++ except OSError as e: + print("OS error" + str(e)) +- raise exceptions.OSError() ++ raise OSError() + + # This is in the old form, let's shovel the shit around to the new form + utils.mkdir_p(os.path.join(path, 'src', 'main', 'java')) +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_package.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/create_package.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_package.py +@@ -12,8 +12,8 @@ import catkin_pkg + from catkin_pkg.package_templates import create_package_xml, PackageTemplate + + # local imports +-import utils +-import console ++from rosjava_build_tools import utils ++from rosjava_build_tools import console + + ############################################################################## + # Methods +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_rosjava_project.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/create_rosjava_project.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/create_rosjava_project.py +@@ -13,8 +13,8 @@ import argparse + import xml.etree.ElementTree as ElementTree + + # local imports +-import utils +-import console ++from rosjava_build_tools import utils ++from rosjava_build_tools import console + + ############################################################################## + # Methods +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/release.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/release.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/release.py +@@ -6,7 +6,7 @@ + + import rosdistro + import catkin_pkg +-from . import catkin ++from rosjava_build_tools import catkin + + ############################################################################## + # Imports +@@ -16,9 +16,9 @@ from . import catkin + def scrape_for_release_message_packages(track): + url = rosdistro.get_index_url() + index = rosdistro.get_index(url) +- cache = rosdistro.get_release_cache(index, 'kinetic') ++ cache = rosdistro.get_release_cache(index, 'noetic') + packages = [] +- for package_name, package_string in cache.package_xmls.iteritems(): ++ for package_name, package_string in cache.package_xmls.items(): + package = catkin_pkg.package.parse_package_string(package_string) + #print(" Name: %s" % package_name) + #print(" Buildtool Depends %s" % package.build) +Index: ros-noetic-rosjava-build-tools/src/rosjava_build_tools/utils.py +=================================================================== +--- ros-noetic-rosjava-build-tools.orig/src/rosjava_build_tools/utils.py ++++ ros-noetic-rosjava-build-tools/src/rosjava_build_tools/utils.py +@@ -8,7 +8,7 @@ import os + import sys + import errno + import pwd +-import console ++from rosjava_build_tools import console + + ############################################################################## + # Methods diff --git a/scripts/setup/debians/rosjava/build-tools/patches/series b/scripts/setup/debians/rosjava/build-tools/patches/series new file mode 100644 index 0000000000..8e52d4741b --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/patches/series @@ -0,0 +1 @@ +python3_compat.patch diff --git a/scripts/setup/debians/rosjava/build-tools/rules b/scripts/setup/debians/rosjava/build-tools/rules new file mode 100755 index 0000000000..8f579861ce --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-rosjava-build-tools//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/build-tools/source/format b/scripts/setup/debians/rosjava/build-tools/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/rosjava/build-tools/source/options b/scripts/setup/debians/rosjava/build-tools/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/rosjava/build-tools/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/rosjava/build_bootstrap.sh b/scripts/setup/debians/rosjava/build_bootstrap.sh new file mode 100755 index 0000000000..02cfe6f566 --- /dev/null +++ b/scripts/setup/debians/rosjava/build_bootstrap.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=ros-noetic-rosjava-bootstrap +ORIG_TAR=ros-noetic-rosjava-bootstrap_0.3.3.orig.tar.gz +DEB_DIR=bootstrap + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/rosjava_bootstrap.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_build-tools.sh b/scripts/setup/debians/rosjava/build_build-tools.sh new file mode 100755 index 0000000000..87486116cd --- /dev/null +++ b/scripts/setup/debians/rosjava/build_build-tools.sh @@ -0,0 +1,36 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# make sure setuptools from pip3 matches apt version +# you may need to fix it like this: pip3 install setuptools==45.2.0 && pip3 install -U testresources + +PACKAGE_NAME=ros-noetic-rosjava-build-tools +ORIG_TAR=ros-noetic-rosjava-build-tools_0.3.3.orig.tar.gz +DEB_DIR=build-tools + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/rosjava_build_tools.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_genjava.sh b/scripts/setup/debians/rosjava/build_genjava.sh new file mode 100755 index 0000000000..10624dfd7b --- /dev/null +++ b/scripts/setup/debians/rosjava/build_genjava.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=ros-noetic-genjava +ORIG_TAR=ros-noetic-genjava_0.3.4.orig.tar.gz +DEB_DIR=genjava + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/genjava.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_messages.sh b/scripts/setup/debians/rosjava/build_messages.sh new file mode 100755 index 0000000000..853edd4f9d --- /dev/null +++ b/scripts/setup/debians/rosjava/build_messages.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=ros-noetic-rosjava-messages +ORIG_TAR=ros-noetic-rosjava-messages_0.3.0.orig.tar.gz +DEB_DIR=messages + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi + +git clone --quiet https://github.com/rosjava/rosjava_messages.git --branch kinetic $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/rosjava/build_rosjava_debians.sh b/scripts/setup/debians/rosjava/build_rosjava_debians.sh new file mode 100755 index 0000000000..d5a84a57c1 --- /dev/null +++ b/scripts/setup/debians/rosjava/build_rosjava_debians.sh @@ -0,0 +1,78 @@ +#/bin/bash -e +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# Build and install flight software custom dependencies. +set -e + +debian_loc=$(dirname "$(readlink -f "$0")") +build_list=() +install_debians=false +install_deps=false + +die () { + printf "${c_red}${c_bold}[ FATAL ]${c_reset} ${c_bold}%b${c_reset}\\n" "$1" >&2 + exit "${2:-1}" +} + +cleanup() { + rm -rf ros-noetic-rosjava-build-tools ros-noetic-rosjava-bootstrap \ + ros-noetic-genjava ros-noetic-rosjava-messages +} + +# delete old files (-f avoids 'no such file' warning on first run) +rm -f *.deb *.debian.tar.xz *.orig.tar.gz *.dsc *.build *.buildinfo *.changes *.ddeb + +# Process arguments and take action +while [[ $# -gt 0 ]]; do + case "$1" in + -i|--install) + install_debians=true + ;; + -d|--install-with-deps) + install_debians=true + install_deps=true + ;; + esac + shift +done + +# Add public debians to build list +build_list+=( build-tools bootstrap genjava messages ) + +echo "Building debians" +trap 'cleanup' EXIT + +for pkg in ${build_list[@]} +do + # Dependencies + if $install_deps ; then + cd ${debian_loc}/$pkg || die + sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control || + die "Failed to install dependencies for $pkg" + fi + + # Building + cd "$debian_loc" + ./build_${pkg}.sh || die "Failed to build $pkg" + + # Installing + if $install_debians ; then + sudo dpkg -i *${pkg}*.deb || die "Failed to install $pkg" + fi +done diff --git a/scripts/setup/debians/rosjava/genjava/changelog b/scripts/setup/debians/rosjava/genjava/changelog new file mode 100644 index 0000000000..8d1af52dec --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/changelog @@ -0,0 +1,5 @@ +ros-noetic-genjava (0.3.4-1astrobeefocal) focal; urgency=high + + * Add Python3 basic support + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 diff --git a/scripts/setup/debians/rosjava/genjava/compat b/scripts/setup/debians/rosjava/genjava/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/genjava/control b/scripts/setup/debians/rosjava/genjava/control new file mode 100644 index 0000000000..f86ceb1948 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/control @@ -0,0 +1,12 @@ +Source: ros-noetic-genjava +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), python3-catkin-pkg, python3-rospkg, ros-noetic-catkin, ros-noetic-genmsg, ros-noetic-rosjava-bootstrap, ros-noetic-rosjava-build-tools +Homepage: http://www.ros.org/wiki/genjava +Standards-Version: 3.9.2 + +Package: ros-noetic-genjava +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, python3-catkin-pkg, python3-rospkg, ros-noetic-genmsg, ros-noetic-rosjava-bootstrap, ros-noetic-rosjava-build-tools +Description: Java ROS message and service generators. diff --git a/scripts/setup/debians/rosjava/genjava/patches/python3_compat.patch b/scripts/setup/debians/rosjava/genjava/patches/python3_compat.patch new file mode 100644 index 0000000000..e374e1e638 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/patches/python3_compat.patch @@ -0,0 +1,13 @@ +Index: ros-noetic-genjava/src/genjava/gradle_project.py +=================================================================== +--- ros-noetic-genjava.orig/src/genjava/gradle_project.py ++++ ros-noetic-genjava/src/genjava/gradle_project.py +@@ -79,7 +79,7 @@ def get_templates(): + + def populate_project(project_name, project_version, pkg_directory, gradle_project_dir, msg_dependencies): + author = author_name() +- for filename, template in get_templates().iteritems(): ++ for filename, template in get_templates().items(): + contents = instantiate_genjava_template(template, project_name, project_version, pkg_directory, author, msg_dependencies) + try: + p = os.path.abspath(os.path.join(gradle_project_dir, filename)) diff --git a/scripts/setup/debians/rosjava/genjava/patches/series b/scripts/setup/debians/rosjava/genjava/patches/series new file mode 100644 index 0000000000..8e52d4741b --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/patches/series @@ -0,0 +1 @@ +python3_compat.patch diff --git a/scripts/setup/debians/rosjava/genjava/rules b/scripts/setup/debians/rosjava/genjava/rules new file mode 100755 index 0000000000..6a5918ca68 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/rules @@ -0,0 +1,61 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ -v --buildsystem=cmake + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-genjava//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/genjava/source/format b/scripts/setup/debians/rosjava/genjava/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/rosjava/genjava/source/options b/scripts/setup/debians/rosjava/genjava/source/options new file mode 100644 index 0000000000..8bc9182a24 --- /dev/null +++ b/scripts/setup/debians/rosjava/genjava/source/options @@ -0,0 +1,5 @@ +# Automatically add upstream changes to the quilt overlay. +# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html +# This supports reusing the orig.tar.gz for debian increments. +auto-commit + diff --git a/scripts/setup/debians/rosjava/messages/changelog b/scripts/setup/debians/rosjava/messages/changelog new file mode 100644 index 0000000000..30b0aaebcc --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/changelog @@ -0,0 +1,5 @@ +ros-noetic-rosjava-messages (0.3.0-0astrobeefocal) focal; urgency=high + + * Generate only Astrobee dependencies + + -- Ruben Garcia Thu, 15 Feb 2024 02:00:00 -0000 diff --git a/scripts/setup/debians/rosjava/messages/compat b/scripts/setup/debians/rosjava/messages/compat new file mode 100644 index 0000000000..f11c82a4cb --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/compat @@ -0,0 +1 @@ +9 \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/messages/control b/scripts/setup/debians/rosjava/messages/control new file mode 100644 index 0000000000..2871a23c61 --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/control @@ -0,0 +1,12 @@ +Source: ros-noetic-rosjava-messages +Section: misc +Priority: extra +Maintainer: Ruben Garcia +Build-Depends: debhelper (>= 9.0.0), ros-noetic-actionlib-msgs, ros-noetic-catkin, ros-noetic-genjava, ros-noetic-geometry-msgs, ros-noetic-roscpp, ros-noetic-rosjava-build-tools, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-trajectory-msgs +Homepage: http://ros.org/wiki/rosjava_messages +Standards-Version: 3.9.2 + +Package: ros-noetic-rosjava-messages +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends}, ros-noetic-genjava, ros-noetic-rosjava-build-tools +Description: Message generation for rosjava. diff --git a/scripts/setup/debians/rosjava/messages/patches/reduce_package_selection.patch b/scripts/setup/debians/rosjava/messages/patches/reduce_package_selection.patch new file mode 100644 index 0000000000..7bb9b4d339 --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/patches/reduce_package_selection.patch @@ -0,0 +1,67 @@ +Index: ros-noetic-rosjava-messages/CMakeLists.txt +=================================================================== +--- ros-noetic-rosjava-messages.orig/CMakeLists.txt ++++ ros-noetic-rosjava-messages/CMakeLists.txt +@@ -14,36 +14,36 @@ find_package(catkin REQUIRED genjava) + generate_rosjava_messages( + PACKAGES + std_msgs +- rosgraph_msgs # ros/ros_comm_msgs +- std_srvs +- rosjava_test_msgs # rosjava/rosjava_test_msgs ++ #rosgraph_msgs # ros/ros_comm_msgs ++ #std_srvs ++ #rosjava_test_msgs # rosjava/rosjava_test_msgs + actionlib_msgs # ros/common_msgs +- common_msgs +- diagnostic_msgs ++ #common_msgs ++ #diagnostic_msgs + geometry_msgs +- nav_msgs ++ #nav_msgs + sensor_msgs +- shape_msgs +- stereo_msgs ++ #shape_msgs ++ #stereo_msgs + trajectory_msgs +- visualization_msgs +- tf2_msgs # geometry_experimental/tf2_msgs +- ar_track_alvar_msgs # sniekum/ar_track_alvar_msgs +- uuid_msgs # ros-geographic-info/unique_identifier +- yocs_msgs # yujinrobot/yocs_msgs +- concert_msgs # robotics-in-concert/rocon_msgs +- concert_service_msgs +- gateway_msgs +- rocon_app_manager_msgs +- rocon_device_msgs +- rocon_interaction_msgs +- rocon_service_pair_msgs +- rocon_std_msgs +- rocon_tutorial_msgs +- world_canvas_msgs +- scheduler_msgs +- move_base_msgs # ros-planning/navigation +- map_store # ros-planning/map_store ++ #visualization_msgs ++ #tf2_msgs # geometry_experimental/tf2_msgs ++ #ar_track_alvar_msgs # sniekum/ar_track_alvar_msgs ++ #uuid_msgs # ros-geographic-info/unique_identifier ++ #yocs_msgs # yujinrobot/yocs_msgs ++ #concert_msgs # robotics-in-concert/rocon_msgs ++ #concert_service_msgs ++ #gateway_msgs ++ #rocon_app_manager_msgs ++ #rocon_device_msgs ++ #rocon_interaction_msgs ++ #rocon_service_pair_msgs ++ #rocon_std_msgs ++ #rocon_tutorial_msgs ++ #world_canvas_msgs ++ #scheduler_msgs ++ #move_base_msgs # ros-planning/navigation ++ #map_store # ros-planning/map_store + + roscpp # !!!!!!! REMOVE THIS ONCE https://github.com/ros-planning/navigation/pull/312 is fixed! !!!!!!!! + ) diff --git a/scripts/setup/debians/rosjava/messages/patches/series b/scripts/setup/debians/rosjava/messages/patches/series new file mode 100644 index 0000000000..388ec32e1e --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/patches/series @@ -0,0 +1 @@ +reduce_package_selection.patch diff --git a/scripts/setup/debians/rosjava/messages/rules b/scripts/setup/debians/rosjava/messages/rules new file mode 100755 index 0000000000..cbe2127f2a --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/rules @@ -0,0 +1,62 @@ +#!/usr/bin/make -f +# -*- makefile -*- +# Sample debian/rules that uses debhelper. +# This file was originally written by Joey Hess and Craig Small. +# As a special exception, when this file is copied by dh-make into a +# dh-make output file, you may use that output file without restriction. +# This special exception was added by Craig Small in version 0.37 of dh-make. + +# Uncomment this to turn on verbose mode. +export DH_VERBOSE=1 +export DH_OPTIONS=-v --buildsystem=cmake +# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems +# of this sort: +# https://code.ros.org/trac/ros/ticket/2977 +# https://code.ros.org/trac/ros/ticket/3842 +export LDFLAGS= +export PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +# Explicitly enable -DNDEBUG, see: +# https://github.com/ros-infrastructure/bloom/issues/327 +export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG + +%: + dh $@ + +override_dh_auto_configure: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_configure -- \ + -DCATKIN_BUILD_BINARY_PACKAGE="1" \ + -DCMAKE_INSTALL_PREFIX="/opt/ros/noetic" \ + -DCMAKE_PREFIX_PATH="/opt/ros/noetic" + +override_dh_auto_build: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_build + +override_dh_auto_test: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + echo -- Running tests. Even if one of them fails the build is not canceled. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_test || true + +override_dh_shlibdeps: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_shlibdeps -l$(CURDIR)/debian/ros-noetic-rosjava-messages//opt/ros/noetic/lib/ + +override_dh_auto_install: + # In case we're installing to a non-standard location, look for a setup.sh + # in the install tree that was dropped by catkin, and source it. It will + # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. + if [ -f "/opt/ros/noetic/setup.sh" ]; then . "/opt/ros/noetic/setup.sh"; fi && \ + dh_auto_install diff --git a/scripts/setup/debians/rosjava/messages/source/format b/scripts/setup/debians/rosjava/messages/source/format new file mode 100644 index 0000000000..46ebe02665 --- /dev/null +++ b/scripts/setup/debians/rosjava/messages/source/format @@ -0,0 +1 @@ +3.0 (quilt) \ No newline at end of file diff --git a/scripts/setup/debians/rosjava/readme.md b/scripts/setup/debians/rosjava/readme.md new file mode 100644 index 0000000000..498e28ba63 --- /dev/null +++ b/scripts/setup/debians/rosjava/readme.md @@ -0,0 +1,48 @@ +# Java Generator for ROS Noetic + +These scripts are meant to patch and repackage the Java Generator for ROS +Noetic. + +Generated Debians include: + +- ros-noetic-rosjava-build-tools +- ros-noetic-rosjava-bootstrap +- ros-noetic-genjava +- ros-noetic-rosjava-messages + +These are based on official ros-kinetic versions. Minimal patches have been +applied to make them compatible with Astrobee usage. + +These Debians are not meant for general rosjava use. They are made with +Astrobee-specific needs in mind. + +## Notes + +### Building multiple times + +Depending on the system, some of these Debians may not be able to compile again +if their package is installed. If you need to recompile these Debians for some +reason, please remove them before: + +```shell +sudo apt remove ros-noetic-rosjava-messages ros-noetic-genjava ros-noetic-rosjava-bootstrap ros-noetic-rosjava-build-tools +``` + +### Build-tools and Python setuptools + +The build-tools Debian may have issues compiling if the pip3 setuptools +package does not match the default Ubuntu 20 version. + +**If pip3 is not installed in your system this may not apply to you.** + +```shell +# Versions should match (default: 45.2.0) +pip3 list | grep setuptools +apt show python3-setuptools +``` + +If they don't match, adjust the pip3 version. Example: + +```shell +pip3 install setuptools==45.2.0 && pip3 install -U testresources +``` \ No newline at end of file diff --git a/scripts/setup/packages_base_bionic.lst b/scripts/setup/packages_base_bionic.lst deleted file mode 100644 index 231598112a..0000000000 --- a/scripts/setup/packages_base_bionic.lst +++ /dev/null @@ -1,138 +0,0 @@ -libjsoncpp1 -libprotoc10 -libgoogle-glog0v5 -libtinyxml2.6.2v5 -libyaml-cpp0.5v5 -libace-6.4.5 -libqt4-qt3support -libqt4-xml -libamd2 -libcamd2 -libbtf1 -libcolamd2 -libccolamd2 -libcholmod3 -libsuitesparse-dev -libklu1 -libblas3 -liblapack3 -libatlas3-base -libv4l-0 -libv4lconvert0 -libv4l2rds0 -libgl1-mesa-glx -libgmp10 -libusb-1.0-0 -libtinyxml2.6.2v5 -libpcl-common1.8 -libpcl-features1.8 -libpcl-filters1.8 -libpcl-kdtree1.8 -libpcl-keypoints1.8 -libpcl-octree1.8 -libpcl-registration1.8 -libpcl-sample-consensus1.8 -libpcl-search1.8 -libjson-c3 -libargtable2-0 -libboost-program-options1.62.0 -libboost-signals1.62.0 -libboost-system1.62.0 -libboost-chrono1.62.0 -libboost-thread1.62.0 -libboost-program-options1.62.0 -libboost-signals1.62.0 -libboost-system1.62.0 -libboost-chrono1.62.0 -libboost-thread1.62.0 -libboost-filesystem1.65.1 -ros-melodic-ros-base -ros-melodic-cpp-common -ros-melodic-rosbag -ros-melodic-common-msgs -ros-melodic-nodelet -ros-melodic-pluginlib -ros-melodic-actionlib -ros-melodic-image-transport -ros-melodic-compressed-image-transport -ros-melodic-tf2 -ros-melodic-tf2-eigen -ros-melodic-tf2-geometry-msgs -ros-melodic-tf2-kdl -ros-melodic-tf2-msgs -ros-melodic-tf2-ros -ros-melodic-tf2-sensor-msgs -ros-melodic-message-filters -ros-melodic-robot-state-publisher -ros-melodic-rosconsole -ros-melodic-roscpp -ros-melodic-rosgraph -ros-melodic-rosgraph-msgs -ros-melodic-roslaunch -ros-melodic-rosmaster -ros-melodic-rosmsg -ros-melodic-rosnode -ros-melodic-rosout -ros-melodic-rosparam -ros-melodic-rosservice -ros-melodic-rostopic -ros-melodic-roswtf -ros-melodic-std-srvs -ros-melodic-topic-tools -ros-melodic-xmlrpcpp -ros-melodic-mavlink -ros-melodic-xacro -ros-melodic-octomap -ros-melodic-catkin -libalvar2 -libdbow21 -libgtsam -libopenmvg1 -libroyale1 -libceres1 -rti -libmiro0 -libsoracore1 -libdecomputil0 -libjps3d0 -build-essential -cmake -libgoogle-glog-dev -libgtest-dev -libgflags-dev -libace-dev -libqt4-dev -protobuf-compiler -libv4l-dev -libeigen3-dev -libluajit-5.1-dev -libjsoncpp-dev -libtinyxml-dev -libyaml-cpp-dev -libusb-1.0-0-dev -libpcl-dev -libpcl-conversions-dev -libboost-program-options-dev -libboost-signals-dev -libboost-system-dev -libboost-chrono-dev -libboost-thread-dev -libboost-filesystem-dev -libsuitesparse-dev -libatlas-base-dev -libjson-c-dev -libargtable2-dev -libalvar-dev -libdbow2-dev -libgtsam-dev -libopenmvg-dev -libroyale-dev -libceres-dev -rti-dev -libsoracore-dev -libmiro-dev -libdecomputil-dev -libjps3d-dev -libopencv3.3.1 -libopencv-dev3.3.1 -python-catkin-tools diff --git a/scripts/setup/packages_desktop_bionic.lst b/scripts/setup/packages_desktop_bionic.lst deleted file mode 100644 index b83dec89ae..0000000000 --- a/scripts/setup/packages_desktop_bionic.lst +++ /dev/null @@ -1,10 +0,0 @@ -ros-melodic-rviz -gazebo9 -gazebo9-common -ros-melodic-gazebo-ros-pkgs -libgazebo9-dev -python-pyqtgraph -doxygen -zlib1g-dev -libusb-1.0-0-dev -clang-format-8 \ No newline at end of file diff --git a/shared/ff_common/include/ff_common/ff_names.h b/shared/ff_common/include/ff_common/ff_names.h index acdcc0d52b..0a8ec2ebfb 100644 --- a/shared/ff_common/include/ff_common/ff_names.h +++ b/shared/ff_common/include/ff_common/ff_names.h @@ -384,6 +384,7 @@ #define TOPIC_HARDWARE_DOCK_CAM "hw/cam_dock" #define TOPIC_HARDWARE_CAM_SUFFIX_BAYER_RAW "_bayer" #define TOPIC_HARDWARE_CAM_SUFFIX_BAYER_COLOR "_color" +#define TOPIC_HARDWARE_CAM_INFO "/camera_info" #define TOPIC_HARDWARE_SCI_CAM "hw/cam_sci" #define TOPIC_HARDWARE_LIGHT_FRONT "hw/light_front" #define TOPIC_HARDWARE_LIGHT_AFT "hw/light_aft" @@ -449,5 +450,6 @@ #define SERVICE_HARDWARE_PMC_TIMEOUT "hw/pmc/set_timeout" #define SERVICE_STREAMING_LIGHTS "hw/signal_lights/streaming" +#define SERVICE_SET_EXPOSURE "/set_exposure" #endif // FF_COMMON_FF_NAMES_H_ diff --git a/submodules/android b/submodules/android index d6f045ab8a..ad16e76730 160000 --- a/submodules/android +++ b/submodules/android @@ -1 +1 @@ -Subproject commit d6f045ab8a276576cae8c046dce21fb26e67cdfb +Subproject commit ad16e76730c664a0cc890bd40010973edec2acdf diff --git a/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py b/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py index 528626b78a..560e542f1a 100755 --- a/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py +++ b/tools/bag_processing/scripts/apply_histogram_equalization_to_images.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/check_bag_for_gaps.py b/tools/bag_processing/scripts/check_bag_for_gaps.py index 2d28b384b8..67f81e2952 100755 --- a/tools/bag_processing/scripts/check_bag_for_gaps.py +++ b/tools/bag_processing/scripts/check_bag_for_gaps.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/clock_skew.py b/tools/bag_processing/scripts/clock_skew.py index d9532e2f06..5122ef80b9 100755 --- a/tools/bag_processing/scripts/clock_skew.py +++ b/tools/bag_processing/scripts/clock_skew.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/csv_join.py b/tools/bag_processing/scripts/csv_join.py index 82afc6994a..98cc2f3f71 100755 --- a/tools/bag_processing/scripts/csv_join.py +++ b/tools/bag_processing/scripts/csv_join.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/get_msg_stats.py b/tools/bag_processing/scripts/get_msg_stats.py index 3dc0f11b61..eced70b9f3 100755 --- a/tools/bag_processing/scripts/get_msg_stats.py +++ b/tools/bag_processing/scripts/get_msg_stats.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_debayer.py b/tools/bag_processing/scripts/rosbag_debayer.py index 246cbf7afb..db972541fb 100755 --- a/tools/bag_processing/scripts/rosbag_debayer.py +++ b/tools/bag_processing/scripts/rosbag_debayer.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_detect_bad_topics.py b/tools/bag_processing/scripts/rosbag_detect_bad_topics.py index ba970327da..8162dd697d 100755 --- a/tools/bag_processing/scripts/rosbag_detect_bad_topics.py +++ b/tools/bag_processing/scripts/rosbag_detect_bad_topics.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_fix_all.py b/tools/bag_processing/scripts/rosbag_fix_all.py index 4fb7b554f6..734916db91 100755 --- a/tools/bag_processing/scripts/rosbag_fix_all.py +++ b/tools/bag_processing/scripts/rosbag_fix_all.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_merge.py b/tools/bag_processing/scripts/rosbag_merge.py index 055f1f1504..4877b96ff0 100755 --- a/tools/bag_processing/scripts/rosbag_merge.py +++ b/tools/bag_processing/scripts/rosbag_merge.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # @@ -24,9 +24,10 @@ import os import re import string +import subprocess import sys -import rosbag +import localization_common.utilities as lu # https://stackoverflow.com/a/4836734 @@ -59,29 +60,31 @@ def merge_bag(input_bag_prefix, input_bag_suffix, merged_bag, only_loc_topics=Fa merged_bag_name = input_bag_prefix + ".merged.bag" sorted_bag_names = natural_sort(bag_names) + merge_bags_command = ( + "rosrun localization_node merge_bags " + + " ".join(sorted_bag_names) + + " -output_bag " + + str(merged_bag_name) + ) - topics = None if only_loc_topics: - topics = [ - "/hw/imu", - "/loc/of/features", - "/loc/ml/features", - "/loc/ar/features", - "/mgt/img_sampler/nav_cam/image_record", - "/graph_loc/state", - "/gnc/ekf", - "/sparse_mapping/pose", - "/mob/flight_mode", - "/beh/inspection/feedback", - "/beh/inspection/goal", - "/beh/inspection/result", - ] + merge_bags_command += ( + " -save_topics" + + " '/hw/imu" + + " /loc/of/features" + + " /loc/ml/features" + + " /loc/ar/features" + + " /mgt/img_sampler/nav_cam/image_record" + + " /graph_loc/state" + + " /gnc/ekf" + + " /sparse_mapping/pose" + + " /mob/flight_mode" + + " /beh/inspection/feedback" + + " /beh/inspection/goal" + + " /beh/inspection/result'" + ) - with rosbag.Bag(merged_bag_name, "w") as merged_bag: - for sorted_bag_name in sorted_bag_names: - with rosbag.Bag(sorted_bag_name, "r") as sorted_bag: - for topic, msg, t in sorted_bag.read_messages(topics): - merged_bag.write(topic, msg, t) + lu.run_command_and_save_output(merge_bags_command) if __name__ == "__main__": @@ -136,7 +139,6 @@ def merge_bag(input_bag_prefix, input_bag_suffix, merged_bag, only_loc_topics=Fa print(bag_names) for bag_name in bag_names: - print(bag_name) merge_bag( bag_name, args.input_bag_suffix, args.merged_bag, args.only_loc_topics ) diff --git a/tools/bag_processing/scripts/rosbag_rewrite_types.py b/tools/bag_processing/scripts/rosbag_rewrite_types.py index 6f1d21780b..04c204279f 100755 --- a/tools/bag_processing/scripts/rosbag_rewrite_types.py +++ b/tools/bag_processing/scripts/rosbag_rewrite_types.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_sample.py b/tools/bag_processing/scripts/rosbag_sample.py index 4e8fdebfce..ecfddbc824 100755 --- a/tools/bag_processing/scripts/rosbag_sample.py +++ b/tools/bag_processing/scripts/rosbag_sample.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_splice.py b/tools/bag_processing/scripts/rosbag_splice.py index 2ac047381a..d8de7671ce 100755 --- a/tools/bag_processing/scripts/rosbag_splice.py +++ b/tools/bag_processing/scripts/rosbag_splice.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_topic_filter.py b/tools/bag_processing/scripts/rosbag_topic_filter.py index cac94fbc1c..e892be43e9 100755 --- a/tools/bag_processing/scripts/rosbag_topic_filter.py +++ b/tools/bag_processing/scripts/rosbag_topic_filter.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_trim.py b/tools/bag_processing/scripts/rosbag_trim.py index fc11a59c82..6598a4ed54 100755 --- a/tools/bag_processing/scripts/rosbag_trim.py +++ b/tools/bag_processing/scripts/rosbag_trim.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/rosbag_verify.py b/tools/bag_processing/scripts/rosbag_verify.py index 1e5538856c..10fe8ba360 100755 --- a/tools/bag_processing/scripts/rosbag_verify.py +++ b/tools/bag_processing/scripts/rosbag_verify.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py b/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py index 5d0f46dfb4..3650c35d2f 100755 --- a/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py +++ b/tools/bag_processing/scripts/utilities/bmr_renumber_enum.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/tools/bag_processing/test/test_fix_all.py b/tools/bag_processing/test/test_fix_all.py index f075a1262f..acaab7c0cb 100755 --- a/tools/bag_processing/test/test_fix_all.py +++ b/tools/bag_processing/test/test_fix_all.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. #