diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 56615632..acc9a169 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -24,14 +24,14 @@ pipelines: - rosdep update - rosdep install --from-paths ./ -i -y --rosdistro melodic --skip-keys=ignition-common3 - --skip-keys=ignition-gazebo2 - --skip-keys=ignition-launch1 + --skip-keys=ignition-gazebo3 + --skip-keys=ignition-launch2 --skip-keys=ignition-math6 - --skip-keys=ignition-msgs4 + --skip-keys=ignition-msgs5 --skip-keys=ignition-plugin1 - --skip-keys=ignition-rendering2 - --skip-keys=ignition-sensors2 - --skip-keys=ignition-transport7 + --skip-keys=ignition-rendering3 + --skip-keys=ignition-sensors3 + --skip-keys=ignition-transport8 --skip-keys=ros_ign_bridge --skip-keys=ros_ign_image # SubT diff --git a/docker/build.bash b/docker/build.bash index 3d08825a..76611911 100755 --- a/docker/build.bash +++ b/docker/build.bash @@ -36,7 +36,7 @@ fi user_id=$(id -u) image_name=$(basename $1) -image_plus_tag=$image_name:$(date +%Y_%b_%d_%H%M) +image_plus_tag=$image_name:latest-$(date +%Y_%b_%d_%H%M) shift diff --git a/docker/cloudsim_bridge/Dockerfile b/docker/cloudsim_bridge/Dockerfile index a480aa97..cd360e0f 100644 --- a/docker/cloudsim_bridge/Dockerfile +++ b/docker/cloudsim_bridge/Dockerfile @@ -1,17 +1,9 @@ # Ubuntu 18.04 with nvidia-docker2 beta opengl support -FROM nvidia/opengl:1.0-glvnd-devel-ubuntu18.04 - -RUN export DEBIAN_FRONTEND=noninteractive \ - && apt-get update \ - && apt-get install -y \ - tzdata \ - && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \ - && dpkg-reconfigure --frontend noninteractive tzdata \ - && apt-get clean +FROM osrf/subt-virtual-testbed:models_latest # Tools I find useful during development -RUN apt-get update -qq \ - && apt-get install --no-install-recommends -y -qq \ +RUN sudo apt-get update -qq \ + && sudo apt-get install --no-install-recommends -y -qq \ build-essential \ bwm-ng \ atop \ @@ -19,6 +11,7 @@ RUN apt-get update -qq \ cppcheck \ gdb \ git \ + gnutls-bin \ libbluetooth-dev \ libccd-dev \ libcwiid-dev \ @@ -41,16 +34,16 @@ RUN apt-get update -qq \ wget \ net-tools \ iputils-ping \ - && apt-get clean -qq + && sudo apt-get clean -qq # Install AWS CLI. This is needed by cloudsim to capture ROS logs. RUN pip3 install --upgrade awscli=="1.16.220" VOLUME /root/.aws # install ROS and required packages -RUN /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \ - && apt-get update \ - && apt-get install -y \ +RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \ + && sudo apt-get update \ + && sudo apt-get install -y \ python-catkin-tools \ python-rosdep \ python-rosinstall \ @@ -63,34 +56,22 @@ RUN /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb ros-melodic-twist-mux \ ros-melodic-rviz-imu-plugin \ ros-melodic-rotors-control \ - && rosdep init \ - && apt-get clean + ros-melodic-theora-image-transport \ + && sudo rosdep init \ + && sudo apt-get clean # sdformat8-sdf conflicts with sdformat-sdf installed from gazebo # so we need to workaround this using a force overwrite # Do this before installing ign-gazebo -RUN /bin/sh -c 'echo "deb [trusted=yes] http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \ - && /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' \ - && /bin/sh -c 'apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' - -# install ign-blueprint and the ign-bridge -RUN apt-get update \ -&& apt-get install -y \ - ignition-blueprint \ - ros-melodic-ros-ign \ - && apt-get clean - -# Add a user with the same user_id as the user outside the container -# Requires a docker build argument `user_id` -ARG user_id -ENV USERNAME developer -RUN useradd -U --uid ${user_id} -ms /bin/bash $USERNAME \ - && echo "$USERNAME:$USERNAME" | chpasswd \ - && adduser $USERNAME sudo \ - && echo "$USERNAME ALL=NOPASSWD: ALL" >> /etc/sudoers.d/$USERNAME +RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \ + && sudo /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' \ + && sudo /bin/sh -c 'apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' -# Commands below run as the developer user -USER $USERNAME +# install ign-dome and the ign-bridge +RUN sudo apt-get update \ +&& sudo apt-get install -y \ + ignition-dome \ + && sudo apt-get clean # Make a couple folders for organizing docker volumes RUN mkdir ~/workspaces ~/other @@ -104,16 +85,13 @@ RUN rosdep update # docker is run RUN mkdir -p subt_ws/src \ && cd subt_ws/src \ + && git config --global http.postBuffer 1048576000 \ + && git clone https://github.com/ignitionrobotics/ros_ign -b melodic \ && git clone https://github.com/osrf/subt WORKDIR /home/$USERNAME/subt_ws -# Install Rotors -# RUN wget https://s3.amazonaws.com/osrf-distributions/subt_robot_examples/releases/subt_robot_examples_latest.tgz -# RUN tar xvf subt_robot_examples_latest.tgz - RUN /bin/bash -c 'source /opt/ros/melodic/setup.bash && catkin_make install' - RUN /bin/sh -c 'echo ". /opt/ros/melodic/setup.bash" >> ~/.bashrc' \ && /bin/sh -c 'echo ". ~/subt_ws/install/setup.sh" >> ~/.bashrc' diff --git a/docker/cloudsim_sim/Dockerfile b/docker/cloudsim_sim/Dockerfile index 5721c859..d92a85bd 100644 --- a/docker/cloudsim_sim/Dockerfile +++ b/docker/cloudsim_sim/Dockerfile @@ -11,6 +11,7 @@ RUN sudo apt-get update -qq \ cppcheck \ gdb \ git \ + gnutls-bin \ libbluetooth-dev \ libccd-dev \ libcwiid-dev \ @@ -33,8 +34,13 @@ RUN sudo apt-get update -qq \ iputils-ping \ libyaml-cpp-dev \ xvfb \ + g++-8 \ && sudo apt-get clean -qq +RUN sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 + +RUN gcc --version + # Install AWS CLI. This is needed by cloudsim to capture ROS logs. RUN pip3 install --upgrade awscli=="1.16.220" VOLUME /root/.aws @@ -55,6 +61,7 @@ RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu ros-melodic-twist-mux \ ros-melodic-rviz-imu-plugin \ ros-melodic-rotors-control \ + ros-melodic-theora-image-transport \ && sudo rosdep init \ && sudo apt-get clean @@ -65,10 +72,10 @@ RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.osrfoundation.org/g && sudo /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' \ && sudo /bin/sh -c 'apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' -# install ign-blueprint +# install ign-dome RUN sudo apt-get update \ && sudo apt-get install -y \ - ignition-blueprint \ + ignition-dome \ && sudo apt-get clean # install the ros to ign bridge @@ -88,12 +95,12 @@ RUN rosdep update # Install subt sim RUN mkdir -p subt_ws/src \ && cd subt_ws/src \ + && git config --global http.postBuffer 1048576000 \ && git clone https://github.com/osrf/subt WORKDIR /home/$USERNAME/subt_ws RUN /bin/bash -c 'source /opt/ros/melodic/setup.bash && catkin_make install' - RUN /bin/sh -c 'echo ". /opt/ros/melodic/setup.bash" >> ~/.bashrc' \ && /bin/sh -c 'echo ". ~/subt_ws/install/setup.sh" >> ~/.bashrc' diff --git a/docker/subt_models/Dockerfile b/docker/subt_models/Dockerfile index e826a3fc..12e6b92c 100644 --- a/docker/subt_models/Dockerfile +++ b/docker/subt_models/Dockerfile @@ -20,10 +20,10 @@ RUN /bin/sh -c 'echo "deb [trusted=yes] http://packages.osrfoundation.org/gazebo && /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' \ && /bin/sh -c 'apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' -# install ign-citadel +# install ign-dome RUN apt-get update \ && apt-get install -y \ - ignition-citadel \ + ignition-dome \ && apt-get clean # Add a user with the same user_id as the user outside the container @@ -45,5 +45,5 @@ WORKDIR /home/$USERNAME RUN ign fuel download -v 4 -j 8 --type model -u "https://fuel.ignitionrobotics.org/OpenRobotics/collections/SubT Tech Repo" # Cleanup ignition -RUN sudo apt-get remove ignition-citadel -y \ +RUN sudo apt-get remove ignition-dome -y \ && sudo apt-get clean diff --git a/docker/subt_sim_entry/Dockerfile b/docker/subt_sim_entry/Dockerfile index eabe9aad..557a41e2 100644 --- a/docker/subt_sim_entry/Dockerfile +++ b/docker/subt_sim_entry/Dockerfile @@ -48,6 +48,7 @@ RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu ros-melodic-twist-mux \ ros-melodic-rviz-imu-plugin \ ros-melodic-rotors-control \ + ros-melodic-theora-image-transport \ && sudo rosdep init \ && sudo apt-get clean -qq @@ -66,10 +67,10 @@ RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.osrfoundation.org/g && sudo /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' \ && sudo /bin/sh -c 'apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' -# install ign-blueprint -RUN sudo apt-get update \ -&& sudo apt-get install -y \ - ignition-blueprint \ +# install ign-dome +RUN sudo apt-get update -qq \ + && sudo apt-get install -y -qq \ + ignition-dome \ && sudo apt-get clean # install the ros to ign bridge @@ -82,7 +83,7 @@ RUN sudo apt-get update \ # docker is run RUN mkdir -p subt_ws/src \ && cd subt_ws/src \ - && git clone https://github.com/osrf/subt + && git clone https://github.com/osrf/subt -b dome WORKDIR /home/$USERNAME/subt_ws diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/CMakeLists.txt b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/CMakeLists.txt index 8e583af2..3389a0a6 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/CMakeLists.txt +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/CMakeLists.txt @@ -3,7 +3,7 @@ project(ctu_cras_norlab_absolem_sensor_config_1) set(CMAKE_CXX_STANDARD 17) -find_package(ignition-gazebo2 REQUIRED) +find_package(ignition-gazebo4 REQUIRED) find_package(ignition-common3 REQUIRED) find_package(catkin REQUIRED) @@ -11,10 +11,10 @@ find_package(catkin REQUIRED) catkin_package() add_library(laser_rotate_plugin src/laser_rotate_plugin.cpp) -target_link_libraries(laser_rotate_plugin PRIVATE ignition-gazebo2::core ignition-common3::ignition-common3) +target_link_libraries(laser_rotate_plugin PRIVATE ignition-gazebo4::core ignition-common3::ignition-common3) add_library(flipper_control_plugin src/flipper_control_plugin.cpp) -target_link_libraries(flipper_control_plugin PRIVATE ignition-gazebo2::core ignition-common3::ignition-common3) +target_link_libraries(flipper_control_plugin PRIVATE ignition-gazebo4::core ignition-common3::ignition-common3) install(TARGETS laser_rotate_plugin flipper_control_plugin ARCHIVE DESTINATION lib diff --git a/subt-communication/subt_communication_broker/CMakeLists.txt b/subt-communication/subt_communication_broker/CMakeLists.txt index 2fb5b87b..d0b3a76c 100644 --- a/subt-communication/subt_communication_broker/CMakeLists.txt +++ b/subt-communication/subt_communication_broker/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) project(subt_communication_broker) set(PKG_DEPENDS @@ -18,22 +18,22 @@ set(RUN_DEPENDS find_package(catkin REQUIRED ${BUILD_DEPENDS}) find_package(ignition-common3 REQUIRED) -find_package(ignition-msgs4 REQUIRED) -find_package(ignition-transport7 REQUIRED) +find_package(ignition-msgs6 REQUIRED) +find_package(ignition-transport9 REQUIRED) include_directories( include ${CATKIN_DEVEL_PREFIX}/include ${catkin_INCLUDE_DIRS} - ${ignition-transport7_INCLUDE_DIRS} - ${ignition-msgs4_INCLUDE_DIRS} + ${ignition-transport9_INCLUDE_DIRS} + ${ignition-msgs6_INCLUDE_DIRS} ${ignition-common3_INCLUDE_DIRS} ) set(project_libs ${catkin_LIBRARIES} - ${ignition-transport7_LIBRARIES} - ${ignition-msgs4_LIBRARIES} + ${ignition-transport9_LIBRARIES} + ${ignition-msgs6_LIBRARIES} ${ignition-common3_LIBRARIES}) ####################################### diff --git a/subt-communication/subt_communication_broker/cmake/subt_communication_broker-extras.cmake.in b/subt-communication/subt_communication_broker/cmake/subt_communication_broker-extras.cmake.in index 6d807418..6716d53f 100644 --- a/subt-communication/subt_communication_broker/cmake/subt_communication_broker-extras.cmake.in +++ b/subt-communication/subt_communication_broker/cmake/subt_communication_broker-extras.cmake.in @@ -1,6 +1,6 @@ -find_package(ignition-transport7 REQUIRED) +find_package(ignition-transport9 REQUIRED) -list(APPEND catkin_INCLUDE_DIRS ${ignition-transport7_INCLUDE_DIRS}) -list(APPEND catkin_LIBRARIES ${ignition-transport7_LIBRARIES} ${Protobuf_LIBRARIES}) +list(APPEND catkin_INCLUDE_DIRS ${ignition-transport9_INCLUDE_DIRS}) +list(APPEND catkin_LIBRARIES ${ignition-transport9_LIBRARIES} ${Protobuf_LIBRARIES}) include_directories(@CATKIN_DEVEL_PREFIX@/include) diff --git a/subt-communication/subt_communication_broker/package.xml b/subt-communication/subt_communication_broker/package.xml index 1ba37a87..9c1fa190 100644 --- a/subt-communication/subt_communication_broker/package.xml +++ b/subt-communication/subt_communication_broker/package.xml @@ -16,8 +16,8 @@ roscpp subt_communication_model subt_rf_interface - ignition-msgs4 - ignition-transport7 + ignition-msgs6 + ignition-transport9 subt_msgs diff --git a/subt-communication/subt_communication_broker/src/protobuf/CMakeLists.txt b/subt-communication/subt_communication_broker/src/protobuf/CMakeLists.txt index 0523b0c6..445c2596 100644 --- a/subt-communication/subt_communication_broker/src/protobuf/CMakeLists.txt +++ b/subt-communication/subt_communication_broker/src/protobuf/CMakeLists.txt @@ -18,7 +18,7 @@ protobuf_generate_cpp_with_descriptor( PROTO_HEADERS PROTO_DESCRIPTORS IMPORT_DIRS - ${ignition-msgs4_INCLUDE_DIRS} + ${ignition-msgs6_INCLUDE_DIRS} DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/protobuf ${PROTO_MESSAGES} diff --git a/subt_example/CMakeLists.txt b/subt_example/CMakeLists.txt index da50e912..898ac882 100644 --- a/subt_example/CMakeLists.txt +++ b/subt_example/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5.1) +cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) project(subt_example) if(NOT WIN32) @@ -11,6 +11,12 @@ find_package(catkin REQUIRED subt_ign message_generation subt_communication_broker + geometry_msgs + nav_msgs + angles + tf2 + tf2_ros + eigen_conversions ) add_service_files(DIRECTORY srv @@ -40,6 +46,9 @@ add_executable(teleop_node src/teleop_node.cc) add_dependencies(teleop_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(teleop_node ${catkin_LIBRARIES}) +# add_subdirectory(src/truth_controller) + + ########### ## Tests ## ########### diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index 4b9c4b04..c83f8364 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -23,19 +23,19 @@ find_package(catkin REQUIRED rosbag ) -find_package(ignition-gazebo2 2.1 QUIET REQUIRED) -set(IGN_GAZEBO_VER ${ignition-gazebo2_VERSION_MAJOR}) +find_package(ignition-gazebo4 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo4_VERSION_MAJOR}) file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) find_package(ignition-common3 REQUIRED COMPONENTS graphics) -find_package(ignition-fuel_tools3 REQUIRED) +find_package(ignition-fuel_tools5 REQUIRED) find_package(ignition-math6 REQUIRED) -find_package(ignition-msgs4 REQUIRED) -find_package(ignition-transport7 REQUIRED) +find_package(ignition-msgs6 REQUIRED) +find_package(ignition-transport9 REQUIRED) find_package(ignition-plugin1 REQUIRED COMPONENTS loader register) -find_package(ignition-launch1 REQUIRED) -find_package(sdformat8 REQUIRED) +find_package(ignition-launch3 REQUIRED) +find_package(sdformat10 REQUIRED) find_package(yaml-cpp REQUIRED) find_package(PkgConfig REQUIRED) @@ -84,7 +84,7 @@ set_target_properties(${protobuf_lib_name} PROPERTIES POSITION_INDEPENDENT_CODE ON COMPILE_FLAGS "-Wno-unused-parameter" ) target_link_libraries(${protobuf_lib_name} PRIVATE - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ) install(TARGETS ${protobuf_lib_name} ARCHIVE DESTINATION lib @@ -110,11 +110,11 @@ target_link_libraries(${game_logic_plugin_name} PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core ignition-common3::ignition-common3 - ignition-launch1::ignition-launch1 + ignition-launch3::ignition-launch3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-transport7::ignition-transport7 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ${protobuf_lib_name} ${YAML_CPP_LIBRARIES} @@ -130,11 +130,11 @@ target_link_libraries(${artifact_validator_name} PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core ignition-common3::ignition-common3 - ignition-launch1::ignition-launch1 + ignition-launch3::ignition-launch3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-transport7::ignition-transport7 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ${protobuf_lib_name} ) @@ -153,11 +153,11 @@ target_link_libraries(${connection_validator_name} PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core ignition-common3::ignition-common3 - ignition-launch1::ignition-launch1 + ignition-launch3::ignition-launch3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-transport7::ignition-transport7 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ${protobuf_lib_name} ) @@ -176,11 +176,11 @@ target_link_libraries(validate_connections PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core ignition-common3::ignition-common3 - ignition-launch1::ignition-launch1 + ignition-launch3::ignition-launch3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-transport7::ignition-transport7 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ${protobuf_lib_name} ) @@ -189,6 +189,19 @@ install(TARGETS validate_connections LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +add_executable(path_tracer + src/path_tracer.cc) +target_link_libraries(path_tracer + PRIVATE + ignition-gazebo${IGN_GAZEBO_VER}::core + ${catkin_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) +install(TARGETS path_tracer + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + add_library(Visibility STATIC src/ign_to_fcl.cc @@ -233,11 +246,11 @@ target_link_libraries(${comms_broker_plugin_name} PUBLIC ignition-common3::ignition-common3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-launch1::ignition-launch1 - ignition-transport7::ignition-transport7 - sdformat8::sdformat8 + ignition-launch3::ignition-launch3 + ignition-transport9::ignition-transport9 + sdformat10::sdformat10 ${protobuf_lib_name} Visibility ) @@ -255,11 +268,11 @@ target_link_libraries(${visibility_plugin_name} PRIVATE ignition-common3::ignition-common3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-launch1::ignition-launch1 - ignition-transport7::ignition-transport7 - sdformat8::sdformat8 + ignition-launch3::ignition-launch3 + ignition-transport9::ignition-transport9 + sdformat10::sdformat10 ${protobuf_lib_name} Visibility ) @@ -276,9 +289,9 @@ target_include_directories(${base_station_plugin_name} target_link_libraries(${base_station_plugin_name} ignition-common3::ignition-common3 ignition-plugin1::loader - ignition-launch1::ignition-launch1 - ignition-transport7::ignition-transport7 - sdformat8::sdformat8 + ignition-launch3::ignition-launch3 + ignition-transport9::ignition-transport9 + sdformat10::sdformat10 ${catkin_LIBRARIES} ${protobuf_lib_name} ) @@ -295,9 +308,9 @@ target_include_directories(${controller_plugin_name} target_link_libraries(${controller_plugin_name} ignition-common3::ignition-common3 ignition-plugin1::loader - ignition-launch1::ignition-launch1 - ignition-transport7::ignition-transport7 - sdformat8::sdformat8 + ignition-launch3::ignition-launch3 + ignition-transport9::ignition-transport9 + sdformat10::sdformat10 ${catkin_LIBRARIES} ${protobuf_lib_name} ) @@ -317,11 +330,11 @@ target_link_libraries(${gas_emitter_detector_plugin_name} PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core ignition-common3::ignition-common3 - ignition-launch1::ignition-launch1 + ignition-launch3::ignition-launch3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 + ignition-msgs6::ignition-msgs6 ignition-plugin1::loader - ignition-transport7::ignition-transport7 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ${protobuf_lib_name} ) @@ -333,8 +346,8 @@ install(TARGETS ${gas_emitter_detector_plugin_name} # Create log_checker executable. add_executable(log_checker src/LogChecker.cc) target_link_libraries(log_checker - ignition-transport7::ignition-transport7 - ignition-transport7::ignition-transport7-log) + ignition-transport9::ignition-transport9 + ignition-transport9::ignition-transport9-log) install(TARGETS log_checker RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) @@ -342,7 +355,7 @@ add_executable(dot_generator src/dot_generator.cc src/ConnectionHelper.cc src/Sd target_link_libraries(dot_generator ignition-math6::ignition-math6 ignition-common3::ignition-common3 - sdformat8::sdformat8) + sdformat10::sdformat10) target_include_directories(dot_generator PRIVATE ${CATKIN_DEVEL_PREFIX}/include) install(TARGETS dot_generator @@ -352,7 +365,7 @@ add_executable(level_generator src/level_generator.cc src/ConnectionHelper.cc sr target_link_libraries(level_generator ignition-math6::ignition-math6 ignition-common3::ignition-common3 - sdformat8::sdformat8) + sdformat10::sdformat10) target_include_directories(level_generator PRIVATE ${CATKIN_DEVEL_PREFIX}/include) install(TARGETS level_generator @@ -362,8 +375,8 @@ add_executable(world_generator_cave src/world_generator_cave.cc src/ConnectionHe target_link_libraries(world_generator_cave ignition-math6::ignition-math6 ignition-common3::requested - ignition-fuel_tools3::ignition-fuel_tools3 - sdformat8::sdformat8) + ignition-fuel_tools5::ignition-fuel_tools5 + sdformat10::sdformat10) target_include_directories(world_generator_cave PRIVATE ${CATKIN_DEVEL_PREFIX}/include) install(TARGETS world_generator_cave diff --git a/subt_ign/cmake/subt_ign-extras.cmake b/subt_ign/cmake/subt_ign-extras.cmake index a1331e0f..b265e142 100644 --- a/subt_ign/cmake/subt_ign-extras.cmake +++ b/subt_ign/cmake/subt_ign-extras.cmake @@ -1,4 +1,4 @@ -find_package(ignition-transport7 REQUIRED) +find_package(ignition-transport9 REQUIRED) -list(APPEND catkin_INCLUDE_DIRS ${ignition-transport7_INCLUDE_DIRS}) -list(APPEND catkin_LIBRARIES ${ignition-transport7_LIBRARIES} ${Protobuf_LIBRARIES}) +list(APPEND catkin_INCLUDE_DIRS ${ignition-transport9_INCLUDE_DIRS}) +list(APPEND catkin_LIBRARIES ${ignition-transport9_LIBRARIES} ${Protobuf_LIBRARIES}) diff --git a/subt_ign/include/subt_ign/VisibilityRfModel.hh b/subt_ign/include/subt_ign/VisibilityRfModel.hh index 6424c0aa..ad69a2c7 100644 --- a/subt_ign/include/subt_ign/VisibilityRfModel.hh +++ b/subt_ign/include/subt_ign/VisibilityRfModel.hh @@ -121,6 +121,8 @@ namespace subt private: void OnPose(const ignition::msgs::Pose_V &_msg); /// \brief Transport node + private: ignition::transport::Node n2; + private: ignition::transport::Node node; private: subt::VisibilityTable visibilityTable; private: visibilityModel::RfConfiguration visibilityConfig; diff --git a/subt_ign/launch/path_tracer.ign b/subt_ign/launch/path_tracer.ign new file mode 100644 index 00000000..d2454945 --- /dev/null +++ b/subt_ign/launch/path_tracer.ign @@ -0,0 +1,197 @@ + + + +<% + # Check if worldName is not defined or is empty/nil + if !defined?(worldName) || worldName == nil || worldName.empty? + $worldName = 'tunnel_circuit_practice_01' + else + $worldName = worldName + end + + worldNumber = $worldName.split('_').last +%> + + + + + IGN_GAZEBO_SYSTEM_PLUGIN_PATH + $LD_LIBRARY_PATH + + + + <% if $worldName.include?('tunnel_circuit_') && + !$worldName.include?('practice') %> + tunnel_circuit/<%= worldNumber %>/<%= $worldName %>.sdf + <% elsif $worldName.include?('urban_circuit_') && + !$worldName.include?('practice') %> + urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf + <% else %> + <%= $worldName %>.sdf + <% end %> + <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%> + <%= updateRate %> + <%end%> + true + false + + true + + /tmp/ign/logs + + <%if defined?(seed) && seed != nil && !seed.empty?%> + <%= seed %> + <%end%> + + + + + + + + + + + + + + + + <%= $worldName %> + SubT Simulator + <%= ENV['SUBT_IMAGES_PATH'] %>/SubT_logo.svg + + + 3D View + false + docked + + + ogre2 + scene + 1.0 1.0 1.0 + 1.0 1.0 1.0 + + + + + + -40 -80 190 0 1.5708 1.5708 + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/<%= $worldName %>/control + /world/<%= $worldName %>/stats + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/<%= $worldName %>/stats + + + + + + + + + + diff --git a/subt_ign/launch/urban_circuit.ign b/subt_ign/launch/urban_circuit.ign index 6accec41..eb4a04ac 100644 --- a/subt_ign/launch/urban_circuit.ign +++ b/subt_ign/launch/urban_circuit.ign @@ -132,7 +132,7 @@ "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416), "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416) } - + allExecutables = "" # key: parent, value: child diff --git a/subt_ign/package.xml b/subt_ign/package.xml index 9de474c9..aa85530a 100644 --- a/subt_ign/package.xml +++ b/subt_ign/package.xml @@ -8,11 +8,11 @@ catkin ignition-common3 - ignition-launch1 + ignition-launch2 ignition-math6 - ignition-msgs4 + ignition-msgs6 ignition-plugin1 - ignition-transport7 + ignition-transport9 subt_rf_interface subt_communication_model diff --git a/subt_ign/src/ArtifactValidator.cc b/subt_ign/src/ArtifactValidator.cc index 1196a3b2..73ecc8d8 100644 --- a/subt_ign/src/ArtifactValidator.cc +++ b/subt_ign/src/ArtifactValidator.cc @@ -317,7 +317,7 @@ void ArtifactValidatorPrivate::ParseArtifacts() for (uint64_t modelIndex = 0; modelIndex < world->ModelCount(); ++modelIndex) { auto model = world->ModelByIndex(modelIndex); - auto pose = model->Pose(); + auto pose = model->RawPose(); auto name = model->Name(); if (name == "artifact_origin") diff --git a/subt_ign/src/ConnectionHelper.cc b/subt_ign/src/ConnectionHelper.cc index e749433f..138c33fe 100644 --- a/subt_ign/src/ConnectionHelper.cc +++ b/subt_ign/src/ConnectionHelper.cc @@ -298,10 +298,10 @@ bool ConnectionHelper::ComputePoint(VertexData *_tile1, VertexData *_tile2, for (const auto& pt1 : ConnectionHelper::connectionPoints[_tile1->tileType]) { - auto pt1tf = _tile1->model.Pose().CoordPositionAdd(pt1); + auto pt1tf = _tile1->model.RawPose().CoordPositionAdd(pt1); for (const auto& pt2 : ConnectionHelper::connectionPoints[_tile2->tileType]) { - auto pt2tf = _tile2->model.Pose().CoordPositionAdd(pt2); + auto pt2tf = _tile2->model.RawPose().CoordPositionAdd(pt2); if (pt1tf.Equal(pt2tf, 1)) { _pt = pt1tf; @@ -315,14 +315,14 @@ bool ConnectionHelper::ComputePoint(VertexData *_tile1, VertexData *_tile2, for (const auto& pt1 : ConnectionHelper::connectionPoints[_tile1->tileType]) { - auto pt1tf = _tile1->model.Pose().CoordPositionAdd(pt1); + auto pt1tf = _tile1->model.RawPose().CoordPositionAdd(pt1); for (const auto& pt2 : ConnectionHelper::connectionPoints[_tile2->tileType]) { - auto pt2tf = _tile2->model.Pose().CoordPositionAdd(pt2); + auto pt2tf = _tile2->model.RawPose().CoordPositionAdd(pt2); igndbg << - _tile1->tileType << " [" << _tile1->model.Pose() << "] -- " << - _tile2->tileType << " [" << _tile2->model.Pose() << "]" + _tile1->tileType << " [" << _tile1->model.RawPose() << "] -- " << + _tile2->tileType << " [" << _tile2->model.RawPose() << "]" << " [" << pt1tf << "] [" << pt2tf << "]" << std::endl; } @@ -345,7 +345,7 @@ std::vector ConnectionHelper::GetConnectionPoints(Vert { for (const auto& pt1 : ConnectionHelper::connectionPoints[_tile1->tileType]) { - auto pt1tf = _tile1->model.Pose().CoordPositionAdd(pt1); + auto pt1tf = _tile1->model.RawPose().CoordPositionAdd(pt1); ret.push_back(pt1tf); } } diff --git a/subt_ign/src/ConnectionHelper.hh b/subt_ign/src/ConnectionHelper.hh index 086eb2f3..28c41033 100644 --- a/subt_ign/src/ConnectionHelper.hh +++ b/subt_ign/src/ConnectionHelper.hh @@ -23,8 +23,7 @@ #include #include -#include -#include +#include namespace subt { diff --git a/subt_ign/src/ConnectionValidatorPrivate.cc b/subt_ign/src/ConnectionValidatorPrivate.cc index baaa4f7f..ded334af 100644 --- a/subt_ign/src/ConnectionValidatorPrivate.cc +++ b/subt_ign/src/ConnectionValidatorPrivate.cc @@ -241,7 +241,7 @@ void ConnectionValidatorPrivate::PopulateConnections() for (uint64_t modelIndex = 0; modelIndex < world->ModelCount(); ++modelIndex) { auto model = world->ModelByIndex(modelIndex); - const auto pose = model->Pose(); + const auto pose = model->RawPose(); const auto name = model->Name(); if (this->vertData.count(name)) diff --git a/subt_ign/src/LogChecker.cc b/subt_ign/src/LogChecker.cc index f49e2c8b..e1b6386f 100644 --- a/subt_ign/src/LogChecker.cc +++ b/subt_ign/src/LogChecker.cc @@ -193,7 +193,7 @@ class LogChecker return 1; // Begin playback as fast as possible. - const auto handle = this->player->Start(std::chrono::seconds(1), false); + const auto handle = this->player->Start(std::chrono::seconds(1)); if (!handle) { std::cerr << "Failed to start playback" << std::endl; diff --git a/subt_ign/src/SdfParser.cc b/subt_ign/src/SdfParser.cc index fc4d0c2d..c673728f 100644 --- a/subt_ign/src/SdfParser.cc +++ b/subt_ign/src/SdfParser.cc @@ -76,7 +76,7 @@ bool SdfParser::FillVertexData(const std::string &_includeStr, VertexData &_vd, return false; sdf::Model modelSdf; modelSdf.SetName(name); - modelSdf.SetPose(pose); + modelSdf.SetRawPose(pose); static int tileId = 0; // Try getting the tile id from the tile name first. @@ -96,5 +96,3 @@ bool SdfParser::FillVertexData(const std::string &_includeStr, VertexData &_vd, return true; } - - diff --git a/subt_ign/src/VisibilityRfModel.cc b/subt_ign/src/VisibilityRfModel.cc index 9d2e0823..445ea276 100644 --- a/subt_ign/src/VisibilityRfModel.cc +++ b/subt_ign/src/VisibilityRfModel.cc @@ -185,24 +185,28 @@ bool VisibilityModel::VisualizeVisibility(const ignition::msgs::StringMsg &_req, // Available colors // - // RedGlow, YellowGlow, GreenGlow, TurquoiseGlow, BlueGlow + // GreenGlow, TurquoiseGlow, BlueGlow, YellowGlow, RedGlow // High (good) Low (bad) - std::map indexToColor; - indexToColor[0] = "Gazebo/RedGlow"; - indexToColor[1] = "Gazebo/YellowGlow"; - indexToColor[2] = "Gazebo/GreenGlow"; - indexToColor[3] = "Gazebo/TurquoiseGlow"; - indexToColor[4] = "Gazebo/BlueGlow"; - - for (int i = 0; i < 5; ++i) { + std::map indexToColor; + indexToColor[0] = ignition::math::Color(0, 1, 0); + indexToColor[1] = ignition::math::Color(0, 1, 1); + indexToColor[2] = ignition::math::Color(0, 0, 1); + indexToColor[3] = ignition::math::Color(1, 1, 0); + indexToColor[4] = ignition::math::Color(1, 0, 0); + indexToColor[5] = ignition::math::Color(1, 1, 1); + + for (int i = 0; i < 6; ++i) + { markerMsg.set_id(i); ignition::msgs::Material *matMsg = markerMsg.mutable_material(); - matMsg->mutable_script()->set_name(indexToColor[i]); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); + ignition::msgs::Set(matMsg->mutable_ambient(), indexToColor[i]); + ignition::msgs::Set(matMsg->mutable_diffuse(), indexToColor[i]); + ignition::msgs::Set(matMsg->mutable_emissive(), indexToColor[i]); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(1.0, 1.0, 1.0)); - perCostMarkers.insert(std::make_pair(i, markerMsg)); + perCostMarkers.insert(std::make_pair(i, markerMsg)); } std::string modelName = _req.data(); diff --git a/subt_ign/src/level_generator.cc b/subt_ign/src/level_generator.cc index 48fd8171..1014bce1 100644 --- a/subt_ign/src/level_generator.cc +++ b/subt_ign/src/level_generator.cc @@ -137,8 +137,8 @@ std::string computeLevels(std::vector &_vertexData, // get min & max model position values for (const auto &vd : _vertexData) { - min.Min(vd.model.Pose().Pos()); - max.Max(vd.model.Pose().Pos()); + min.Min(vd.model.RawPose().Pos()); + max.Max(vd.model.RawPose().Pos()); } // compute level size and position @@ -153,8 +153,8 @@ std::string computeLevels(std::vector &_vertexData, { std::string name = vd.tileName; - double x = vd.model.Pose().Pos().X(); - double y = vd.model.Pose().Pos().Y(); + double x = vd.model.RawPose().Pos().X(); + double y = vd.model.RawPose().Pos().Y(); // Create a new level for each unique x if (levelX.find(x) == levelX.end()) diff --git a/subt_ign/src/path_tracer.cc b/subt_ign/src/path_tracer.cc new file mode 100644 index 00000000..01235509 --- /dev/null +++ b/subt_ign/src/path_tracer.cc @@ -0,0 +1,559 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * 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 "path_tracer.hh" + +////////////////////////////////////////////////// +// Load a color from YAML helper function. +// \param[in] _node YAML node that contains color information +// \param[in] _def Default color value that is used if a color property is +// missing. +ignition::math::Color loadColor(const YAML::Node &_node, double _def = 1.0) +{ + ignition::math::Color clr(_def, _def, _def, _def); + + if (_node["r"]) + clr.R(_node["r"].as()); + + if (_node["g"]) + clr.G(_node["g"].as()); + + if (_node["b"]) + clr.B(_node["b"].as()); + + if (_node["a"]) + clr.A(_node["a"].as()); + + return clr; +} + +////////////////////////////////////////////////// +MarkerColor::MarkerColor(const YAML::Node &_node) +{ + if (_node["ambient"]) + this->ambient = loadColor(_node["ambient"]); + + if (_node["diffuse"]) + this->diffuse = loadColor(_node["diffuse"]); + + if (_node["emissive"]) + this->emissive = loadColor(_node["emissive"]); +} + +////////////////////////////////////////////////// +MarkerColor::MarkerColor(const MarkerColor &_clr) + :ambient(_clr.ambient), diffuse(_clr.diffuse), emissive(_clr.emissive) +{ +} + +////////////////////////////////////////////////// +MarkerColor::MarkerColor(const ignition::math::Color &_ambient, + const ignition::math::Color &_diffuse, + const ignition::math::Color &_emissive) + : ambient(_ambient), diffuse(_diffuse), emissive(_emissive) +{ +} + +////////////////////////////////////////////////// +Processor::Processor(const std::string &_path, const std::string &_configPath) +{ + YAML::Node cfg; + if (!_configPath.empty()) + { + if (ignition::common::exists(_configPath)) + { + try + { + cfg = YAML::LoadFile(_configPath); + } + catch (YAML::Exception &ex) + { + std::cerr << "Unable to load configuration file[" + << _configPath << "]: " + << "error at line " << ex.mark.line + 1 << ", column " + << ex.mark.column + 1 << ": " << ex.msg << "\n"; + } + } + else + { + std::cerr << "Configuration file[" << _configPath << "] doesn't exist\n"; + } + } + + // Set the RTF value + if (cfg && cfg["rtf"]) + this->rtf = cfg["rtf"].as(); + + // Color of incorrect reports. + if (cfg && cfg["incorrect_report_color"]) + { + this->artifactColors["incorrect_report_color"] = + MarkerColor(cfg["incorrect_report_color"]); + } + else + { + this->artifactColors["incorrect_report_color"] = + MarkerColor({1.0, 0.0, 0.0, 0.5}, + {1.0, 0.0, 0.0, 0.5}, + {0.2, 0.0, 0.0, 0.1}); + } + + // Color of correct reports. + if (cfg && cfg["correct_report_color"]) + { + this->artifactColors["correct_report_color"] = + MarkerColor(cfg["correct_report_color"]); + } + else + { + this->artifactColors["correct_report_color"] = + MarkerColor({0.0, 1.0, 0.0, 1.0}, + {0.0, 1.0, 0.0, 1.0}, + {0.0, 1.0, 0.0, 1.0}); + } + + // Color of artifact locations + if (cfg && cfg["artifact_location_color"]) + { + this->artifactColors["artifact_location_color"] = + MarkerColor(cfg["artifact_location_color"]); + } + else + { + this->artifactColors["artifact_location_color"] = + MarkerColor({0.0, 1.0, 1.0, 0.5}, + {0.0, 1.0, 1.0, 0.5}, + {0.0, 0.2, 0.2, 0.5}); + } + + // Color of robot paths + if (cfg && cfg["robot_colors"]) + { + for (std::size_t i = 0; i < cfg["robot_colors"].size(); ++i) + { + this->robotColors.push_back(MarkerColor(cfg["robot_colors"][i])); + } + } + else + { + this->robotColors.push_back( + MarkerColor({0.6, 0.0, 1.0, 1.0}, + {0.6, 0.0, 1.0, 1.0}, + {0.6, 0.0, 1.0, 1.0})); + this->robotColors.push_back( + MarkerColor({0.678, 0.2, 1.0, 1.0}, + {0.678, 0.2, 1.0, 1.0}, + {0.678, 0.2, 1.0, 1.0})); + this->robotColors.push_back( + MarkerColor({0.761, 0.4, 1.0, 1.0}, + {0.761, 0.4, 1.0, 1.0}, + {0.761, 0.4, 1.0, 1.0})); + this->robotColors.push_back( + MarkerColor({0.839, 0.6, 1.0, 1.0}, + {0.839, 0.6, 1.0, 1.0}, + {0.839, 0.6, 1.0, 1.0})); + this->robotColors.push_back( + MarkerColor({1.0, 0.6, 0.0, 1.0}, + {1.0, 0.6, 0.0, 1.0}, + {1.0, 0.6, 0.0, 1.0})); + this->robotColors.push_back( + MarkerColor({1.0, 0.678, 0.2, 1.0}, + {1.0, 0.678, 0.2, 1.0}, + {1.0, 0.678, 0.2, 1.0})); + this->robotColors.push_back( + MarkerColor({1.0, 0.761, 0.4, 1.0}, + {1.0, 0.761, 0.4, 1.0}, + {1.0, 0.761, 0.4, 1.0})); + } + + // Create the transport node with the partition used by simulation + // world. + ignition::transport::NodeOptions opts; + opts.SetPartition("PATH_TRACER"); + this->markerNode = std::make_unique(opts); + this->ClearMarkers(); + + // Subscribe to the artifact poses. + this->SubscribeToArtifactPoseTopics(); + + // Playback the log file. + std::unique_lock lock(this->mutex); + std::thread playbackThread(std::bind(&Processor::Playback, this, _path)); + + this->cv.wait(lock); + + // Create a transport node that uses the default partition. + ignition::transport::Node node; + + // Subscribe to the robot pose topic + bool subscribed = false; + for (int i = 0; i < 5 && !subscribed; ++i) + { + std::vector topics; + node.TopicList(topics); + + // Subscribe to the first /dynamic_pose/info topic + for (auto const &topic : topics) + { + if (topic.find("/dynamic_pose/info") != std::string::npos) + { + // Subscribe to a topic by registering a callback. + if (!node.Subscribe(topic, &Processor::Cb, this)) + { + std::cerr << "Error subscribing to topic [" + << topic << "]" << std::endl; + return; + } + subscribed = true; + break; + } + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Wait for log playback to end. + playbackThread.join(); + + // Process the events log file. + std::string eventsFilepath = _path + "/events.yml"; + if (ignition::common::exists(eventsFilepath)) + { + YAML::Node events; + try + { + events = YAML::LoadFile(eventsFilepath); + } + catch (...) + { + // There was a bug in the events.yml generation that will be fixed + // before Cave Circuit. The replaceAll can be removed after Cave Circuit, + // but leaving this code in place also shouldn't hurt anything. + std::ifstream t(eventsFilepath); + std::string ymlStr((std::istreambuf_iterator(t)), + std::istreambuf_iterator()); + ignition::common::replaceAll(ymlStr, ymlStr, + "_time ", "_time: "); + try + { + events = YAML::Load(ymlStr); + } + catch (...) + { + std::cerr << "Error processing " << eventsFilepath + << ". Please check the the YAML file has correct syntax. " + << "There will be no artifact report visualization.\n"; + } + } + + for (std::size_t i = 0; i < events.size(); ++i) + { + if (events[i]["type"].as() == "artifact_report_attempt") + { + ignition::math::Vector3d reportedPos; + + // Read the reported pose. + std::stringstream stream; + stream << events[i]["reported_pose"].as(); + stream >> reportedPos; + + int sec = events[i]["time_sec"].as(); + std::unique_ptr data = std::make_unique(); + data->type = REPORT; + data->pos = reportedPos; + data->score = events[i]["points_scored"].as(); + + this->logData[sec].push_back(std::move(data)); + } + } + } + else + { + std::cerr << "Missing " << eventsFilepath + << ". There will be no artifact report visualization.\n"; + } + // Display all of the artifacts using visual markers. + this->DisplayArtifacts(); + + // Display all of the poses using visual markers. + this->DisplayPoses(); +} + +////////////////////////////////////////////////// +Processor::~Processor() +{ +} + +////////////////////////////////////////////////// +void Processor::ClearMarkers() +{ + ignition::msgs::Marker markerMsg; + markerMsg.set_ns("default"); + markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + markerNode->Request("/marker", markerMsg); + std::this_thread::sleep_for(std::chrono::seconds(1)); +} + +////////////////////////////////////////////////// +void Processor::Playback(std::string _path) +{ + std::unique_lock lock(this->mutex); + ignition::transport::log::Playback player(_path + "/state.tlog"); + bool valid = false; + ignition::transport::log::PlaybackHandlePtr handle; + + // Playback all topics + const int64_t addTopicResult = player.AddTopic(std::regex(".*")); + if (addTopicResult == 0) + { + std::cerr << "No topics to play back\n"; + } + else if (addTopicResult < 0) + { + std::cerr << "Failed to advertise topics: " << addTopicResult << "\n"; + } + else + { + // Begin playback + handle = player.Start(std::chrono::seconds(5), false); + if (!handle) + { + std::cerr << "Failed to start playback\n"; + return; + } + valid = true; + } + cv.notify_all(); + lock.unlock(); + + // Wait until the player stops on its own + if (valid) + { + std::cerr << "Playing all messages in the log file\n"; + handle->WaitUntilFinished(); + } +} + +///////////////////////////////////////////////// +void Processor::SubscribeToArtifactPoseTopics() +{ + bool subscribed = false; + for (int i = 0; i < 5 && !subscribed; ++i) + { + std::vector topics; + this->markerNode->TopicList(topics); + + for (auto const &topic : topics) + { + if (topic.find("/pose/info") != std::string::npos) + { + this->markerNode->Subscribe(topic, &Processor::ArtifactCb, this); + subscribed = true; + } + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +} + +///////////////////////////////////////////////// +void Processor::ArtifactCb(const ignition::msgs::Pose_V &_msg) +{ + // Process each pose in the message. + for (int i = 0; i < _msg.pose_size(); ++i) + { + // Only consider artifacts. + std::string name = _msg.pose(i).name(); + if (name.find("rescue") == 0 || + name.find("backpack") == 0 || + name.find("vent") == 0 || + name.find("gas") == 0 || + name.find("drill") == 0 || + name.find("extinguisher") == 0 || + name.find("phone") == 0 || + name.find("rope") == 0 || + name.find("helmet") == 0) + { + ignition::math::Pose3d pose = ignition::msgs::Convert(_msg.pose(i)); + this->artifacts[name] = pose; + } + } +} + +////////////////////////////////////////////////// +void Processor::DisplayPoses() +{ + for (std::map>>::iterator iter = + this->logData.begin(); iter != this->logData.end(); ++iter) + { + auto start = std::chrono::steady_clock::now(); + printf("\r %ds/%ds (%06.2f%%)", iter->first, this->logData.rbegin()->first, + static_cast(iter->first) / this->logData.rbegin()->first * 100); + fflush(stdout); + + for (std::unique_ptr &data : iter->second) + { + data->Render(this); + } + + // Get the next time stamp, and sleep the correct amount of time. + auto next = std::next(iter, 1); + if (next != this->logData.end()) + { + int sleepTime = (((next->first - iter->first) / this->rtf)*1000); + auto duration = std::chrono::steady_clock::now() - start; + std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime) - + std::chrono::duration_cast( + duration)); + } + } +} + +///////////////////////////////////////////////// +void Processor::DisplayArtifacts() +{ + for (const auto &artifact : this->artifacts) + { + this->SpawnMarker(this->artifactColors["artifact_location_color"], + artifact.second.Pos(), + ignition::msgs::Marker::SPHERE, + ignition::math::Vector3d(8, 8, 8)); + } +} + +////////////////////////////////////////////////// +void Processor::SpawnMarker(MarkerColor &_color, + const ignition::math::Vector3d &_pos, + ignition::msgs::Marker::Type _type, + const ignition::math::Vector3d &_scale) +{ + // Create the marker message + ignition::msgs::Marker markerMsg; + ignition::msgs::Material matMsg; + markerMsg.set_ns("default"); + markerMsg.set_id(this->markerId++); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(_type); + markerMsg.set_visibility(ignition::msgs::Marker::GUI); + + // Set color + markerMsg.mutable_material()->mutable_ambient()->set_r(_color.ambient.R()); + markerMsg.mutable_material()->mutable_ambient()->set_g(_color.ambient.G()); + markerMsg.mutable_material()->mutable_ambient()->set_b(_color.ambient.B()); + markerMsg.mutable_material()->mutable_ambient()->set_a(_color.ambient.A()); + markerMsg.mutable_material()->mutable_diffuse()->set_r(_color.diffuse.R()); + markerMsg.mutable_material()->mutable_diffuse()->set_g(_color.diffuse.G()); + markerMsg.mutable_material()->mutable_diffuse()->set_b(_color.diffuse.B()); + markerMsg.mutable_material()->mutable_diffuse()->set_a(_color.diffuse.A()); + markerMsg.mutable_material()->mutable_emissive()->set_r(_color.emissive.R()); + markerMsg.mutable_material()->mutable_emissive()->set_g(_color.emissive.G()); + markerMsg.mutable_material()->mutable_emissive()->set_b(_color.emissive.B()); + markerMsg.mutable_material()->mutable_emissive()->set_a(_color.emissive.A()); + + ignition::msgs::Set(markerMsg.mutable_scale(), _scale); + + // The rest of this function adds different shapes and/or modifies shapes. + // Read the terminal statements to figure out what each node.Request + // call accomplishes. + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(_pos.X(), _pos.Y(), _pos.Z(), 0, 0, 0)); + this->markerNode->Request("/marker", markerMsg); +} + +////////////////////////////////////////////////// +void Processor::Cb(const ignition::msgs::Pose_V &_msg) +{ + std::unique_ptr data(new RobotPoseData); + + // Process each pose in the message. + for (int i = 0; i < _msg.pose_size(); ++i) + { + // Only conder robots. + std::string name = _msg.pose(i).name(); + if (name.find("_wheel") != std::string::npos || + name.find("rotor_") != std::string::npos || name == "base_link") { + continue; + } + + ignition::math::Pose3d pose = ignition::msgs::Convert(_msg.pose(i)); + + if (this->robots.find(name) == this->robots.end()) + { + this->robots[name] = this->robotColors[ + this->robots.size() % this->robotColors.size()]; + this->prevPose[name] = pose; + } + + // Filter poses. + if (this->prevPose[name].Pos().Distance(pose.Pos()) > 1.0) + { + data->poses[name].push_back(pose); + this->prevPose[name] = pose; + } + } + + int sec = _msg.header().stamp().sec(); + // Store data. + if (!data->poses.empty()) + this->logData[sec].push_back(std::move(data)); +} + +///////////////////////////////////////////////// +void RobotPoseData::Render(Processor *_p) +{ + // Render the paths using colored spheres. + for (std::map>::const_iterator + iter = this->poses.begin(); iter != this->poses.end(); iter++) + { + for (const ignition::math::Pose3d &p : iter->second) + { + _p->SpawnMarker(_p->robots[iter->first], + p.Pos() + ignition::math::Vector3d(0, 0, 0.5), + ignition::msgs::Marker::SPHERE, + ignition::math::Vector3d(1, 1, 1)); + } + } +} + +///////////////////////////////////////////////// +void ReportData::Render(Processor *_p) +{ + // If scored, then render a green sphere. + // Otherwise render a red box. + if (this->score > 0) + { + _p->SpawnMarker(_p->artifactColors["correct_report_color"], this->pos, + ignition::msgs::Marker::SPHERE, + ignition::math::Vector3d(4, 4, 4)); + } + else + { + _p->SpawnMarker(_p->artifactColors["incorrect_report_color"], this->pos, + ignition::msgs::Marker::BOX, + ignition::math::Vector3d(4, 4, 4)); + } +} + +///////////////////////////////////////////////// +int main(int _argc, char **_argv) +{ + // Create and run the processor. + if (_argc > 2) + Processor p(_argv[1], _argv[2]); + else + Processor p(_argv[1]); + std::cout << "\nPlayback complete.\n"; + return 0; +} diff --git a/subt_ign/src/path_tracer.hh b/subt_ign/src/path_tracer.hh new file mode 100644 index 00000000..1fbf57a9 --- /dev/null +++ b/subt_ign/src/path_tracer.hh @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * 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 +#include + +#include +#include +#include +#include +#include + +// # Usage: +// +// 1. Run the SubT world using the `path_tracer.ign` launch file along with +// an IGN_PARTITION name of PATH_TRACER. For example +// +// $ IGN_PARTITION=PATH_TRACER ign launch -v 4 path_tracer.ign worldName:=cave_qual +// +// 2. Run this program by passing in the directory that contains the simulation log files. Optionally specify a configuration file. For example: +// +// $ ./path_tracer /data/logs/ /home/developer/path_tracer.yml +// +// +// # Sample YAML configuration file: +// +// rtf: 4.0 +// incorrect_report_color: +// ambient: +// r: 1.0 +// g: 0.0 +// b: 0.0 +// a: 0.5 +// diffuse: +// r: 1.0 +// g: 0.0 +// b: 0.0 +// a: 0.5 +// emissive: +// r: 0.2 +// g: 0.0 +// b: 0.0 +// a: 0.1 +// correct_report_color: +// ambient: +// r: 0.0 +// g: 1.0 +// b: 0.0 +// a: 1.0 +// diffuse: +// r: 0.0 +// g: 1.0 +// b: 0.0 +// a: 1.0 +// emissive: +// r: 0.0 +// g: 1.0 +// b: 0.0 +// a: 1.0 +// artficat_location_color: +// ambient: +// r: 0.0 +// g: 1.0 +// b: 1.0 +// a: 0.5 +// diffuse: +// r: 0.0 +// g: 1.0 +// b: 1.0 +// a: 0.5 +// emissive: +// r: 0.0 +// g: 0.2 +// b: 0.2 +// a: 0.5 +// robot_colors: +// - color: +// ambient: +// r: 0.6 +// g: 0.0 +// b: 1.0 +// a: 1.0 +// diffuse: +// r: 0.6 +// g: 0.0 +// b: 1.0 +// a: 1.0 +// emissive: +// r: 0.6 +// g: 0.0 +// b: 1.0 +// a: 1.0 +// - color: +// ambient: +// r: 0.678 +// g: 0.2 +// b: 1.0 +// a: 1.0 +// diffuse: +// r: 0.678 +// g: 0.2 +// b: 1.0 +// a: 1.0 +// emissive: +// r: 0.678 +// g: 0.2 +// b: 1.0 +// a: 1.0 + +// Forward declaration. +class Processor; + +/// \brief Type of data, which is used to determine how to visualize the +/// data. +enum DataType +{ + ROBOT = 0, + REPORT = 1 +}; + +/// \brief Color properties for a marker. +class MarkerColor +{ + /// \brief Default constructor. + public: MarkerColor() = default; + + /// \brief Create from YAML. + /// \param[in] _node YAML node. + public: MarkerColor(const YAML::Node &_node); + + /// \brief Copy constructor + /// \param[in] _clr MarkerColor to copy. + public: MarkerColor(const MarkerColor &_clr); + + /// \brief Constructor + /// \param[in] _ambient Ambient color + /// \param[in] _diffuse Diffuse color + /// \param[in] _emissive Emissive color + public: MarkerColor(const ignition::math::Color &_ambient, + const ignition::math::Color &_diffuse, + const ignition::math::Color &_emissive); + + /// \brief The ambient value + public: ignition::math::Color ambient; + + /// \brief The diffuse value + public: ignition::math::Color diffuse; + + /// \brief The emissive value + public: ignition::math::Color emissive; +}; + +/// \brief Base class for data visualization +class Data +{ + /// \brief The type of data. + public: DataType type; + + /// \brief Pure virtual render function. + /// \param[in] _p Pointer to the processor. + public: virtual void Render(Processor *_p) = 0; +}; + +/// \brief RobotPoseData contains all pose data for a single pose +/// message callback. +class RobotPoseData : public Data +{ + /// \brief All pose information. + public: std::map> poses; + + /// \brief The render function for robot pose data. + public: void Render(Processor *_p); +}; + +/// \brief Artifact report data +class ReportData : public Data +{ + /// \brief Position of the artifact report. + public: ignition::math::Vector3d pos; + + /// \brief Change in score. + public: int score; + + /// \brief The render function for artifact report data. + public: void Render(Processor *_p); +}; + +/// \brief The log file processor. +class Processor +{ + /// \brief Constructor, which also kicks off all of the data + /// visualization. + /// \param[in] _path Path to the directory containing the log files. + /// \param[in] _configPath Path to the configuration file, or empty string. + public: Processor(const std::string &_path, + const std::string &_configPath = ""); + + /// \brief Destructor + public: ~Processor(); + + /// \brief Clear all of the markers. + public: void ClearMarkers(); + + /// Playback a log file. + public: void Playback(std::string _path); + + /// Subscribe to the artifact poses. + public: void SubscribeToArtifactPoseTopics(); + + /// \brief Get the artifact poses. + /// \param[in] _msg Pose message. + public: void ArtifactCb(const ignition::msgs::Pose_V &_msg); + + /// \brief Display the poses. + public: void DisplayPoses(); + + /// \brief Display the artifacts. + public: void DisplayArtifacts(); + + /// \brief Helper function that spawns a visual marker. + /// \param[in] _color Color of the visual marker. + /// \param[in] _pos Position of the visual marker. + /// \param[in] _type Type of the visual marker. + /// \param[in] _scale scale of the visual marker. + public: void SpawnMarker(MarkerColor &_color, + const ignition::math::Vector3d &_pos, + ignition::msgs::Marker::Type _type, + const ignition::math::Vector3d &_scale); + + /// \brief This callback is triggered on every pose message in the log file. + public: void Cb(const ignition::msgs::Pose_V &_msg); + + /// \brief Mapping of robot name to color + public: std::map robots; + + /// \brief The colors used to represent artifacts and reports. + public: std::map artifactColors; + + /// \brief The colors used to represent each robot. + public: std::vector robotColors; + + /// \brief Last pose of a robot. This is used to reduce the number of markers. + private: std::map prevPose; + + /// \brief Artifacts and their pose information. + private:std::map artifacts; + + /// \brief Marker ID, used to create unique markers. + private: int markerId = 0; + + /// \brief Node that will display the visual markers. + private: std::unique_ptr markerNode; + + /// \brief A mutex. + private: std::mutex mutex; + + /// \brief A condition variable. + private: std::condition_variable cv; + + /// \brief All of the pose data. + private: std::map>> logData; + + /// \brief Realtime factor for playback. + private: double rtf = 1.0; +}; diff --git a/subt_ign/src/path_tracer.yml b/subt_ign/src/path_tracer.yml new file mode 100644 index 00000000..110baae3 --- /dev/null +++ b/subt_ign/src/path_tracer.yml @@ -0,0 +1,81 @@ +incorrect_report_color: + ambient: + r: 1.0 + g: 0.0 + b: 0.0 + a: 0.5 + diffuse: + r: 1.0 + g: 0.0 + b: 0.0 + a: 0.5 + emissive: + r: 0.2 + g: 0.0 + b: 0.0 + a: 0.1 +correct_report_color: + ambient: + r: 0.0 + g: 1.0 + b: 0.0 + a: 1.0 + diffuse: + r: 0.0 + g: 1.0 + b: 0.0 + a: 1.0 + emissive: + r: 0.0 + g: 1.0 + b: 0.0 + a: 1.0 +artficat_location_color: + ambient: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + diffuse: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + emissive: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 +robot_colors: + 1: + ambient: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + diffuse: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + emissive: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + 2: + ambient: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + diffuse: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 + emissive: + r: 0.0 + g: 1.0 + b: 1.0 + a: 0.5 diff --git a/subt_ign/src/protobuf/CMakeLists.txt b/subt_ign/src/protobuf/CMakeLists.txt index 0b744dbd..2446e270 100644 --- a/subt_ign/src/protobuf/CMakeLists.txt +++ b/subt_ign/src/protobuf/CMakeLists.txt @@ -17,7 +17,7 @@ protobuf_generate_cpp_with_descriptor( PROTO_HEADERS PROTO_DESCRIPTORS IMPORT_DIRS - ${ignition-msgs4_INCLUDE_DIRS} + ${ignition-msgs6_INCLUDE_DIRS} DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/protobuf ${PROTO_MESSAGES} diff --git a/subt_ign/src/world_generator_cave.cc b/subt_ign/src/world_generator_cave.cc index 3662721b..5b708529 100644 --- a/subt_ign/src/world_generator_cave.cc +++ b/subt_ign/src/world_generator_cave.cc @@ -316,7 +316,7 @@ WorldSection WorldGenerator::CreateWorldSectionFromTile(const std::string &_type VertexData t; t.tileType = _type; - t.model.SetPose(math::Pose3d(_rot * -_entry, _rot)); + t.model.SetRawPose(math::Pose3d(_rot * -_entry, _rot)); s.tiles.push_back(t); for (const auto &o : it->second) @@ -363,27 +363,27 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave Vertical Shaft Straight Bottom Type A"; - t.model.SetPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave Vertical Shaft Cantilevered Type A"; - t.model.SetPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave Vertical Shaft Straight Top Type A"; - t.model.SetPose(math::Pose3d(75, 0, 25, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(75, 0, 25, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave 3 Way Elevation 02 Type A"; - t.model.SetPose(math::Pose3d(100, 0, 0, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(100, 0, 0, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave Elevation 02 Type A"; - t.model.SetPose(math::Pose3d(150, 0, 25, 0, 0, -3.14159)); + t.model.SetRawPose(math::Pose3d(150, 0, 25, 0, 0, -3.14159)); s.tiles.push_back(t); t.tileType = "Cave Cap Type A"; - t.model.SetPose(math::Pose3d(50, 0, 25, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(50, 0, 25, 0, 0, 1.5708)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -405,24 +405,24 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave 3 Way 01 Type A"; - t.model.SetPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave Elevation Straight Type A"; - t.model.SetPose(math::Pose3d(75, 75, -25, 0, 0, -3.14159)); + t.model.SetRawPose(math::Pose3d(75, 75, -25, 0, 0, -3.14159)); s.tiles.push_back(t); t.tileType = "Cave Corner 03 Type A"; - t.model.SetPose(math::Pose3d(75, 150, -25, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(75, 150, -25, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave 3 Way 01 Type A"; - t.model.SetPose(math::Pose3d(75, -100, 0, 0, 0, 0)); + t.model.SetRawPose(math::Pose3d(75, -100, 0, 0, 0, 0)); s.tiles.push_back(t); t.tileType = "Cave U Turn Elevation Type A"; - t.model.SetPose(math::Pose3d(175, -50, 0, 0, 0, 0)); + t.model.SetRawPose(math::Pose3d(175, -50, 0, 0, 0, 0)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -443,31 +443,31 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave Straight Type A"; - t.model.SetPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave 4 Way 01 Type A"; - t.model.SetPose(math::Pose3d(75, 0, 0, 0, 0, 0)); + t.model.SetRawPose(math::Pose3d(75, 0, 0, 0, 0, 0)); s.tiles.push_back(t); t.tileType = "Cave 3 Way 01 Type A"; - t.model.SetPose(math::Pose3d(75, -50, 0, 0, 0, 3.14159)); + t.model.SetRawPose(math::Pose3d(75, -50, 0, 0, 0, 3.14159)); s.tiles.push_back(t); t.tileType = "Cave Straight Shift Type A"; - t.model.SetPose(math::Pose3d(50, -100, 0, 0, 0, 3.14159)); + t.model.SetRawPose(math::Pose3d(50, -100, 0, 0, 0, 3.14159)); s.tiles.push_back(t); t.tileType = "Cave 2 Way 01 Type A"; - t.model.SetPose(math::Pose3d(100, -125, 0, 0, 0, 3.14159)); + t.model.SetRawPose(math::Pose3d(100, -125, 0, 0, 0, 3.14159)); s.tiles.push_back(t); t.tileType = "Cave 4 Way 01 Type A"; - t.model.SetPose(math::Pose3d(125, -100, 0, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(125, -100, 0, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave Corner 02 Type A"; - t.model.SetPose(math::Pose3d(125, -50, 0, 0, 0, 3.14159)); + t.model.SetRawPose(math::Pose3d(125, -50, 0, 0, 0, 3.14159)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -486,11 +486,11 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave Elevation Straight Type A"; - t.model.SetPose(math::Pose3d(50, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(50, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave 3 Way Elevation 03 Type A"; - t.model.SetPose(math::Pose3d(150, 0, 75, 0, 0, 3.14159)); + t.model.SetRawPose(math::Pose3d(150, 0, 75, 0, 0, 3.14159)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -509,15 +509,15 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave Straight Type A"; - t.model.SetPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(25, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave Cavern Type A"; - t.model.SetPose(math::Pose3d(75, 0, -25, 0, 0, 3.14159)); + t.model.SetRawPose(math::Pose3d(75, 0, -25, 0, 0, 3.14159)); s.tiles.push_back(t); t.tileType = "Cave 3 Way 02 Type A"; - t.model.SetPose(math::Pose3d(125, 0, 0, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(125, 0, 0, 0, 0, -1.5708)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -540,19 +540,19 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave 3 Way Elevation 01 Type A"; - t.model.SetPose(math::Pose3d(50, 0, 0, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(50, 0, 0, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave Straight Shift Type A"; - t.model.SetPose(math::Pose3d(125, -75, -25, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(125, -75, -25, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave Straight Type A"; - t.model.SetPose(math::Pose3d(125, 0, -25, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(125, 0, -25, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave U Turn 01 Type A"; - t.model.SetPose(math::Pose3d(175, -25, -25, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(175, -25, -25, 0, 0, -1.5708)); s.tiles.push_back(t); this->worldSectionsTypeA.push_back(s); @@ -566,27 +566,27 @@ void WorldGenerator::CreateTypeAWorldSections() VertexData t; t.tileType = "Cave 3 Way Elevation 01 Type A"; - t.model.SetPose(math::Pose3d(50, 0, 0, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(50, 0, 0, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave Straight Shift Type A"; - t.model.SetPose(math::Pose3d(125, 25, -25, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(125, 25, -25, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave Straight Shift Type A"; - t.model.SetPose(math::Pose3d(125, -75, -25, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(125, -75, -25, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave Corner 01 Type A"; - t.model.SetPose(math::Pose3d(175, 50, -25, 0, 0, -1.5708)); + t.model.SetRawPose(math::Pose3d(175, 50, -25, 0, 0, -1.5708)); s.tiles.push_back(t); t.tileType = "Cave 3 Way 02 Type A"; - t.model.SetPose(math::Pose3d(225, 0, -25, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(225, 0, -25, 0, 0, 1.5708)); s.tiles.push_back(t); t.tileType = "Cave Straight Type A"; - t.model.SetPose(math::Pose3d(175, -50, -25, 0, 0, 1.5708)); + t.model.SetRawPose(math::Pose3d(175, -50, -25, 0, 0, 1.5708)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -770,11 +770,11 @@ void WorldGenerator::CreateTypeBWorldSections() VertexData t; t.tileType = "Cave Cavern Split 02 Type B"; - t.model.SetPose(math::Pose3d(halfTileSize, 0, 0, 0, 0, 0)); + t.model.SetRawPose(math::Pose3d(halfTileSize, 0, 0, 0, 0, 0)); s.tiles.push_back(t); t.tileType = "Cave Cavern Split 01 Type B"; - t.model.SetPose(math::Pose3d(halfTileSize + tileSize, 0, 0, 0, 0, 0)); + t.model.SetRawPose(math::Pose3d(halfTileSize + tileSize, 0, 0, 0, 0, 0)); s.tiles.push_back(t); s.connectionPoints.push_back(std::make_pair( @@ -1075,7 +1075,7 @@ bool WorldGenerator::IntersectionCheck(WorldSection &_section, // section against all tiles that have been added to the world for (const auto &tile : _section.tiles) { - math::Pose3d pose = tile.model.Pose() + _pose; + math::Pose3d pose = tile.model.RawPose() + _pose; math::AxisAlignedBox box = this->tileBoundingBoxes[tile.tileType]; box = transformAxisAlignedBox(box, pose); // save the bounding box in case we do use this section so that we don't @@ -1272,8 +1272,8 @@ void WorldGenerator::Generate() if (tileType == CAVE_TYPE_A) { auto &transitionTile = s.tiles[0].model; - transitionTile.SetPose(math::Pose3d(transitionTile.Pose().Pos(), - math::Quaterniond(0, 0, IGN_PI) * transitionTile.Pose().Rot())); + transitionTile.SetRawPose(math::Pose3d(transitionTile.RawPose().Pos(), + math::Quaterniond(0, 0, IGN_PI) * transitionTile.RawPose().Rot())); flip = true; } } @@ -1337,7 +1337,7 @@ void WorldGenerator::Generate() std::string name = "tile_" + std::to_string(++tileNo); // convert tile pose to world coordinates - math::Pose3d pose = t.model.Pose() + s.pose; + math::Pose3d pose = t.model.RawPose() + s.pose; std::string uri = "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/" + t.tileType; diff --git a/subt_ign/worlds/tunnel_circuit/01/tunnel_circuit_01.dat b/subt_ign/worlds/tunnel_circuit/01/tunnel_circuit_01.dat index ea9f8dbc..776522a9 100644 Binary files a/subt_ign/worlds/tunnel_circuit/01/tunnel_circuit_01.dat and b/subt_ign/worlds/tunnel_circuit/01/tunnel_circuit_01.dat differ diff --git a/subt_ign/worlds/tunnel_circuit/02/tunnel_circuit_02.dat b/subt_ign/worlds/tunnel_circuit/02/tunnel_circuit_02.dat index 200ed886..915f70a8 100644 Binary files a/subt_ign/worlds/tunnel_circuit/02/tunnel_circuit_02.dat and b/subt_ign/worlds/tunnel_circuit/02/tunnel_circuit_02.dat differ diff --git a/subt_ign/worlds/tunnel_circuit/03/tunnel_circuit_03.dat b/subt_ign/worlds/tunnel_circuit/03/tunnel_circuit_03.dat index 4a69b123..a1d60c67 100644 Binary files a/subt_ign/worlds/tunnel_circuit/03/tunnel_circuit_03.dat and b/subt_ign/worlds/tunnel_circuit/03/tunnel_circuit_03.dat differ diff --git a/subt_ign/worlds/tunnel_circuit/04/tunnel_circuit_04.dat b/subt_ign/worlds/tunnel_circuit/04/tunnel_circuit_04.dat index ede1c847..03b0a7e0 100644 Binary files a/subt_ign/worlds/tunnel_circuit/04/tunnel_circuit_04.dat and b/subt_ign/worlds/tunnel_circuit/04/tunnel_circuit_04.dat differ diff --git a/subt_ign/worlds/tunnel_circuit/05/tunnel_circuit_05.dat b/subt_ign/worlds/tunnel_circuit/05/tunnel_circuit_05.dat index e7b8e547..0eb7f295 100644 Binary files a/subt_ign/worlds/tunnel_circuit/05/tunnel_circuit_05.dat and b/subt_ign/worlds/tunnel_circuit/05/tunnel_circuit_05.dat differ diff --git a/subt_ign/worlds/tunnel_circuit_practice_01.dat b/subt_ign/worlds/tunnel_circuit_practice_01.dat index d45c3661..b807afad 100644 Binary files a/subt_ign/worlds/tunnel_circuit_practice_01.dat and b/subt_ign/worlds/tunnel_circuit_practice_01.dat differ diff --git a/subt_ign/worlds/tunnel_circuit_practice_02.dat b/subt_ign/worlds/tunnel_circuit_practice_02.dat index 831b766e..e62bd352 100644 Binary files a/subt_ign/worlds/tunnel_circuit_practice_02.dat and b/subt_ign/worlds/tunnel_circuit_practice_02.dat differ diff --git a/subt_ign/worlds/tunnel_circuit_practice_03.dat b/subt_ign/worlds/tunnel_circuit_practice_03.dat index 37008b3c..464cdd89 100644 Binary files a/subt_ign/worlds/tunnel_circuit_practice_03.dat and b/subt_ign/worlds/tunnel_circuit_practice_03.dat differ diff --git a/subt_ign/worlds/tunnel_qual_ign.dat b/subt_ign/worlds/tunnel_qual_ign.dat index 95d1165f..46c0eab9 100644 Binary files a/subt_ign/worlds/tunnel_qual_ign.dat and b/subt_ign/worlds/tunnel_qual_ign.dat differ diff --git a/subt_ign/worlds/urban_circuit/01/urban_circuit_01.dat b/subt_ign/worlds/urban_circuit/01/urban_circuit_01.dat index 03af7163..16b616d7 100644 Binary files a/subt_ign/worlds/urban_circuit/01/urban_circuit_01.dat and b/subt_ign/worlds/urban_circuit/01/urban_circuit_01.dat differ diff --git a/subt_ign/worlds/urban_circuit/02/urban_circuit_02.dat b/subt_ign/worlds/urban_circuit/02/urban_circuit_02.dat index 7a49a28f..4957c9fe 100644 Binary files a/subt_ign/worlds/urban_circuit/02/urban_circuit_02.dat and b/subt_ign/worlds/urban_circuit/02/urban_circuit_02.dat differ diff --git a/subt_ign/worlds/urban_circuit/03/urban_circuit_03.dat b/subt_ign/worlds/urban_circuit/03/urban_circuit_03.dat index 40b7d22d..9fb88bb0 100644 Binary files a/subt_ign/worlds/urban_circuit/03/urban_circuit_03.dat and b/subt_ign/worlds/urban_circuit/03/urban_circuit_03.dat differ diff --git a/subt_ign/worlds/urban_circuit/04/urban_circuit_04.dat b/subt_ign/worlds/urban_circuit/04/urban_circuit_04.dat index a8db3f3e..09589cb0 100644 Binary files a/subt_ign/worlds/urban_circuit/04/urban_circuit_04.dat and b/subt_ign/worlds/urban_circuit/04/urban_circuit_04.dat differ diff --git a/subt_ign/worlds/urban_circuit/05/urban_circuit_05.dat b/subt_ign/worlds/urban_circuit/05/urban_circuit_05.dat index c9040b55..a8193eef 100644 Binary files a/subt_ign/worlds/urban_circuit/05/urban_circuit_05.dat and b/subt_ign/worlds/urban_circuit/05/urban_circuit_05.dat differ diff --git a/subt_ign/worlds/urban_circuit/06/urban_circuit_06.dat b/subt_ign/worlds/urban_circuit/06/urban_circuit_06.dat index d53efb91..218bccd3 100644 Binary files a/subt_ign/worlds/urban_circuit/06/urban_circuit_06.dat and b/subt_ign/worlds/urban_circuit/06/urban_circuit_06.dat differ diff --git a/subt_ign/worlds/urban_circuit/07/urban_circuit_07.dat b/subt_ign/worlds/urban_circuit/07/urban_circuit_07.dat index ae65b1cf..2e427963 100644 Binary files a/subt_ign/worlds/urban_circuit/07/urban_circuit_07.dat and b/subt_ign/worlds/urban_circuit/07/urban_circuit_07.dat differ diff --git a/subt_ign/worlds/urban_circuit/08/urban_circuit_08.dat b/subt_ign/worlds/urban_circuit/08/urban_circuit_08.dat index 8f7104fe..c311ae49 100644 Binary files a/subt_ign/worlds/urban_circuit/08/urban_circuit_08.dat and b/subt_ign/worlds/urban_circuit/08/urban_circuit_08.dat differ diff --git a/subt_ign/worlds/urban_circuit_practice_01.dat b/subt_ign/worlds/urban_circuit_practice_01.dat index f9020edd..02ddc870 100644 Binary files a/subt_ign/worlds/urban_circuit_practice_01.dat and b/subt_ign/worlds/urban_circuit_practice_01.dat differ diff --git a/subt_ign/worlds/urban_circuit_practice_02.dat b/subt_ign/worlds/urban_circuit_practice_02.dat index 39f5fa3e..79b3f37c 100644 Binary files a/subt_ign/worlds/urban_circuit_practice_02.dat and b/subt_ign/worlds/urban_circuit_practice_02.dat differ diff --git a/subt_ign/worlds/urban_circuit_practice_03.dat b/subt_ign/worlds/urban_circuit_practice_03.dat index 10310e2e..0fce373a 100644 Binary files a/subt_ign/worlds/urban_circuit_practice_03.dat and b/subt_ign/worlds/urban_circuit_practice_03.dat differ diff --git a/subt_ign/worlds/urban_qual.dat b/subt_ign/worlds/urban_qual.dat index 887d39d9..f245dac2 100644 Binary files a/subt_ign/worlds/urban_qual.dat and b/subt_ign/worlds/urban_qual.dat differ diff --git a/subt_ros/CMakeLists.txt b/subt_ros/CMakeLists.txt index c35bc372..aa4d25f5 100644 --- a/subt_ros/CMakeLists.txt +++ b/subt_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) project(subt_ros) ## Compile as C++11, supported in ROS Kinetic and newer @@ -38,8 +38,8 @@ generate_messages( find_package(ignition-common3 REQUIRED) find_package(ignition-math6 REQUIRED) -find_package(ignition-msgs4 REQUIRED) -find_package(ignition-transport7 REQUIRED) +find_package(ignition-msgs6 REQUIRED) +find_package(ignition-transport9 REQUIRED) catkin_package( CATKIN_DEPENDS @@ -78,8 +78,8 @@ target_link_libraries(subt_ros_relay PUBLIC ignition-common3::ignition-common3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 - ignition-transport7::ignition-transport7 + ignition-msgs6::ignition-msgs6 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ) add_dependencies(subt_ros_relay ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -91,8 +91,8 @@ target_link_libraries(set_pose_relay PUBLIC ignition-common3::ignition-common3 ignition-math6::ignition-math6 - ignition-msgs4::ignition-msgs4 - ignition-transport7::ignition-transport7 + ignition-msgs6::ignition-msgs6 + ignition-transport9::ignition-transport9 ${catkin_LIBRARIES} ) add_dependencies(set_pose_relay ${catkin_EXPORTED_TARGETS}) diff --git a/subt_ros/package.xml b/subt_ros/package.xml index 9f282e49..a759b1c2 100644 --- a/subt_ros/package.xml +++ b/subt_ros/package.xml @@ -12,9 +12,8 @@ geometry_msgs ignition-common3 ignition-math6 - ignition-msgs4 - ignition-transport7 - rosbag + ignition-msgs6 + ignition-transport9 theora_image_transport tf2 tf2_ros