diff --git a/.gitignore b/.gitignore index 3c7252939..067c81d30 100644 --- a/.gitignore +++ b/.gitignore @@ -47,8 +47,11 @@ qtcreator-* # Emacs .#* -# Catkin custom files -CATKIN_IGNORE +# ROS2 custom files +COLCON_IGNORE +frames_*.gv +frames_*.pdf + nucleus_token.txt diff --git a/.vscode/settings.json b/.vscode/settings.json index e3adfa24b..9d4ba0ef8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -735,8 +735,78 @@ "files.associations": { "*.launch": "xml", "*.config": "python", - "functional": "cpp" + "functional": "cpp", + "__atomic": "cpp", + "filesystem": "cpp", + "unordered_map": "cpp", + "__node_handle": "cpp", + "iterator": "cpp", + "array": "cpp", + "deque": "cpp", + "list": "cpp", + "string": "cpp", + "vector": "cpp", + "string_view": "cpp", + "initializer_list": "cpp", + "__bit_reference": "cpp", + "__hash_table": "cpp", + "__split_buffer": "cpp", + "__tree": "cpp", + "bitset": "cpp", + "map": "cpp", + "set": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "atomic": "cpp", + "*.tcc": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "bit": "cpp", + "__nullptr": "cpp", + "__locale": "cpp", + "codecvt": "cpp", + "any": "cpp", + "future": "cpp" }, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 100 }", "editor.formatOnSave": true -} +} \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile index 37a543b49..7104791d1 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -46,6 +46,17 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ kmod lsof libfuse-dev iproute2 tcpdump xterm \ python3-dev python3-opencv python3-wxgtk4.0 python3-pip python3-matplotlib python3-lxml python3-pygame \ && rm -rf /var/lib/apt/lists/* +# Package dependencies +# perform ROS dependency installation for our workspace. +COPY ros_ws/src /tmp/ros_ws/src +RUN rosdep init && rosdep update && rosdep install --from-paths /tmp/ros_ws/src --ignore-src -r -y && rm -rf /tmp/ros_ws || echo "Some ROS dependencies installation failed" + +# Install Gazebo Harmonic +RUN apt-get update && apt-get install -y lsb-release gnupg +RUN curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg +RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null +RUN apt-get update +RUN apt-get install -y gz-harmonic # package vdb_mapping # Build and install patched version of OpenVDB (see https://github.com/SteveMacenski/spatio_temporal_voxel_layer/issues/232). Hopefully this gets fixed in the future. @@ -57,6 +68,8 @@ RUN apt-get remove -y libopenvdb*; apt-get update && apt-get install -y libboost cd ..; rm -rf /opt/openvdb/build RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh +# Install ROS2 packages +RUN apt install ros-humble-tf2* ros-humble-perception-pcl libpcl-dev -y RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null @@ -73,11 +86,4 @@ RUN /isaac-sim/python.sh -m pip install git+https://github.com/dronekit/dronekit RUN pip3 install PyYAML mavproxy tmuxp -#RUN echo 'root:passme24' | chpasswd - -#RUN useradd -G video -ms /bin/bash uav -#RUN usermod -a -G dialout uav -#RUN usermod -a -G sudo uav -#RUN echo 'uav:passme24' | chpasswd -#USER uav -#WORKDIR /home/uav \ No newline at end of file +WORKDIR /root/AirStack diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index c103022b0..7a398147c 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -8,7 +8,7 @@ services: dockerfile: docker/Dockerfile tags: - theairlab.org/airstack-dev:latest - - theairlab.org/airstack-dev:0.2.0-pre + - theairlab.org/airstack-dev:0.3.0 # main dev container airstack_dev: diff --git a/docker/extras/bash_history b/docker/extras/bash_history deleted file mode 100644 index abee8909d..000000000 --- a/docker/extras/bash_history +++ /dev/null @@ -1,118 +0,0 @@ -ping 8.8.8.8 -cd /extras/ -ls -./QGroundControl.AppImage -apt install libfuse-dev -./QGroundControl.AppImage -modprobe fuse -apt install modprobe -apt install mod -apt install kmod -modprobe fuse -./QGroundControl.AppImage -ls /root/.bash_history -cat /root/.bash_history -cd /extras/ -ls -ls AscentAeroSystemsSITLPackage -cd AirLab-Autonomy-Stack/ -ls -cd simulation/ -ls -cd AscentAeroSystems/ -ls -./launch_ascent_sitl.bash -ls -./launch_ascent_sitl.bash -pip3 install PyYAML mavproxy --user -./launch_ascent_sitl.bash -find / -name *mavproxy* -find / -name *mavproxy.py* -./launch_ascent_sitl.bash -mavproxy.py -apt install emacs -emacs launch_ascent_sitl.bash -emacs launch_ascent_sitl.bash -nw -find / -name *mavproxy.py* -fg -./launch_ascent_sitl.bash -apt install libgstgl-dev -apt install libgstgl-1.0-dev -apt install libgstgl-1-dev -apt install libgstgl-1-0-dev -apt install libgstgl-1-0 -apt install libgstgl-1.0 -apt install libgstreamer-gl1.0-0 -./launch_ascent_sitl.bash -sudo lsof -i -P -n | grep LISTEN -lsof -i -P -n | grep LISTEN -apt install lsof -lsof -i -P -n | grep LISTEN -kill -9 5519 -lsof -i -P -n | grep LISTEN -./launch_ascent_sitl.bash -lsof -i -P -n | grep LISTEN -./launch_ascent_sitl.bash -ls /isaac-sim/exts/omni.isaac.ros2_bridge/humble/lib/ -./launch_ascent_sitl.bash -cat /root/.bash_history -ls /extras/ -ls /root/.local/share/ov/data/ -find -find / -name "isaac-sim-4.0.0" -find / -name "isaac" -ls -cat runapp.sh -cat isaac-sim.sh -find / -name "python.sh" -/isaac-sim/kit/python.sh -m pip install scipy -/isaac-sim/kit/python.sh -m pip install --editable ./dronekit-python -sudo apt-get install python3-dev python3-opencv python3-wxgtk4.0 python3-pip python3-matplotlib python3-lxml python3-pygame -apt-get install python3-dev python3-opencv python3-wxgtk4.0 python3-pip python3-matplotlib python3-lxml python3-pygame -apt update -apt-get install python3-dev python3-opencv python3-wxgtk4.0 python3-pip python3-matplotlib python3-lxml python3-pygame -ls /extras/ -pip3 install PyYAML mavproxy --user -/isaac-sim/kit/python.sh -m pip install --editable dronekit-python -/isaac-sim/kit/python.sh launch_sim.py -cd /extras/AirLab-Autonomy-Stack/simulation/ -/isaac-sim/kit/python.sh launch_sim.py -ls -ls dronekit-python/ -/isaac-sim/kit/python.sh -m pip install --editable ./dronekit-python -/isaac-sim/kit/python.sh launch_sim.py -cat launch_sim.py -find / -name "python.sh" -/isaac-sim/python.sh launch_sim.py -locale -sudo apt update && sudo apt install locales -apt update && apt install locales -locale-gen en_US en_US.UTF-8 -update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 -export LANG=en_US.UTF-8 -locale -apt install software-properties-common -add-apt-repository universe -apt update && apt install curl -y -curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg - -curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg -echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null - -echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null -apt update -apt install ros-humble-desktop -apt install ros-dev-tools -/isaac-sim/python.sh launch_sim.py -/isaac-sim/runapp.sh -export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/isaac-sim/exts/omni.isaac.ros2_bridge/humble/lib -export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -/isaac-sim/runapp.sh -/isaac-sim/python.sh launch_sim.py -cat /root/.bash_history -shopt -s histappend -PROMPT_COMMAND="history -a;$PROMPT_COMMAND" -history -a -cat /root/.bash_history -cat /root/.bash_history | grep apt -cat /root/.bash_history | grep export diff --git a/ros_ws/src/robot/autonomy/controls/controls_bringup/launch/launch_controls.yaml b/ros_ws/src/robot/autonomy/controls/controls_bringup/launch/launch_controls.yaml index 229ca539c..ae11734c7 100644 --- a/ros_ws/src/robot/autonomy/controls/controls_bringup/launch/launch_controls.yaml +++ b/ros_ws/src/robot/autonomy/controls/controls_bringup/launch/launch_controls.yaml @@ -4,13 +4,14 @@ launch: namespace: "controls" # include MAVROS ardupilot - - include: - file: "$(find-pkg-share mavros)/launch/apm.launch" - arg: - - name: "fcu_url" - value: "udp://:14554@" - - name: "respawn_mavros" - value: "true" + # assume the interface is MAVROS and that MAVROS is already running + # - include: + # file: "$(find-pkg-share mavros)/launch/apm.launch" + # arg: + # - name: "fcu_url" + # value: "udp://:14554@" + # - name: "respawn_mavros" + # value: "true" - node: pkg: "robot_interface" diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/fixed_trajectory_generator.py b/ros_ws/src/robot/autonomy/controls/trajectory_library/src/fixed_trajectory_generator.py index ba12de293..c4be08549 100755 --- a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/fixed_trajectory_generator.py +++ b/ros_ws/src/robot/autonomy/controls/trajectory_library/src/fixed_trajectory_generator.py @@ -9,10 +9,10 @@ #from at_fcs_mavros.msg import Waypoint #from ca_nav_msgs.msg import XYZVPsi #from ca_nav_msgs.msg import PathXYZVPsi -from core_trajectory_msgs.msg import WaypointXYZVYaw -from core_trajectory_msgs.msg import TrajectoryXYZVYaw -from core_trajectory_controller.msg import Trajectory -from core_trajectory_msgs.msg import FixedTrajectory +from airstack_msgs.msg import WaypointXYZVYaw +from airstack_msgs.msg import TrajectoryXYZVYaw +from trajectory_controller.msg import Trajectory +from airstack_msgs.msg import FixedTrajectory import time import copy diff --git a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py b/ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py index af755a0ce..a8fd1a8f5 100644 --- a/ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py +++ b/ros_ws/src/robot/autonomy/controls/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py @@ -19,7 +19,7 @@ from python_qt_binding.QtGui import QColor from python_qt_binding.QtWidgets import QWidget, QVBoxLayout, QSizePolicy -from core_trajectory_msgs.msg import FixedTrajectory +from airstack_msgs.msg import FixedTrajectory from diagnostic_msgs.msg import KeyValue class FixedTrajectorySelectorPlugin(Plugin): @@ -102,7 +102,7 @@ def publish_trajectory(self): def select_config_file(self): - starting_path = os.path.join(rospkg.RosPack().get_path('core_trajectory_library'), 'config') + starting_path = os.path.join(rospkg.RosPack().get_path('trajectory_library'), 'config') filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] self.set_config(filename) diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/base_main_class/CMakeLists.txt new file mode 100644 index 000000000..a71ca1e56 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 2.8.3) +project(base) + +add_compile_options(-Wno-reorder -Wno-unused-parameter) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + nodelet +) + + +catkin_package( + INCLUDE_DIRS include + LIBRARIES base_node base_nodelet + CATKIN_DEPENDS roscpp std_msgs nodelet +) + +include_directories( + ${catkin_INCLUDE_DIRS} + include +) + +#============================================================================================ +# ---------------------------- Base Nodelet Library ----------------------------------------- +#============================================================================================ + +add_library(base_nodelet + src/Base.cpp + src/BaseNodelet.cpp + src/HealthMonitor.cpp +) + +add_dependencies(base_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(base_nodelet + ${catkin_LIBRARIES} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + + +#target_link_libraries(base_nodelet ${catkin_LIBRARIES}) +#if(catkin_EXPORTED_LIBRARIES) +# add_dependencies(base_nodelet ${catkin_EXPORTED_LIBRARIES}) +#endif() + +#install(FILES base_nodelet.xml +#DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +#============================================================================================ +# ---------------------------- Base Node Library -------------------------------------------- +#============================================================================================ + +add_library(base_node + src/Base.cpp + src/BaseNode.cpp + src/HealthMonitor.cpp + src/main.cpp +) + +add_dependencies(base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(base_node + ${catkin_LIBRARIES} +) + +install(TARGETS base_nodelet base_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY include/base/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/COLCON_IGNORE b/ros_ws/src/robot/autonomy/planning/local/base_main_class/COLCON_IGNORE similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/COLCON_IGNORE rename to ros_ws/src/robot/autonomy/planning/local/base_main_class/COLCON_IGNORE diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/LICENSE b/ros_ws/src/robot/autonomy/planning/local/base_main_class/LICENSE new file mode 100644 index 000000000..b039fc086 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/LICENSE @@ -0,0 +1,11 @@ +Copyright 2020 Carnegie Mellon University + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/README.md b/ros_ws/src/robot/autonomy/planning/local/base_main_class/README.md new file mode 100644 index 000000000..5fb8cd906 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/README.md @@ -0,0 +1,18 @@ +This package contains the BaseNode and BaseNodelet classes which monitor the status of the process. All nodes and nodelets you create should inherit from these classes. + +For both nodes and nodelets three functions need to be implemented by derived classes: + +initialize(): This is where you should set up ROS related objects like publishers and subscribers and check for ROS parameters. + +execute(): This is where your main program logic should go. It gets called in a loop at a rate set by the execute_target parameter in your node’s namespace. + +~Destructor(): Do cleanup, like memory deallocation etc., in the destructor. + +For nodes, you also need to implement the BaseNode::get() function to return a pointer to an instance of your class derived from BaseNode. This is used by the main function to run your node. For nodelets, this is not necessary. They are started automatically. + +This package also contains a HealthMonitor class. This lets you time code's running time by calling tic("name") then toc("name"). When tic("name") is called for the first time, a ROS parameter with the name "name_target" is looked up which indicates what the target update rate for the code between tic and toc is. For now, a message is printed if the target rate is not achieved. + +An example implementation of a BaseNode can be found in the example_node package. +An example implementation of a BaseNodelet can be found in the example_nodelet package. + +Author: John Keller jkeller2@andrew.cmu.edu slack: kellerj diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/Base.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/Base.h new file mode 100644 index 000000000..6a984ed34 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/Base.h @@ -0,0 +1,53 @@ +#ifndef _BASE_H_ +#define _BASE_H_ + +#include +#include +#include "HealthMonitor.h" + +class Base{ + private: + bool failed_; + ros::Timer execute_timer; + + public: + HealthMonitor monitor; + + // These functions will be called by main, do health monitoring + // and call the dervied funtions below + bool _initialize(); + bool _execute(); + virtual ~Base(); + + void execute_timer_callback(const ros::TimerEvent& te); + + // These functions are implemented by the user + virtual bool initialize() = 0; + virtual bool execute() = 0; + + // Failing will stop execution and deinitialize + void fail(std::string reason); + + // Returns the name the user gave to the node/nodelet + std::string get_node_name(); + + // Get node handles. + ros::NodeHandle* get_node_handle(); + ros::NodeHandle* get_private_node_handle(); + + protected: + Base(std::string node_name); + + ros::NodeHandle* node_handle_; + ros::NodeHandle* private_node_handle_; + + std::string node_name_; + + // If this is true, the pure virtual deinitialize() function will not be + // called in _deinitialize(), since you can only catch the end of + // a nodelet by implementing its destructor. + bool is_nodelet_; +}; + + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNode.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNode.h new file mode 100644 index 000000000..0880aeecc --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNode.h @@ -0,0 +1,27 @@ +#ifndef _BASE_NODE_H_ +#define _BASE_NODE_H_ + +#include +#include "Base.h" + +class BaseNode : public Base { + public: + // This will be implemented by a specific task and will return an instance of itself, it will be called by the main function + static BaseNode* get(); + + virtual ~BaseNode(); + + // Needed to call ros init + static int argc; + static char** argv; + + protected: + BaseNode(std::string node_name, bool anonymous=false); + + private: + bool anonymous_; +}; + + + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNodelet.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNodelet.h new file mode 100644 index 000000000..4d799c14f --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/BaseNodelet.h @@ -0,0 +1,20 @@ +#ifndef _BASE_NODELET_H_ +#define _BASE_NODELET_H_ + +#include +#include +#include +#include "Base.h" + +class BaseNodelet : public Base, public nodelet::Nodelet { + public: + // This is inherited from nodelet::Nodelet + virtual void onInit(); + + protected: + BaseNodelet(std::string node_name); + ~BaseNodelet(); +}; + + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/HealthMonitor.h b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/HealthMonitor.h new file mode 100644 index 000000000..0ec4a2aba --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/include/base/HealthMonitor.h @@ -0,0 +1,39 @@ +#ifndef _HEALTH_MONITOR_H_ +#define _HEALTH_MONITOR_H_ + +#include +#include +#include +#include + +class Base; //forward declaration + +// Struct for time logging. +struct time_info { + boost::chrono::time_point start; + int64_t elapsed, min_us, max_us; + uint64_t calls; + double target_hz; +}; + +class HealthMonitor { + private: + std::map times; + Base* base_; + + public: + HealthMonitor(Base* base); + + + //========================================================================================= + // ---------------------------- Time Measurement Functions -------------------------------- + //========================================================================================= + + void tic(std::string id); + int64_t toc(std::string id); + double get_target_hz(std::string id); + void print_time_statistics(); +}; + + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/package.xml b/ros_ws/src/robot/autonomy/planning/local/base_main_class/package.xml new file mode 100644 index 000000000..ca89dd118 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/package.xml @@ -0,0 +1,21 @@ + + + base + 0.0.0 + The base package + + john + TODO + + catkin + roscpp + std_msgs + nodelet + + roscpp + std_msgs + nodelet + + + + diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/Base.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/Base.cpp new file mode 100644 index 000000000..102372a40 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/Base.cpp @@ -0,0 +1,86 @@ +#include +#include +#include "base/Base.h" +#include "base/HealthMonitor.h" +#include +#include + +Base::Base(std::string node_name) + : node_name_(node_name) + , monitor(this) + , is_nodelet_(false) + , failed_(false){ + +} + +Base::~Base(){ + monitor.print_time_statistics(); +} + +bool Base::_initialize(){ + // get the rate that execute should be called at + double execute_timer_rate; + bool found_param = get_private_node_handle()->getParam("execute_target", execute_timer_rate); + if(!found_param){ + ROS_FATAL_STREAM("The execute_target parameter was not set. Exiting."); + return false; + } + + if (execute_timer_rate <= 0.0){ + ROS_FATAL_STREAM("The execute_target parameter must be larger than 0.0, Exiting."); + return false; + } + + // call the derived class's initialize + bool status = initialize(); + if(!status){ + ROS_FATAL_STREAM("The initialize() function failed. Exiting."); + return false; + } + + // setup the execute timer + execute_timer = get_node_handle()->createTimer(ros::Duration(1./execute_timer_rate), + &Base::execute_timer_callback, this); + + return true; +} + +void Base::execute_timer_callback(const ros::TimerEvent& te){ + bool status = _execute(); + + if(!status){ + execute_timer.stop(); + + if(!is_nodelet_) + ros::shutdown(); + } +} + +bool Base::_execute(){ + monitor.tic("execute"); + bool status = execute(); + monitor.toc("execute"); + + return status && !failed_; +} + + +void Base::fail(std::string reason){ + ROS_FATAL_STREAM("Node " << get_node_name() << " has failed." << std::endl + << "Reason: " << reason); + + failed_ = true; +} + +std::string Base::get_node_name(){ + return ros::this_node::getName(); +} + + +ros::NodeHandle* Base::get_node_handle(){ + return node_handle_; +} + +ros::NodeHandle* Base::get_private_node_handle(){ + return private_node_handle_; +} diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNode.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNode.cpp new file mode 100644 index 000000000..d6a213efb --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNode.cpp @@ -0,0 +1,26 @@ +#include + +#include "base/BaseNode.h" + + +BaseNode::BaseNode(std::string node_name, bool anonymous) + : Base(node_name) + , anonymous_(anonymous){ + + // call ros init + if(anonymous_) + ros::init(argc, argv, node_name_, ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); + else + ros::init(argc, argv, node_name_, ros::init_options::NoSigintHandler); + + // set up the node handles + node_handle_ = new ros::NodeHandle(); + private_node_handle_ = new ros::NodeHandle("~"); +} + +BaseNode::~BaseNode(){ + +} + +int BaseNode::argc = 0; +char** BaseNode::argv = NULL; diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNodelet.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNodelet.cpp new file mode 100644 index 000000000..f70f5263a --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/BaseNodelet.cpp @@ -0,0 +1,22 @@ +#include +#include +#include +#include + +#include "base/BaseNodelet.h" + + +BaseNodelet::BaseNodelet(std::string node_name) + : Base(node_name){ + is_nodelet_ = true; +} + +BaseNodelet::~BaseNodelet(){ + +} + +void BaseNodelet::onInit(){ + node_handle_ = &getNodeHandle(); + private_node_handle_ = &getPrivateNodeHandle(); + _initialize(); +} diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/HealthMonitor.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/HealthMonitor.cpp new file mode 100644 index 000000000..8c7aedf26 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/HealthMonitor.cpp @@ -0,0 +1,82 @@ +#include "base/HealthMonitor.h" + +#include + +#include + +#include "base/Base.h" + +HealthMonitor::HealthMonitor(Base* base) : base_(base) {} + +//========================================================================================= +// ---------------------------- Time Measurement Functions -------------------------------- +//========================================================================================= + +void HealthMonitor::tic(std::string id) { + // initialize time struct if tic is called for the first time on id + if (times.count(id) == 0) { + times[id].elapsed = 0; + times[id].calls = 0; + times[id].min_us = std::numeric_limits::max(); + times[id].max_us = std::numeric_limits::min(); + + base_->get_private_node_handle()->param(id + "_target", times[id].target_hz, -1); + } + + // log the current time + times[id].start = boost::chrono::system_clock::now(); +} + +int64_t HealthMonitor::toc(std::string id) { + // get elapsed time if tic has been called. + if (times.count(id) > 0) { + typedef boost::chrono::duration microseconds; + microseconds delta_us = boost::chrono::duration_cast( + boost::chrono::system_clock::now() - times[id].start); + int64_t count = delta_us.count(); + times[id].elapsed += count; + times[id].calls++; + times[id].min_us = std::min(count, times[id].min_us); + times[id].max_us = std::max(count, times[id].max_us); + + double hz = 1000000. / count; + bool is_hitting_target_hz = hz >= times[id].target_hz; + if (!is_hitting_target_hz) + ROS_ERROR_STREAM(std::setprecision(2) + << "execute function is not running fast enought." + << " Actual Hz: " << hz << " Target Hz: " << times[id].target_hz); + + return count; + } + + // otherwise return -1 + return -1; +} + +double HealthMonitor::get_target_hz(std::string id) { return times[id].target_hz; } + +void HealthMonitor::print_time_statistics() { + ROS_INFO_STREAM("Time Statistics for " << base_->get_node_name()); // node_name_); + int col_width = 15; + ROS_INFO_STREAM(std::setw(col_width) + << "Name" << std::setw(col_width) << "Avg Hz" << std::setw(col_width) + << "Target Hz" << std::setw(col_width) << "Avg ms" << std::setw(col_width) + << "Min ms" << std::setw(col_width) << "Max ms" << std::setw(col_width) + << "Calls"); + + for (std::map::iterator it = times.begin(); it != times.end(); it++) { + if (it->second.calls == 0) { + ROS_INFO_STREAM(it->first << ": toc(\"" << it->first << "\"); was never called"); + } else { + double average_elapsed = (double)it->second.elapsed / (double)it->second.calls; + double average_hz = 1000000. / average_elapsed; + ROS_INFO_STREAM(std::setprecision(2) + << std::setw(col_width) << it->first << std::setw(col_width) + << average_hz << std::setw(col_width) << it->second.target_hz + << std::setw(col_width) << average_elapsed / 1000. + << std::setw(col_width) << (double)it->second.min_us / 1000. + << std::setw(col_width) << (double)it->second.max_us / 1000. + << std::setw(col_width) << it->second.calls); + } + } +} diff --git a/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/main.cpp b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/main.cpp new file mode 100644 index 000000000..9e2696ee9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/base_main_class/src/main.cpp @@ -0,0 +1,25 @@ +#include "base/BaseNode.h" +#include +#include + +BaseNode* base_node; + +void interrupt_handler(int sig){ + delete base_node; + ros::shutdown(); +} + +int main(int argc, char** argv){ + BaseNode::argc = argc; + BaseNode::argv = argv; + base_node = BaseNode::get(); + + signal(SIGINT, interrupt_handler); + + if(base_node->_initialize()) + ros::spin(); + else + ROS_FATAL_STREAM("Node initialization failed. Exiting."); + + return 0; +} diff --git a/ros_ws/src/robot/autonomy/planning/local/core_disparity_map_representation b/ros_ws/src/robot/autonomy/planning/local/core_disparity_map_representation deleted file mode 160000 index d858097c9..000000000 --- a/ros_ws/src/robot/autonomy/planning/local/core_disparity_map_representation +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d858097c9bfad8f1040f04971429899c8db6aa51 diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/CMakeLists.txt index 22e424eb6..fcf8381aa 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/CMakeLists.txt @@ -1,20 +1,33 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(core_local_planner) -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - sensor_msgs - nav_msgs - core_trajectory_library - core_trajectory_controller - core_map_representation_interface - tflib - actionlib_msgs - behavior_tree - core_trajectory_msgs - pluginlib -) +#find_package(catkin REQUIRED COMPONENTS +# roscpp +# std_msgs +# sensor_msgs +# nav_msgs +# trajectory_library +# trajectory_controller +# core_map_representation_interface +# tflib +# actionlib_msgs +# behavior_tree +# airstack_msgs +# pluginlib +#) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(trajectory_library REQUIRED) +find_package(trajectory_controller REQUIRED) +find_package(core_map_representation_interface REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(actionlib_msgs REQUIRED) +find_package(behavior_tree REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(pluginlib REQUIRED) generate_messages( DEPENDENCIES actionlib_msgs std_msgs @@ -27,16 +40,45 @@ catkin_package( ) include_directories( - ${catkin_INCLUDE_DIRS} - ${base_INCLUDE_DIRS} include + ${trajectory_library_INCLUDE_DIRS} + ${trajectory_controller_INCLUDE_DIRS} + ${core_map_representation_interface_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${behaviortree_cpp_v3_INCLUDE_DIRS} + ) add_executable(local_planner src/local_planner.cpp) +ament_target_dependencies(local_planner +rclcpp +rclpy +std_msgs +sensor_msgs +nav_msgs +trajectory_library +trajectory_controller +core_map_representation_interface +tf2_ros +action_msgs +airstack_msgs +pluginlib + # Add other dependencies here +) -add_dependencies(local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${base_EXPORTED_TARGETS}) +##add_dependencies(local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${base_EXPORTED_TARGETS}) target_link_libraries(local_planner ${catkin_LIBRARIES} ${base_LIBRARIES} ) + +install(TARGETS local_planner + DESTINATION lib/${PROJECT_NAME} +) +install(FILES package.xml + DESTINATION share/${PROJECT_NAME} +) +#install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/include/core_local_planner/local_planner.h b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/include/core_local_planner/local_planner.h index 983afeb21..fb4b2a360 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/include/core_local_planner/local_planner.h +++ b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/include/core_local_planner/local_planner.h @@ -1,109 +1,124 @@ #ifndef _LOCAL_PLANNER_H_ #define _LOCAL_PLANNER_H_ +#include #include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include +#include #include #include -#include -#include -#include +#include +#include +#include #include #include -//#include -//#include -//#include -#include -//#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include class LocalPlanner : public BaseNode { -private: - TrajectoryLibrary* traj_lib; - - std::string map_representation; - bool got_global_plan; - core_trajectory_msgs::TrajectoryXYZVYaw global_plan; - double global_plan_trajectory_distance; - bool got_look_ahead, got_tracking_point; - nav_msgs::Odometry look_ahead_odom, tracking_point_odom; - - std::vector static_trajectories; - - double waypoint_spacing, obstacle_check_radius, obstacle_penalty_weight, forward_progress_penalty_weight; - double robot_radius; - int obstacle_check_points; - - double look_past_distance; - - float waypoint_buffer_duration, waypoint_spacing_threshold, waypoint_angle_threshold; - std::list waypoint_buffer; - - // bool use_fixed_height; - const int GLOBAL_PLAN_HEIGHT = 0; - const int FIXED_HEIGHT = 1; - const int RANGE_SENSOR_HEIGHT = 2; - int height_mode; - double height_above_ground; - double fixed_height; - bool got_range_up, got_range_down; - sensor_msgs::Range range_up, range_down; - - const int TRAJECTORY_YAW = 0; - const int SMOOTH_YAW = 1; - int yaw_mode; - - - // custom waypoint params - enum GoalMode {CUSTOM_WAYPOINT, AUTO_WAYPOINT, TRAJECTORY}; - GoalMode goal_mode; - double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold; - - //MapRepresentationDeprecated* map; - //MapRepresentation* pc_map; - boost::shared_ptr pc_map; - - // subscribers - ros::Subscriber global_plan_sub, waypoint_sub, look_ahead_sub, tracking_point_sub, range_up_sub, range_down_sub, custom_waypoint_sub; - tf::TransformListener* listener; - - // publishers - ros::Publisher vis_pub, traj_pub, traj_track_pub, obst_vis_pub, global_plan_vis_pub, look_past_vis_pub; - - // services - ros::ServiceClient traj_mode_client; - - bool get_best_trajectory(std::vector trajs, - Trajectory global_plan, Trajectory* best_traj); - void update_waypoint_mode(); - -public: - LocalPlanner(std::string node_name); - - virtual bool initialize(); - virtual bool execute(); - virtual ~LocalPlanner(); - - // subscriber callbacks - void global_plan_callback(core_trajectory_msgs::TrajectoryXYZVYaw global_plan); - void waypoint_callback(geometry_msgs::PointStamped wp); - void custom_waypoint_callback(geometry_msgs::PoseStamped wp); - void look_ahead_callback(nav_msgs::Odometry odom); - void tracking_point_callback(nav_msgs::Odometry odom); - void range_up_callback(sensor_msgs::Range range); - void range_down_callback(sensor_msgs::Range range); -}; + private: + TrajectoryLibrary* traj_lib; + + std::string map_representation; + bool got_global_plan; + airstack_msgs::msg::TrajectoryXYZVYaw global_plan; + double global_plan_trajectory_distance; + bool got_look_ahead, got_tracking_point; + nav_msgs::msg::Odometry look_ahead_odom, tracking_point_odom; + + std::vector static_trajectories; + + double waypoint_spacing, obstacle_check_radius, obstacle_penalty_weight, + forward_progress_penalty_weight; + double robot_radius; + int obstacle_check_points; + + double look_past_distance; + + float waypoint_buffer_duration, waypoint_spacing_threshold, waypoint_angle_threshold; + std::list waypoint_buffer; + + // bool use_fixed_height; + const int GLOBAL_PLAN_HEIGHT = 0; + const int FIXED_HEIGHT = 1; + const int RANGE_SENSOR_HEIGHT = 2; + int height_mode; + double height_above_ground; + double fixed_height; + bool got_range_up, got_range_down; + sensor_msgs::msg::Range range_up, range_down; + const int TRAJECTORY_YAW = 0; + const int SMOOTH_YAW = 1; + int yaw_mode; + + // custom waypoint params + enum GoalMode { CUSTOM_WAYPOINT, AUTO_WAYPOINT, TRAJECTORY }; + GoalMode goal_mode; + double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold; + + // MapRepresentationDeprecated* map; + // MapRepresentation* pc_map; + std::shared_ptr pc_map; + + rclcpp::Subscription::SharedPtr global_plan_sub; + rclcpp::Subscription::SharedPtr waypoint_sub; + rclcpp::Subscription::SharedPtr look_ahead_sub; + rclcpp::Subscription::SharedPtr tracking_point_sub; + rclcpp::Subscription::SharedPtr range_up_sub; + rclcpp::Subscription::SharedPtr range_down_sub; + rclcpp::Subscription::SharedPtr custom_waypoint_sub; + // subscribers + // ros::Subscriber global_plan_sub, waypoint_sub, look_ahead_sub, tracking_point_sub, + // range_up_sub, + // range_down_sub, custom_waypoint_sub; + // tf::TransformListener* listener; + std::shared_ptr tf_listener; + + // publishers + // ros::Publisher vis_pub, traj_pub, traj_track_pub, obst_vis_pub, global_plan_vis_pub, + // look_past_vis_pub; + rclcpp::Publisher::SharedPtr vis_pub; + rclcpp::Publisher::SharedPtr traj_pub; + rclcpp::Publisher::SharedPtr traj_track_pub; + rclcpp::Publisher::SharedPtr obst_vis_pub; + rclcpp::Publisher::SharedPtr global_plan_vis_pub; + rclcpp::Publisher::SharedPtr look_past_vis_pub; + + // services + // ros::ServiceClient traj_mode_client; + rclcpp::Client::SharedPtr traj_mode_client; + + bool get_best_trajectory(std::vector trajs, Trajectory global_plan, + Trajectory* best_traj); + void update_waypoint_mode(); + + public: + LocalPlanner(const std::string node_name); + + virtual bool initialize(); + virtual bool execute(); + virtual ~LocalPlanner(); + + // subscriber callbacks + void global_plan_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr global_plan); + void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr wp); + void custom_waypoint_callback(const geometry_msgs::msg::PoseStamped::SharedPtr wp); + void look_ahead_callback(const nav_msgs::msg::Odometry::SharedPtr odom); + void tracking_point_callback(const nav_msgs::msg::Odometry::SharedPtr odom); + void range_up_callback(const sensor_msgs::msg::Range::SharedPtr range); + void range_down_callback(const sensor_msgs::msg::Range::SharedPtr range); +}; #endif diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner.launch b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner.launch index 0fef9afec..b8649a7ae 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner.launch +++ b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner.launch @@ -11,7 +11,7 @@ - + diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/package.xml b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/package.xml index 6655f785b..5fd3debff 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/package.xml +++ b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/package.xml @@ -48,52 +48,25 @@ - catkin - base - roscpp - rospy - std_msgs - nav_msgs - core_trajectory_library - core_trajectory_controller - core_map_representation_interface - tflib - actionlib_msgs - behavior_tree - core_trajectory_msgs - pluginlib - - base - roscpp - rospy - std_msgs - nav_msgs - core_trajectory_library - core_trajectory_controller - core_map_representation_interface - tflib - actionlib_msgs - behavior_tree - core_trajectory_msgs - pluginlib - - base - roscpp - rospy - std_msgs - nav_msgs - core_trajectory_library - core_trajectory_controller - core_map_representation_interface - tflib - message_generation - behavior_tree - core_trajectory_msgs - pluginlib - - - - + ament_cmake + + rclcpp + rclpy + std_msgs + nav_msgs + trajectory_library + trajectory_controller + core_map_representation_interface + tf2_ros + action_msgs + behaviortree_cpp_v3 + airstack_msgs + pluginlib + rosidl_default_generators + rosidl_default_runtime + + + ament_cmake - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/src/local_planner.cpp b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/src/local_planner.cpp index 862530a2b..5d49559f3 100644 --- a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/src/local_planner.cpp +++ b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/src/local_planner.cpp @@ -1,683 +1,644 @@ #include -#include -#include +#include #include +#include +#include + #include -#include -//#include +// #include #include - //=================================================================================== //--------------------------------- Local Planner ----------------------------------- //=================================================================================== -LocalPlanner::LocalPlanner(std::string node_name) - : BaseNode(node_name){ -} +LocalPlanner::LocalPlanner(std::string node_name) : BaseNode(node_name) {} + +bool LocalPlanner::initialize() { + ros::NodeHandle* nh = get_node_handle(); + ros::NodeHandle* pnh = get_private_node_handle(); + + // init subscribers + global_plan_sub = nh->subscribe("global_plan", 10, &LocalPlanner::global_plan_callback, this); + waypoint_sub = nh->subscribe("way_point", 10, &LocalPlanner::waypoint_callback, this); + look_ahead_sub = nh->subscribe("look_ahead", 10, &LocalPlanner::look_ahead_callback, this); + tracking_point_sub = + nh->subscribe("tracking_point", 10, &LocalPlanner::tracking_point_callback, this); + range_up_sub = + nh->subscribe("range_up", 1, &LocalPlanner::range_up_callback, this); + range_down_sub = nh->subscribe("range_down", 1, + &LocalPlanner::range_down_callback, this); + custom_waypoint_sub = nh->subscribe( + "custom_waypoint", 1, &LocalPlanner::custom_waypoint_callback, this); + listener = new tf::TransformListener(); + + // init publishers + vis_pub = nh->advertise("trajectory_library_vis", 10); + obst_vis_pub = nh->advertise("obstaccle_vis", 10); + global_plan_vis_pub = + nh->advertise("local_planner_global_plan_vis", 10); + look_past_vis_pub = nh->advertise("look_past", 10); + traj_pub = nh->advertise("trajectory", 10); + traj_track_pub = nh->advertise("trajectory_track", 10); + + // init services + traj_mode_client = + nh->serviceClient("set_trajectory_mode"); + + // init parameters + waypoint_spacing = pnh->param("waypoint_spacing", 0.5); + // obstacle_check_radius = pnh->param("obstacle_check_radius", 3.); + // obstacle_check_points = pnh->param("obstacle_check_points", 3); + obstacle_penalty_weight = pnh->param("obstacle_penalty_weight", 1.); + forward_progress_penalty_weight = pnh->param("forward_progress_penalty_weight", 0.5); + robot_radius = pnh->param("robot_radius", 0.75); + look_past_distance = pnh->param("look_past_distance", 0); + // use_fixed_height = pnh->param("use_fixed_height", false); + height_mode = pnh->param("height_mode", 0); + height_above_ground = pnh->param("height_above_ground", 1.); + fixed_height = pnh->param("fixed_height", 1.); + yaw_mode = pnh->param("yaw_mode", 0); + map_representation = + pnh->param("map_representation", std::string("PointCloudMapRepresentation")); + + waypoint_buffer_duration = pnh->param("waypoint_buffer_duration", 30.); + waypoint_spacing_threshold = pnh->param("waypoint_spacing_threshold", 0.5); + waypoint_angle_threshold = pnh->param("waypoint_angle_threshold", 30.) * M_PI / 180.; + + goal_mode = TRAJECTORY; + custom_waypoint_timeout_factor = pnh->param("custom_waypoint_timeout_factor", 0.3); + custom_waypoint_distance_threshold = pnh->param("custom_waypoint_distance_threshold", 0.5); + + std::string traj_lib_config_filename = pnh->param("trajectory_library_config", std::string("")); + if (traj_lib_config_filename == "") { + ROS_ERROR_STREAM("Trajectory library config file is invalid: " << traj_lib_config_filename); + return false; + } -bool LocalPlanner::initialize(){ - ros::NodeHandle* nh = get_node_handle(); - ros::NodeHandle* pnh = get_private_node_handle(); - - // init subscribers - global_plan_sub = nh->subscribe("global_plan", 10, &LocalPlanner::global_plan_callback, this); - waypoint_sub = nh->subscribe("way_point", 10, &LocalPlanner::waypoint_callback, this); - look_ahead_sub = nh->subscribe("look_ahead", 10, &LocalPlanner::look_ahead_callback, this); - tracking_point_sub = nh->subscribe("tracking_point", 10, &LocalPlanner::tracking_point_callback, this); - range_up_sub = nh->subscribe("range_up", 1, &LocalPlanner::range_up_callback, this); - range_down_sub = nh->subscribe("range_down", 1, &LocalPlanner::range_down_callback, this); - custom_waypoint_sub = nh->subscribe("custom_waypoint", 1, - &LocalPlanner::custom_waypoint_callback, this); - listener = new tf::TransformListener(); - - // init publishers - vis_pub = nh->advertise("trajectory_library_vis", 10); - obst_vis_pub = nh->advertise("obstaccle_vis", 10); - global_plan_vis_pub = - nh->advertise("local_planner_global_plan_vis", 10); - look_past_vis_pub = - nh->advertise("look_past", 10); - traj_pub = nh->advertise("trajectory", 10); - traj_track_pub = nh->advertise("trajectory_track", 10); - - // init services - traj_mode_client = nh->serviceClient("set_trajectory_mode"); - - // init parameters - waypoint_spacing = pnh->param("waypoint_spacing", 0.5); - //obstacle_check_radius = pnh->param("obstacle_check_radius", 3.); - //obstacle_check_points = pnh->param("obstacle_check_points", 3); - obstacle_penalty_weight = pnh->param("obstacle_penalty_weight", 1.); - forward_progress_penalty_weight = pnh->param("forward_progress_penalty_weight", 0.5); - robot_radius = pnh->param("robot_radius", 0.75); - look_past_distance = pnh->param("look_past_distance", 0); - //use_fixed_height = pnh->param("use_fixed_height", false); - height_mode = pnh->param("height_mode", 0); - height_above_ground = pnh->param("height_above_ground", 1.); - fixed_height = pnh->param("fixed_height", 1.); - yaw_mode = pnh->param("yaw_mode", 0); - map_representation = pnh->param("map_representation", std::string("PointCloudMapRepresentation")); - - waypoint_buffer_duration = pnh->param("waypoint_buffer_duration", 30.); - waypoint_spacing_threshold = pnh->param("waypoint_spacing_threshold", 0.5); - waypoint_angle_threshold = pnh->param("waypoint_angle_threshold", 30.)*M_PI/180.; - - goal_mode = TRAJECTORY; - custom_waypoint_timeout_factor = pnh->param("custom_waypoint_timeout_factor", 0.3); - custom_waypoint_distance_threshold = pnh->param("custom_waypoint_distance_threshold", 0.5); - - std::string traj_lib_config_filename = pnh->param("trajectory_library_config", std::string("")); - if(traj_lib_config_filename == ""){ - ROS_ERROR_STREAM("Trajectory library config file is invalid: " << traj_lib_config_filename); - return false; - } - - traj_lib = new TrajectoryLibrary(traj_lib_config_filename, listener); - got_global_plan = false; - global_plan_trajectory_distance = 0; - got_look_ahead = false; - got_tracking_point = false; - got_range_up = false; - got_range_down = false; - - - pluginlib::ClassLoader map_representation_loader("core_map_representation_interface", "MapRepresentation"); - try{ - pc_map = map_representation_loader.createInstance(map_representation); - }catch(pluginlib::PluginlibException& ex){ - ROS_ERROR("The MapRepresentation plugin failed to load. Error: %s", ex.what()); - } - - // init static trajectories and respace them - static_trajectories = traj_lib->get_static_trajectories(); - for(int i = 0; i < static_trajectories.size(); i++) - static_trajectories[i] = static_trajectories[i].respace(waypoint_spacing); - - return true; -} + // 2024.08.16 AndrewJ TODO: this is a naked pointer, should changed to a unique_ptr + traj_lib = new TrajectoryLibrary(traj_lib_config_filename, listener); + got_global_plan = false; + global_plan_trajectory_distance = 0; + got_look_ahead = false; + got_tracking_point = false; + got_range_up = false; + got_range_down = false; + + pluginlib::ClassLoader map_representation_loader( + "core_map_representation_interface", "MapRepresentation"); + try { + pc_map = map_representation_loader.createInstance(map_representation); + } catch (pluginlib::PluginlibException& ex) { + ROS_ERROR("The MapRepresentation plugin failed to load. Error: %s", ex.what()); + } -bool LocalPlanner::execute(){ - update_waypoint_mode(); + // init static trajectories and respace them + static_trajectories = traj_lib->get_static_trajectories(); + for (int i = 0; i < static_trajectories.size(); i++) + static_trajectories[i] = static_trajectories[i].respace(waypoint_spacing); - if(!got_global_plan) return true; - - Trajectory gp(global_plan); - //ROS_INFO_STREAM("initial gp waypoints size: " << gp.waypoint_count()); - //gp = gp.get_subtrajectory_distance(global_plan_trajectory_distance, - // global_plan_trajectory_distance + 10.); - - // set the hieght of the global plan - if(height_mode == FIXED_HEIGHT){ - gp.set_fixed_height(fixed_height); - } - else if(height_mode == RANGE_SENSOR_HEIGHT){ - if(!got_range_up || !got_range_down) - return true; - - try{ - tf::StampedTransform transform_up, transform_down; - listener->waitForTransform(gp.get_frame_id(), range_up.header.frame_id, range_up.header.stamp, ros::Duration(0.1)); - listener->lookupTransform(gp.get_frame_id(), range_up.header.frame_id, range_up.header.stamp, transform_up); - listener->waitForTransform(gp.get_frame_id(), range_down.header.frame_id, range_down.header.stamp, ros::Duration(0.1)); - listener->lookupTransform(gp.get_frame_id(), range_down.header.frame_id, range_down.header.stamp, transform_down); - - tf::Vector3 range_up_gp_frame = transform_up*tf::Vector3(range_up.range, 0, 0); - tf::Vector3 range_down_gp_frame = transform_down*tf::Vector3(range_down.range, 0, 0); - - /* - if(gp.waypoint_count() > 0){ - double z_setpoint = gp.get_waypoint(0).z(); - z_setpoint = std::min(range_up_gp_frame.z() - 1.5*robot_radius, std::max(range_down_gp_frame.z() + 1.5*robot_radius, z_setpoint)); - gp.set_fixed_height(z_setpoint); - } - */ - //* - double tunnel_height = range_up_gp_frame.z() - range_down_gp_frame.z(); - double z_setpoint = (range_up_gp_frame.z() + range_down_gp_frame.z())/2.; - if(tunnel_height/2. >= height_above_ground) - z_setpoint = range_down_gp_frame.z() + height_above_ground; - /* - if(z_setpoint >= range_up_gp_frame.z() - robot_radius) - z_setpoint = (range_up_gp_frame.z() + range_down_gp_frame.z())/2.; - */ - - gp.set_fixed_height(z_setpoint); - //*/ - } - catch(tf::TransformException& ex){ - ROS_ERROR_STREAM("Error transforming range data. " << ex.what()); - } - } - - // transform the look ahead point to the global plan frame - tf::Vector3 look_ahead_position = tflib::to_tf(look_ahead_odom.pose.pose.position); - bool success = tflib::to_frame(listener, look_ahead_position, - look_ahead_odom.header.frame_id, gp.get_frame_id(), - look_ahead_odom.header.stamp, &look_ahead_position); - if(!success){ - ROS_ERROR_STREAM("Couldn't transform from lookahead frame to global plan frame"); - return true; - } - - // increment how far along the global plan we are - double trajectory_distance; - bool valid = gp.get_trajectory_distance_at_closest_point(look_ahead_position, - &trajectory_distance); - if(valid){ - global_plan_trajectory_distance += trajectory_distance; - gp = gp.get_subtrajectory_distance(trajectory_distance, - trajectory_distance + 10.); - //ROS_INFO_STREAM("after trim waypoints size: " << gp.waypoint_count()); - } - else - ROS_INFO("invalid"); - - // publish the segment of the global plan currently being used, for visualization - visualization_msgs::MarkerArray global_markers = gp.get_markers(0, 0, 1); - global_plan_vis_pub.publish(global_markers); - - // get the dynamic trajectories - std::vector dynamic_trajectories = traj_lib->get_dynamic_trajectories(look_ahead_odom); - - // pick the best trajectory - monitor.tic("get_best_trajectory"); - Trajectory best_traj; - bool all_in_collision = get_best_trajectory(dynamic_trajectories, gp, &best_traj); - monitor.toc("get_best_trajectory"); - - // publish the trajectory - //ROS_INFO_STREAM("all_in_collsion: " << all_in_collision); - if(!all_in_collision){ - core_trajectory_msgs::TrajectoryXYZVYaw path = best_traj.get_TrajectoryXYZVYaw(); - - // set yaw - if(yaw_mode == SMOOTH_YAW && path.waypoints.size() > 0){ - bool found_initial_heading = false; - double initial_heading = 0; - try{ - tf::StampedTransform transform; - listener->waitForTransform(best_traj.get_frame_id(), look_ahead_odom.header.frame_id, look_ahead_odom.header.stamp, ros::Duration(0.1)); - listener->lookupTransform(best_traj.get_frame_id(), look_ahead_odom.header.frame_id, look_ahead_odom.header.stamp, transform); - - transform.setOrigin(tf::Vector3(0, 0, 0)); // only care about rotation - initial_heading = tf::getYaw(transform*tflib::to_tf(look_ahead_odom.pose.pose.orientation)); - - found_initial_heading = true; - } - catch(const tf::TransformException& ex){ - ROS_ERROR_STREAM("Transform exception while finding yaw of lookahead: " << ex.what()); - } - - if(found_initial_heading){ - path.waypoints[0].yaw = initial_heading; - - double alpha = 0.1; - double sin_yaw_prev = sin(path.waypoints[0].yaw); - double cos_yaw_prev = cos(path.waypoints[0].yaw); - - //ROS_INFO("-------------------------------------------------"); - //ROS_INFO_STREAM("heading: " << 180./M_PI*tf::getYaw(tflib::to_tf(look_ahead_odom.pose.pose.orientation))); - for(int i = 1; i < path.waypoints.size(); i++){ - core_trajectory_msgs::WaypointXYZVYaw wp_prev = path.waypoints[i-1]; - core_trajectory_msgs::WaypointXYZVYaw& wp_curr = path.waypoints[i]; - - //ROS_INFO_STREAM(wp_curr.position.y - wp_prev.position.y << " " << wp_curr.position.x - wp_prev.position.x); - double yaw = atan2(wp_curr.position.y - wp_prev.position.y, wp_curr.position.x - wp_prev.position.x); - double cos_yaw = alpha*cos(yaw) + (1 - alpha)*cos_yaw_prev; - double sin_yaw = alpha*sin(yaw) + (1 - alpha)*sin_yaw_prev; - yaw = atan2(sin_yaw, cos_yaw); - - sin_yaw_prev = sin_yaw; - cos_yaw_prev = cos_yaw; - - wp_curr.yaw = yaw; - } - } - } - - path.header.stamp = ros::Time::now(); // TODO: make this match the time inside get_best_traj - traj_pub.publish(path); - } - - return true; } -bool LocalPlanner::get_best_trajectory(std::vector trajectories, - Trajectory global_plan, Trajectory* best_traj){ - //ROS_INFO("GET_BEST"); - double min_cost = std::numeric_limits::max(); - int best_traj_index = 0; - bool all_in_collision = true; - - ros::Time now = ros::Time::now(); - - visualization_msgs::MarkerArray traj_lib_markers, look_past_markers; - - - /*std::vector trajs; - for(int i = 0; i < trajectories.size(); i++) - trajs.push_back(trajectories[i].get_TrajectoryXYZVYaw()); - */ - std::vector > trajs; - for(int i = 0; i < trajectories.size(); i++) - trajs.push_back(trajectories[i].get_vector_PointStamped()); - std::vector< std::vector > values = pc_map->get_values(trajs); - - for(int i = 0; i < trajectories.size(); i++){ - Trajectory traj = trajectories[i]; - double average_distance = std::numeric_limits::infinity(); - double closest_obstacle_distance = std::numeric_limits::infinity(); - - Trajectory global_plan_traj_frame; - try{ - global_plan_traj_frame = global_plan.to_frame(traj.get_frame_id(), now); - } - catch(tf::TransformException& ex){ - ROS_ERROR_STREAM("TransformException in get_best_trajectory: " << ex.what()); - return true; +bool LocalPlanner::execute() { + update_waypoint_mode(); + + if (!got_global_plan) return true; + + Trajectory gp(global_plan); + // ROS_INFO_STREAM("initial gp waypoints size: " << gp.waypoint_count()); + // gp = gp.get_subtrajectory_distance(global_plan_trajectory_distance, + // global_plan_trajectory_distance + 10.); + + // set the hieght of the global plan + if (height_mode == FIXED_HEIGHT) { + gp.set_fixed_height(fixed_height); + } else if (height_mode == RANGE_SENSOR_HEIGHT) { + if (!got_range_up || !got_range_down) return true; + + try { + tf::StampedTransform transform_up, transform_down; + listener->waitForTransform(gp.get_frame_id(), range_up.header.frame_id, + range_up.header.stamp, ros::Duration(0.1)); + listener->lookupTransform(gp.get_frame_id(), range_up.header.frame_id, + range_up.header.stamp, transform_up); + listener->waitForTransform(gp.get_frame_id(), range_down.header.frame_id, + range_down.header.stamp, ros::Duration(0.1)); + listener->lookupTransform(gp.get_frame_id(), range_down.header.frame_id, + range_down.header.stamp, transform_down); + + tf::Vector3 range_up_gp_frame = transform_up * tf::Vector3(range_up.range, 0, 0); + tf::Vector3 range_down_gp_frame = transform_down * tf::Vector3(range_down.range, 0, 0); + + /* + if(gp.waypoint_count() > 0){ + double z_setpoint = gp.get_waypoint(0).z(); + z_setpoint = std::min(range_up_gp_frame.z() - 1.5*robot_radius, + std::max(range_down_gp_frame.z() + 1.5*robot_radius, z_setpoint)); + gp.set_fixed_height(z_setpoint); + } + */ + //* + double tunnel_height = range_up_gp_frame.z() - range_down_gp_frame.z(); + double z_setpoint = (range_up_gp_frame.z() + range_down_gp_frame.z()) / 2.; + if (tunnel_height / 2. >= height_above_ground) + z_setpoint = range_down_gp_frame.z() + height_above_ground; + /* + if(z_setpoint >= range_up_gp_frame.z() - robot_radius) + z_setpoint = (range_up_gp_frame.z() + range_down_gp_frame.z())/2.; + */ + + gp.set_fixed_height(z_setpoint); + //*/ + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("Error transforming range data. " << ex.what()); + } } - for(int j = 0; j < traj.waypoint_count(); j++){ - //for(int j = traj.waypoint_count()-1; j < traj.waypoint_count(); j++){ - Waypoint wp = traj.get_waypoint(j); - - // find how far this waypoint is from an obstacle - nav_msgs::Odometry odom = wp.odometry(now, traj.get_frame_id()); - geometry_msgs::PoseStamped pose; - pose.header = odom.header; - pose.pose = odom.pose.pose; - closest_obstacle_distance = std::min(closest_obstacle_distance, - values[i][j]); - - // find how far this waypoint is from the global plan path - tf::Vector3 closest_point; - int wp_index; - double path_distance; - bool valid = global_plan_traj_frame.get_closest_point(wp.position(), - &closest_point, &wp_index, &path_distance); - double forward_progress_penalty = -forward_progress_penalty_weight*path_distance;//(global_plan_traj_frame.waypoint_count()-wp_index)*0.5; - - if(valid){ - if(!std::isfinite(average_distance)) - average_distance = 0; - - //average_distance += closest_point.distance(wp.position()); - average_distance += closest_point.distance(wp.position()) + forward_progress_penalty; - } + // transform the look ahead point to the global plan frame + tf::Vector3 look_ahead_position = tflib::to_tf(look_ahead_odom.pose.pose.position); + bool success = + tflib::to_frame(listener, look_ahead_position, look_ahead_odom.header.frame_id, + gp.get_frame_id(), look_ahead_odom.header.stamp, &look_ahead_position); + if (!success) { + ROS_ERROR_STREAM("Couldn't transform from lookahead frame to global plan frame"); + return true; } - //ROS_INFO_STREAM("CLOSEST: " << closest_obstacle_distance); - - average_distance /= traj.waypoint_count(); - bool collision = closest_obstacle_distance <= robot_radius; - if(!collision) - all_in_collision = false; - - visualization_msgs::MarkerArray traj_markers; - if(collision) - traj_markers = traj.get_markers(1, 0, 0, .5); - else - traj_markers = traj.get_markers(0, 1, 0, .5); - - - if(look_past_distance > 0){ - if(traj.waypoint_count() >= 2){ - Waypoint curr_wp = traj.get_waypoint(traj.waypoint_count()-1); - Waypoint prev_wp = traj.get_waypoint(traj.waypoint_count()-2); - - tf::Vector3 segment = curr_wp.position() - prev_wp.position(); - segment.normalize(); - - tf::Vector3 position = curr_wp.position() + look_past_distance*segment; - - tf::Vector3 closest_point; - int wp_index; - bool valid = global_plan_traj_frame.get_closest_point(position, - &closest_point, &wp_index); - - if(!valid) - collision = true; - else{ - average_distance = position.distance(closest_point); - } - - visualization_msgs::Marker marker; - marker.header.stamp = now; - marker.header.frame_id = traj.get_frame_id(); - marker.ns = "look_past"; - marker.id = i; - marker.type = visualization_msgs::Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - - marker.pose.position.x = position.x(); - marker.pose.position.y = position.y(); - marker.pose.position.z = position.z(); - marker.pose.orientation.w = 1; - marker.scale.x = 0.3; - marker.scale.y = 0.3; - marker.scale.z = 0.3; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - marker.color.a = 1.0; - look_past_markers.markers.push_back(marker); - } + + // increment how far along the global plan we are + double trajectory_distance; + bool valid = + gp.get_trajectory_distance_at_closest_point(look_ahead_position, &trajectory_distance); + if (valid) { + global_plan_trajectory_distance += trajectory_distance; + gp = gp.get_subtrajectory_distance(trajectory_distance, trajectory_distance + 10.); + // ROS_INFO_STREAM("after trim waypoints size: " << gp.waypoint_count()); + } else + ROS_INFO("invalid"); + + // publish the segment of the global plan currently being used, for visualization + visualization_msgs::MarkerArray global_markers = gp.get_markers(0, 0, 1); + global_plan_vis_pub.publish(global_markers); + + // get the dynamic trajectories + std::vector dynamic_trajectories = + traj_lib->get_dynamic_trajectories(look_ahead_odom); + + // pick the best trajectory + monitor.tic("get_best_trajectory"); + Trajectory best_traj; + bool all_in_collision = get_best_trajectory(dynamic_trajectories, gp, &best_traj); + monitor.toc("get_best_trajectory"); + + // publish the trajectory + // ROS_INFO_STREAM("all_in_collsion: " << all_in_collision); + if (!all_in_collision) { + airstack_msgs::TrajectoryXYZVYaw path = best_traj.get_TrajectoryXYZVYaw(); + + // set yaw + if (yaw_mode == SMOOTH_YAW && path.waypoints.size() > 0) { + bool found_initial_heading = false; + double initial_heading = 0; + try { + // rotate + tf::StampedTransform transform; + // translate from global frame to best_traj frame + listener->waitForTransform(best_traj.get_frame_id(), + look_ahead_odom.header.frame_id, + look_ahead_odom.header.stamp, ros::Duration(0.1)); + listener->lookupTransform(best_traj.get_frame_id(), look_ahead_odom.header.frame_id, + look_ahead_odom.header.stamp, transform); + + transform.setOrigin(tf::Vector3(0, 0, 0)); // only care about rotation + // get the yaw of the lookahead odom in body frame, apparently? + initial_heading = + tf::getYaw(transform * tflib::to_tf(look_ahead_odom.pose.pose.orientation)); + + found_initial_heading = true; + } catch (const tf::TransformException& ex) { + ROS_ERROR_STREAM( + "Transform exception while finding yaw of lookahead: " << ex.what()); + } + + if (found_initial_heading) { + path.waypoints[0].yaw = initial_heading; + + double alpha = 0.1; + double sin_yaw_prev = sin(path.waypoints[0].yaw); + double cos_yaw_prev = cos(path.waypoints[0].yaw); + + // ROS_INFO("-------------------------------------------------"); + // ROS_INFO_STREAM("heading: " << + // 180./M_PI*tf::getYaw(tflib::to_tf(look_ahead_odom.pose.pose.orientation))); + for (int i = 1; i < path.waypoints.size(); i++) { + airstack_msgs::WaypointXYZVYaw wp_prev = path.waypoints[i - 1]; + airstack_msgs::WaypointXYZVYaw& wp_curr = path.waypoints[i]; + + // ROS_INFO_STREAM(wp_curr.position.y - wp_prev.position.y << " " << + // wp_curr.position.x - wp_prev.position.x); + double yaw = atan2(wp_curr.position.y - wp_prev.position.y, + wp_curr.position.x - wp_prev.position.x); + double cos_yaw = alpha * cos(yaw) + (1 - alpha) * cos_yaw_prev; + double sin_yaw = alpha * sin(yaw) + (1 - alpha) * sin_yaw_prev; + yaw = atan2(sin_yaw, cos_yaw); + + sin_yaw_prev = sin_yaw; + cos_yaw_prev = cos_yaw; + + wp_curr.yaw = yaw; + } + } + } + + path.header.stamp = + ros::Time::now(); // TODO: make this match the time inside get_best_traj + traj_pub.publish(path); } - - traj_lib_markers.markers.insert(traj_lib_markers.markers.end(), - traj_markers.markers.begin(), traj_markers.markers.end()); - - double cost = average_distance - obstacle_penalty_weight*std::min(closest_obstacle_distance, obstacle_check_radius); - if(!collision && cost < min_cost){ - min_cost = cost; - best_traj_index = i; - *best_traj = traj; + + return true; +} + +bool LocalPlanner::get_best_trajectory(std::vector trajectories, Trajectory global_plan, + Trajectory* best_traj) { + // ROS_INFO("GET_BEST"); + double min_cost = std::numeric_limits::max(); + int best_traj_index = 0; + bool all_in_collision = true; + + ros::Time now = ros::Time::now(); + + visualization_msgs::MarkerArray traj_lib_markers, look_past_markers; + + /*std::vector trajs; + for(int i = 0; i < trajectories.size(); i++) + trajs.push_back(trajectories[i].get_TrajectoryXYZVYaw()); + */ + std::vector > trajs; + for (int i = 0; i < trajectories.size(); i++) + trajs.push_back(trajectories[i].get_vector_PointStamped()); + std::vector > values = pc_map->get_values(trajs); + + for (int i = 0; i < trajectories.size(); i++) { + Trajectory traj = trajectories[i]; + double average_distance = std::numeric_limits::infinity(); + double closest_obstacle_distance = std::numeric_limits::infinity(); + + Trajectory global_plan_traj_frame; + try { + global_plan_traj_frame = global_plan.to_frame(traj.get_frame_id(), now); + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("TransformException in get_best_trajectory: " << ex.what()); + return true; + } + + for (int j = 0; j < traj.waypoint_count(); j++) { + // for(int j = traj.waypoint_count()-1; j < traj.waypoint_count(); j++){ + Waypoint wp = traj.get_waypoint(j); + + // find how far this waypoint is from an obstacle + nav_msgs::Odometry odom = wp.odometry(now, traj.get_frame_id()); + geometry_msgs::PoseStamped pose; + pose.header = odom.header; + pose.pose = odom.pose.pose; + closest_obstacle_distance = std::min(closest_obstacle_distance, values[i][j]); + + // find how far this waypoint is from the global plan path + tf::Vector3 closest_point; + int wp_index; + double path_distance; + bool valid = global_plan_traj_frame.get_closest_point(wp.position(), &closest_point, + &wp_index, &path_distance); + double forward_progress_penalty = + -forward_progress_penalty_weight * + path_distance; //(global_plan_traj_frame.waypoint_count()-wp_index)*0.5; + + if (valid) { + if (!std::isfinite(average_distance)) average_distance = 0; + + // average_distance += closest_point.distance(wp.position()); + average_distance += + closest_point.distance(wp.position()) + forward_progress_penalty; + } + } + // ROS_INFO_STREAM("CLOSEST: " << closest_obstacle_distance); + + average_distance /= traj.waypoint_count(); + bool collision = closest_obstacle_distance <= robot_radius; + if (!collision) all_in_collision = false; + + visualization_msgs::MarkerArray traj_markers; + if (collision) + traj_markers = traj.get_markers(1, 0, 0, .5); + else + traj_markers = traj.get_markers(0, 1, 0, .5); + + if (look_past_distance > 0) { + if (traj.waypoint_count() >= 2) { + Waypoint curr_wp = traj.get_waypoint(traj.waypoint_count() - 1); + Waypoint prev_wp = traj.get_waypoint(traj.waypoint_count() - 2); + + tf::Vector3 segment = curr_wp.position() - prev_wp.position(); + segment.normalize(); + + tf::Vector3 position = curr_wp.position() + look_past_distance * segment; + + tf::Vector3 closest_point; + int wp_index; + bool valid = + global_plan_traj_frame.get_closest_point(position, &closest_point, &wp_index); + + if (!valid) + collision = true; + else { + average_distance = position.distance(closest_point); + } + + visualization_msgs::Marker marker; + marker.header.stamp = now; + marker.header.frame_id = traj.get_frame_id(); + marker.ns = "look_past"; + marker.id = i; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + marker.pose.orientation.w = 1; + marker.scale.x = 0.3; + marker.scale.y = 0.3; + marker.scale.z = 0.3; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + look_past_markers.markers.push_back(marker); + } + } + + traj_lib_markers.markers.insert(traj_lib_markers.markers.end(), + traj_markers.markers.begin(), traj_markers.markers.end()); + + double cost = + average_distance - + obstacle_penalty_weight * std::min(closest_obstacle_distance, obstacle_check_radius); + if (!collision && cost < min_cost) { + min_cost = cost; + best_traj_index = i; + *best_traj = traj; + } } - } - //ROS_INFO_STREAM("Best traj index: " << best_traj_index); + // ROS_INFO_STREAM("Best traj index: " << best_traj_index); - if(best_traj_index < look_past_markers.markers.size()){ - look_past_markers.markers[best_traj_index].color.r = 0; - look_past_markers.markers[best_traj_index].color.g = 1; - look_past_markers.markers[best_traj_index].color.b = 0; - look_past_markers.markers[best_traj_index].color.a = 1; - } + if (best_traj_index < look_past_markers.markers.size()) { + look_past_markers.markers[best_traj_index].color.r = 0; + look_past_markers.markers[best_traj_index].color.g = 1; + look_past_markers.markers[best_traj_index].color.b = 0; + look_past_markers.markers[best_traj_index].color.a = 1; + } - vis_pub.publish(traj_lib_markers); - pc_map->publish_debug(); - look_past_vis_pub.publish(look_past_markers); + vis_pub.publish(traj_lib_markers); + pc_map->publish_debug(); + look_past_vis_pub.publish(look_past_markers); - return all_in_collision; + return all_in_collision; } +void LocalPlanner::global_plan_callback(airstack_msgs::TrajectoryXYZVYaw global_plan) { + ROS_INFO_STREAM("GOT GLOBAL PLAN, goal_mode: " << goal_mode); + if (goal_mode != TRAJECTORY) return; -void LocalPlanner::global_plan_callback(core_trajectory_msgs::TrajectoryXYZVYaw global_plan){ - ROS_INFO_STREAM("GOT GLOBAL PLAN, goal_mode: " << goal_mode); - if(goal_mode != TRAJECTORY) - return; - - this->global_plan = global_plan; - got_global_plan = true; - global_plan_trajectory_distance = 0; + this->global_plan = global_plan; + got_global_plan = true; + global_plan_trajectory_distance = 0; } -void LocalPlanner::waypoint_callback(geometry_msgs::PointStamped wp){ - if(goal_mode != AUTO_WAYPOINT){ - waypoint_buffer.clear(); - waypoint_buffer.push_back(wp); - return; - } - - // TODO: REMOVE THIS - if(wp.point.x == 0 && wp.point.y == 0) - return; - - // remove old waypoints if necessary - waypoint_buffer.push_back(wp); - if((wp.header.stamp - waypoint_buffer.front().header.stamp).toSec() > waypoint_buffer_duration) - waypoint_buffer.pop_front(); - - // stitch together the history of waypoints - core_trajectory_msgs::TrajectoryXYZVYaw global_plan; - global_plan.header.frame_id = wp.header.frame_id; - global_plan.header.stamp = wp.header.stamp; - - std::vector backwards_global_plan; - //double waypoint_spacing_threshold = 0.5; - //double waypoint_angle_threshold = 30.*M_PI/180.; - std::vector prev_wps; - for(auto it = waypoint_buffer.rbegin(); it != waypoint_buffer.rend(); it++){ - geometry_msgs::PointStamped curr_wp = *it; - core_trajectory_msgs::WaypointXYZVYaw waypoint; - waypoint.position.x = curr_wp.point.x; - waypoint.position.y = curr_wp.point.y; - waypoint.position.z = curr_wp.point.z; - - if(prev_wps.size() > 1){ - geometry_msgs::PointStamped prev_wp = prev_wps[prev_wps.size()-1]; - geometry_msgs::PointStamped prev2_wp = prev_wps[prev_wps.size()-2]; - float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + - pow(curr_wp.point.y - prev_wp.point.y, 2)); - - float a = pow(prev_wp.point.x-curr_wp.point.x,2) + pow(prev_wp.point.y-curr_wp.point.y,2); - float b = pow(prev_wp.point.x-prev2_wp.point.x,2) + pow(prev_wp.point.y-prev2_wp.point.y,2); - float c = pow(prev2_wp.point.x-curr_wp.point.x,2) + pow(prev2_wp.point.y-curr_wp.point.y,2); - float angle = acos( (a+b-c) / sqrt(4*a*b) ); - - float angle_diff = fabs(atan2(sin(angle - M_PI), cos(angle - M_PI))); - - //ROS_INFO_STREAM("\tdistance: " << distance << " angle: " << angle*180./M_PI << " angle_diff: " << angle_diff*180./M_PI); - - if(distance >= waypoint_spacing_threshold && angle_diff < waypoint_angle_threshold){ - //ROS_INFO_STREAM("\tADDING wp: " << curr_wp.point.x << " " << curr_wp.point.y << " " << curr_wp.point.z); - backwards_global_plan.push_back(waypoint); - prev_wps.push_back(curr_wp); - } - } - else{ - if(prev_wps.size() == 0){ - //ROS_INFO_STREAM("\tADDING 0 wp: " << curr_wp.point.x << " " << curr_wp.point.y << " " << curr_wp.point.z); - prev_wps.push_back(curr_wp); - backwards_global_plan.push_back(waypoint); - } - else if(prev_wps.size() == 1){ - geometry_msgs::PointStamped prev_wp = prev_wps[prev_wps.size()-1]; - float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + - pow(curr_wp.point.y - prev_wp.point.y, 2)); - if(distance >= waypoint_spacing_threshold){ - //ROS_INFO_STREAM("\tADDING 1 wp: " << curr_wp.point.x << " " << curr_wp.point.y << " " << curr_wp.point.z); - prev_wps.push_back(curr_wp); - backwards_global_plan.push_back(waypoint); - } - } +void LocalPlanner::waypoint_callback(geometry_msgs::PointStamped wp) { + if (goal_mode != AUTO_WAYPOINT) { + waypoint_buffer.clear(); + waypoint_buffer.push_back(wp); + return; } - } - for(int i = backwards_global_plan.size()-1; i >= 0; i--){ - global_plan.waypoints.push_back(backwards_global_plan[i]); - } - /*for(auto it = waypoint_buffer.begin(); it != waypoint_buffer.end(); it++){ - geometry_msgs::PointStamped curr_wp = *it; - core_trajectory_msgs::WaypointXYZVYaw waypoint; - waypoint.position.x = curr_wp.point.x; - waypoint.position.y = curr_wp.point.y; - waypoint.position.z = curr_wp.point.z; - global_plan.waypoints.push_back(waypoint); - } - */ - - // add an extra segment onto the end of the path - if(got_tracking_point){ - try{ - tf::StampedTransform transform; - listener->waitForTransform(tracking_point_odom.header.frame_id, wp.header.frame_id, wp.header.stamp, ros::Duration(0.1)); - listener->lookupTransform(tracking_point_odom.header.frame_id, wp.header.frame_id, wp.header.stamp, transform); - - tf::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.pose.position); - tf::Vector3 wp_position = transform*tflib::to_tf(wp.point); - - tf::Vector3 direction = (wp_position - tp_position).normalized()*3; - tf::Vector3 wp2_position = wp_position + direction; - - // TODO: Are waypoints in the tracking point frame while the previous ones are in wp frame? - core_trajectory_msgs::WaypointXYZVYaw wp1, wp2; - wp1.position.x = wp_position.x(); - wp1.position.y = wp_position.y(); - wp1.position.z = wp_position.z(); - wp2.position.x = wp2_position.x(); - wp2.position.y = wp2_position.y(); - wp2.position.z = wp2_position.z(); - global_plan.waypoints.push_back(wp1); - global_plan.waypoints.push_back(wp2); - } - catch(tf::TransformException& ex){ - ROS_ERROR_STREAM("Transform exception in waypoint_callback: " << ex.what()); + + // TODO: REMOVE THIS + if (wp.point.x == 0 && wp.point.y == 0) return; + + // remove old waypoints if necessary + waypoint_buffer.push_back(wp); + if ((wp.header.stamp - waypoint_buffer.front().header.stamp).toSec() > waypoint_buffer_duration) + waypoint_buffer.pop_front(); + + // stitch together the history of waypoints + airstack_msgs::TrajectoryXYZVYaw global_plan; + global_plan.header.frame_id = wp.header.frame_id; + global_plan.header.stamp = wp.header.stamp; + + std::vector backwards_global_plan; + // double waypoint_spacing_threshold = 0.5; + // double waypoint_angle_threshold = 30.*M_PI/180.; + std::vector prev_wps; + for (auto it = waypoint_buffer.rbegin(); it != waypoint_buffer.rend(); it++) { + geometry_msgs::PointStamped curr_wp = *it; + airstack_msgs::WaypointXYZVYaw waypoint; + waypoint.position.x = curr_wp.point.x; + waypoint.position.y = curr_wp.point.y; + waypoint.position.z = curr_wp.point.z; + + if (prev_wps.size() > 1) { + geometry_msgs::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; + geometry_msgs::PointStamped prev2_wp = prev_wps[prev_wps.size() - 2]; + float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + + pow(curr_wp.point.y - prev_wp.point.y, 2)); + + float a = pow(prev_wp.point.x - curr_wp.point.x, 2) + + pow(prev_wp.point.y - curr_wp.point.y, 2); + float b = pow(prev_wp.point.x - prev2_wp.point.x, 2) + + pow(prev_wp.point.y - prev2_wp.point.y, 2); + float c = pow(prev2_wp.point.x - curr_wp.point.x, 2) + + pow(prev2_wp.point.y - curr_wp.point.y, 2); + float angle = acos((a + b - c) / sqrt(4 * a * b)); + + float angle_diff = fabs(atan2(sin(angle - M_PI), cos(angle - M_PI))); + + // ROS_INFO_STREAM("\tdistance: " << distance << " angle: " << angle*180./M_PI << " + // angle_diff: " << angle_diff*180./M_PI); + + if (distance >= waypoint_spacing_threshold && angle_diff < waypoint_angle_threshold) { + // ROS_INFO_STREAM("\tADDING wp: " << curr_wp.point.x << " " << curr_wp.point.y << " + // " << curr_wp.point.z); + backwards_global_plan.push_back(waypoint); + prev_wps.push_back(curr_wp); + } + } else { + if (prev_wps.size() == 0) { + // ROS_INFO_STREAM("\tADDING 0 wp: " << curr_wp.point.x << " " << curr_wp.point.y << + // " " << curr_wp.point.z); + prev_wps.push_back(curr_wp); + backwards_global_plan.push_back(waypoint); + } else if (prev_wps.size() == 1) { + geometry_msgs::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; + float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + + pow(curr_wp.point.y - prev_wp.point.y, 2)); + if (distance >= waypoint_spacing_threshold) { + // ROS_INFO_STREAM("\tADDING 1 wp: " << curr_wp.point.x << " " << + // curr_wp.point.y << " " << curr_wp.point.z); + prev_wps.push_back(curr_wp); + backwards_global_plan.push_back(waypoint); + } + } + } } - } - - global_plan_trajectory_distance = 0; - this->global_plan = global_plan; - this->got_global_plan = true; - - //ROS_INFO_STREAM("GLOBAL PLAN WAYPOINTS: " << global_plan.waypoints.size()); - - // DEBUG ONLY REMOV THIS - /* - Trajectory gp(global_plan); - visualization_msgs::MarkerArray global_markers = gp.get_markers(0, 0, 1); - global_plan_vis_pub.publish(global_markers); - */ - - /* - if(got_tracking_point){ - this->got_global_plan = true; - global_plan_trajectory_distance = 0; - - try{ - tf::StampedTransform transform; - listener->waitForTransform(tracking_point_odom.header.frame_id, wp.header.frame_id, wp.header.stamp, ros::Duration(0.1)); - listener->lookupTransform(tracking_point_odom.header.frame_id, wp.header.frame_id, wp.header.stamp, transform); - - tf::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.pose.position); - tf::Vector3 wp_position = transform*tflib::to_tf(wp.point); - - tf::Vector3 direction = (wp_position - tp_position).normalized()*3; - tf::Vector3 wp2_position = wp_position + direction; - - core_trajectory_msgs::TrajectoryXYZVYaw global_plan; - global_plan.header.frame_id = tracking_point_odom.header.frame_id; - global_plan.header.stamp = wp.header.stamp; - - core_trajectory_msgs::WaypointXYZVYaw wp1, wp2; - wp1.position.x = wp_position.x(); - wp1.position.y = wp_position.y(); - wp1.position.z = wp_position.z(); - wp2.position.x = wp2_position.x(); - wp2.position.y = wp2_position.y(); - wp2.position.z = wp2_position.z(); - global_plan.waypoints.push_back(wp1); - global_plan.waypoints.push_back(wp2); - - this->global_plan = global_plan; + for (int i = backwards_global_plan.size() - 1; i >= 0; i--) { + global_plan.waypoints.push_back(backwards_global_plan[i]); } - catch(tf::TransformException& ex){ - ROS_ERROR_STREAM("Transform exception in waypoint_callback: " << ex.what()); + // add an extra segment onto the end of the path + if (got_tracking_point) { + try { + tf::StampedTransform transform; + listener->waitForTransform(tracking_point_odom.header.frame_id, wp.header.frame_id, + wp.header.stamp, ros::Duration(0.1)); + listener->lookupTransform(tracking_point_odom.header.frame_id, wp.header.frame_id, + wp.header.stamp, transform); + + tf::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.pose.position); + tf::Vector3 wp_position = transform * tflib::to_tf(wp.point); + + tf::Vector3 direction = (wp_position - tp_position).normalized() * 3; + tf::Vector3 wp2_position = wp_position + direction; + + // TODO: Are waypoints in the tracking point frame while the previous ones are in wp + // frame? + airstack_msgs::WaypointXYZVYaw wp1, wp2; + wp1.position.x = wp_position.x(); + wp1.position.y = wp_position.y(); + wp1.position.z = wp_position.z(); + wp2.position.x = wp2_position.x(); + wp2.position.y = wp2_position.y(); + wp2.position.z = wp2_position.z(); + global_plan.waypoints.push_back(wp1); + global_plan.waypoints.push_back(wp2); + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("Transform exception in waypoint_callback: " << ex.what()); + } } - } - */ -} -void LocalPlanner::custom_waypoint_callback(geometry_msgs::PoseStamped wp){ - if(!got_look_ahead) - return; - - try{ - tf::StampedTransform transform; - listener->waitForTransform(look_ahead_odom.header.frame_id, wp.header.frame_id, wp.header.stamp, ros::Duration(0.1)); - listener->lookupTransform(look_ahead_odom.header.frame_id, wp.header.frame_id, wp.header.stamp, transform); - - tf::Vector3 la_position = tflib::to_tf(look_ahead_odom.pose.pose.position); - tf::Vector3 wp_position = transform*tflib::to_tf(wp.pose.position); - - core_trajectory_msgs::TrajectoryXYZVYaw global_plan; - global_plan.header.frame_id = look_ahead_odom.header.frame_id; - global_plan.header.stamp = ros::Time::now(); - - core_trajectory_msgs::WaypointXYZVYaw wp1, wp2; - wp1.position.x = la_position.x();//la_position.x(); - wp1.position.y = la_position.y();//la_position.y(); - wp1.position.z = la_position.z();//la_position.z(); - wp2.position.x = wp_position.x(); - wp2.position.y = wp_position.y(); - wp2.position.z = wp_position.z(); - global_plan.waypoints.push_back(wp1); - global_plan.waypoints.push_back(wp2); - global_plan_trajectory_distance = 0; this->global_plan = global_plan; this->got_global_plan = true; - - goal_mode = CUSTOM_WAYPOINT; - } - catch(tf::TransformException& ex){ - ROS_ERROR_STREAM("TransformException in custom_waypoint_callback: " << ex.what()); - } } -void LocalPlanner::update_waypoint_mode(){ - if(goal_mode == CUSTOM_WAYPOINT){ - if(global_plan.waypoints.size() < 2) - goal_mode = AUTO_WAYPOINT; - - // check if the time limit for reaching the waypoint has elapsed - double elapsed_time = (ros::Time::now() - global_plan.header.stamp).toSec(); - double distance = 0; - for(int i = 1; i < global_plan.waypoints.size(); i++){ - core_trajectory_msgs::WaypointXYZVYaw prev_wp, curr_wp; - prev_wp = global_plan.waypoints[i-1]; - curr_wp = global_plan.waypoints[i]; - - distance += sqrt(pow(prev_wp.position.x - curr_wp.position.x, 2) + - pow(prev_wp.position.y - curr_wp.position.y, 2)); - } - - //ROS_INFO_STREAM("elapsed: " << elapsed_time << " / " << distance/custom_waypoint_timeout_factor << " distance: " << distance); - if(elapsed_time >= distance/custom_waypoint_timeout_factor){ - //ROS_INFO_STREAM("CUSTOM WAYPOINT TIMEOUT REACHED"); - goal_mode = AUTO_WAYPOINT; +void LocalPlanner::custom_waypoint_callback(geometry_msgs::PoseStamped wp) { + if (!got_look_ahead) return; + + try { + tf::StampedTransform transform; + listener->waitForTransform(look_ahead_odom.header.frame_id, wp.header.frame_id, + wp.header.stamp, ros::Duration(0.1)); + listener->lookupTransform(look_ahead_odom.header.frame_id, wp.header.frame_id, + wp.header.stamp, transform); + + tf::Vector3 la_position = tflib::to_tf(look_ahead_odom.pose.pose.position); + tf::Vector3 wp_position = transform * tflib::to_tf(wp.pose.position); + + airstack_msgs::TrajectoryXYZVYaw global_plan; + global_plan.header.frame_id = look_ahead_odom.header.frame_id; + global_plan.header.stamp = ros::Time::now(); + + airstack_msgs::WaypointXYZVYaw wp1, wp2; + wp1.position.x = la_position.x(); // la_position.x(); + wp1.position.y = la_position.y(); // la_position.y(); + wp1.position.z = la_position.z(); // la_position.z(); + wp2.position.x = wp_position.x(); + wp2.position.y = wp_position.y(); + wp2.position.z = wp_position.z(); + global_plan.waypoints.push_back(wp1); + global_plan.waypoints.push_back(wp2); + + global_plan_trajectory_distance = 0; + this->global_plan = global_plan; + this->got_global_plan = true; + + goal_mode = CUSTOM_WAYPOINT; + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("TransformException in custom_waypoint_callback: " << ex.what()); } +} - // check if we are close enough to the waypoint - if(got_tracking_point){ - try{ - tf::StampedTransform transform; - listener->waitForTransform(tracking_point_odom.header.frame_id, global_plan.header.frame_id, - global_plan.header.stamp,ros::Duration(0.1)); - listener->lookupTransform(tracking_point_odom.header.frame_id, global_plan.header.frame_id, - global_plan.header.stamp, transform); - - tf::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.pose.position); - tp_position.setZ(0); - tf::Vector3 wp_position = transform*tflib::to_tf(global_plan.waypoints.back().position); - wp_position.setZ(0); - - if(tp_position.distance(wp_position) < custom_waypoint_distance_threshold){ - //ROS_INFO_STREAM("CUSTOM WAYPOINT DISTANCE THRESHOLD MET"); - goal_mode = AUTO_WAYPOINT; - } - } - catch(tf::TransformException& ex){ - ROS_ERROR_STREAM("TransformException in update_waypoint_mode: " << ex.what()); - } +void LocalPlanner::update_waypoint_mode() { + if (goal_mode == CUSTOM_WAYPOINT) { + if (global_plan.waypoints.size() < 2) goal_mode = AUTO_WAYPOINT; + + // check if the time limit for reaching the waypoint has elapsed + double elapsed_time = (ros::Time::now() - global_plan.header.stamp).toSec(); + double distance = 0; + for (int i = 1; i < global_plan.waypoints.size(); i++) { + airstack_msgs::WaypointXYZVYaw prev_wp, curr_wp; + prev_wp = global_plan.waypoints[i - 1]; + curr_wp = global_plan.waypoints[i]; + + distance += sqrt(pow(prev_wp.position.x - curr_wp.position.x, 2) + + pow(prev_wp.position.y - curr_wp.position.y, 2)); + } + + // ROS_INFO_STREAM("elapsed: " << elapsed_time << " / " << + // distance/custom_waypoint_timeout_factor << " distance: " << distance); + if (elapsed_time >= distance / custom_waypoint_timeout_factor) { + // ROS_INFO_STREAM("CUSTOM WAYPOINT TIMEOUT REACHED"); + goal_mode = AUTO_WAYPOINT; + } + + // check if we are close enough to the waypoint + if (got_tracking_point) { + try { + tf::StampedTransform transform; + listener->waitForTransform(tracking_point_odom.header.frame_id, + global_plan.header.frame_id, global_plan.header.stamp, + ros::Duration(0.1)); + listener->lookupTransform(tracking_point_odom.header.frame_id, + global_plan.header.frame_id, global_plan.header.stamp, + transform); + + tf::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.pose.position); + tp_position.setZ(0); + tf::Vector3 wp_position = + transform * tflib::to_tf(global_plan.waypoints.back().position); + wp_position.setZ(0); + + if (tp_position.distance(wp_position) < custom_waypoint_distance_threshold) { + // ROS_INFO_STREAM("CUSTOM WAYPOINT DISTANCE THRESHOLD MET"); + goal_mode = AUTO_WAYPOINT; + } + } catch (tf::TransformException& ex) { + ROS_ERROR_STREAM("TransformException in update_waypoint_mode: " << ex.what()); + } + } } - } } -void LocalPlanner::look_ahead_callback(nav_msgs::Odometry odom){ - got_look_ahead = true; - look_ahead_odom = odom; +void LocalPlanner::look_ahead_callback(nav_msgs::Odometry odom) { + got_look_ahead = true; + look_ahead_odom = odom; } -void LocalPlanner::tracking_point_callback(nav_msgs::Odometry odom){ - got_tracking_point = true; - tracking_point_odom = odom; +void LocalPlanner::tracking_point_callback(nav_msgs::Odometry odom) { + got_tracking_point = true; + tracking_point_odom = odom; } -void LocalPlanner::range_up_callback(sensor_msgs::Range range){ - got_range_up = true; - range_up = range; +void LocalPlanner::range_up_callback(sensor_msgs::Range range) { + got_range_up = true; + range_up = range; } -void LocalPlanner::range_down_callback(sensor_msgs::Range range){ - got_range_down = true; - range_down = range; +void LocalPlanner::range_down_callback(sensor_msgs::Range range) { + got_range_down = true; + range_down = range; } -LocalPlanner::~LocalPlanner(){ -} +LocalPlanner::~LocalPlanner() {} -BaseNode* BaseNode::get(){ - LocalPlanner* local_planner = new LocalPlanner("LocalPlanner"); - return local_planner; +BaseNode* BaseNode::get() { + LocalPlanner* local_planner = new LocalPlanner("LocalPlanner"); + return local_planner; } diff --git a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/CMakeLists.txt new file mode 100644 index 000000000..f1d6d1747 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.5) +project(core_map_representation_interface) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) + +# Include directories +include_directories( + include + ${ament_INCLUDE_DIRS} +) + +add_library(core_map_representation_interface INTERFACE) + +ament_export_targets(core_map_representation_interfaceTargets HAS_LIBRARY_TARGET) +#ament_export_include_directories(include) +ament_export_dependencies( + airstack_msgs + geometry_msgs + nav_msgs + pluginlib + rclcpp + rclpy + sensor_msgs + std_msgs + tf2 + tf2_ros + visualization_msgs +) +install( + TARGETS core_map_representation_interface + EXPORT core_map_representation_interfaceTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) +# Install include files +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +# Install Python scripts (if you have any)### +# install(PROGRAMS +# scripts/your_python_script.py +# DESTINATION lib/${PROJECT_NAME} +# ) + + +# Export dependencies + + +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/include/core_map_representation_interface/map_representation.h b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/include/core_map_representation_interface/map_representation.h new file mode 100644 index 000000000..f9b894cb5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/include/core_map_representation_interface/map_representation.h @@ -0,0 +1,44 @@ +#ifndef _CORE_MAP_REPRESENTATION_H_ +#define _CORE_MAP_REPRESENTATION_H_ + +#include +#include +#include +#include + +class MapRepresentation : public rclcpp::Node { + private: + public: + /** + Takes in a list of trajectories and outputs a value for each waypoint in each trajectory. + @return A vector of vectors containing values for each waypoint. There is a vector of vectors + for each trajectory. There is a vector of doubles for each waypoint within a trajectory. + */ + + virtual std::vector > get_values( + std::vector > + trajectories) { // std::vector trajectories){ + RCLCPP_ERROR(this->get_logger(), "get_values CALLED BUT NOT IMPLEMENTED"); + + std::vector > values; + return values; + } + + /** + Clears the map. + */ + virtual void clear() { RCLCPP_ERROR(this->get_logger(), "clear CALLED BUT NOT IMPLEMENTED"); } + + /** + Use this function to publish visualizations of the map that might be helpful for debugging. + */ + virtual void publish_debug() {} + + virtual ~MapRepresentation() {} + + protected: + // MapRepresentation() {} + MapRepresentation() : Node("map_representation") {} +}; + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/package.xml b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/package.xml new file mode 100644 index 000000000..52b15ab66 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_map_representation_interface/package.xml @@ -0,0 +1,31 @@ + + + + core_map_representation_interface + 0.0.0 + The core_map_representation_interface package + + john + TODO + + ament_cmake + + geometry_msgs + nav_msgs + rclcpp + rclpy + sensor_msgs + std_msgs + tf2 + tf2_ros + visualization_msgs + pluginlib + airstack_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/CMakeLists.txt new file mode 100755 index 000000000..3c5b4c404 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.8) +project(trajectory_controller) +set (CMAKE_INCLUDE_CURRENT_DIR ON) +# Use C++14 standard +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_library REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(tflib REQUIRED) +find_package(std_srvs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Include directories +include_directories( + ${rclcpp_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${std_srvs_INCLUDE_DIRS} + ${airstack_msgs_INCLUDE_DIRS} # Add this line + ${rosidl_default_runtime_INCLUDE_DIRS} +) + +# Declare a C++ executable +add_executable(trajectory_controller src/trajectory_controller.cpp) + +ament_target_dependencies(trajectory_controller + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + trajectory_library + airstack_msgs + tflib + rosidl_default_generators + std_srvs +) +target_link_libraries(trajectory_controller + tf2::tf2 + tf2_ros::tf2_ros +) + +# Install targets +install(TARGETS trajectory_controller + DESTINATION lib/${PROJECT_NAME} +) +install(FILES package.xml + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.hpp" +) + +# Install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/COLCON_IGNORE b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/launch/trajectory_controller.launch b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/launch/trajectory_controller.launch new file mode 100644 index 000000000..4bdf86d31 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/launch/trajectory_controller.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/launch/trajectory_controller_bag.launch b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/launch/trajectory_controller_bag.launch new file mode 100644 index 000000000..d7493916f --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/launch/trajectory_controller_bag.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/package.xml b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/package.xml new file mode 100755 index 000000000..b1ec3eeb9 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/package.xml @@ -0,0 +1,46 @@ + + + + + trajectory_controller + 0.0.0 + The trajectory_controller package + + john + TODO + + + ament_cmake + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + trajectory_library + tflib + std_srvs + airstack_msgs + rosidl_default_generators + + + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + trajectory_library + tflib + std_srvs + airstack_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/src/tracking_point.py b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/src/tracking_point.py new file mode 100755 index 000000000..e8a57f8e6 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/src/tracking_point.py @@ -0,0 +1,29 @@ +#!/usr/bin/python +import rospy +from nav_msgs.msg import Odometry +import numpy as np +from tf import transformations as trans + +if __name__ == '__main__': + rospy.init_node('tracking_point', anonymous=True) + + odom_pub = rospy.Publisher('/tracking_point', Odometry, queue_size=10) + odom = Odometry() + odom.header.frame_id = 'map' + odom.child_frame_id = 'base_link' + odom.pose.pose.position.x = 16. + odom.pose.pose.position.y = 5. + odom.pose.pose.position.z = 3. + + q = trans.quaternion_from_euler(0., 0., np.pi/2.) + + odom.pose.pose.orientation.x = q[0] + odom.pose.pose.orientation.y = q[1] + odom.pose.pose.orientation.z = q[2] + odom.pose.pose.orientation.w = q[3] + + rate = rospy.Rate(50.) + + while not rospy.is_shutdown(): + odom_pub.publish(odom) + rate.sleep() diff --git a/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/src/trajectory_controller.cpp b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/src/trajectory_controller.cpp new file mode 100644 index 000000000..cc1c76f45 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/core_trajectory_controller/src/trajectory_controller.cpp @@ -0,0 +1,467 @@ +/*#include +#include +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +*/ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//=================================================================================== +//----------------------------- Trajectory Control Node ----------------------------- +//=================================================================================== + +class TrajectoryControlNode : public rclcpp::Node { + private: + // ros::Subscriber traj_sub, traj_track_sub, odom_sub; + + // ros::Publisher marker_vis_pub, segment_marker_vis_pub, tracking_point_pub, look_ahead_pub, + // trajectory_completion_percentage_pub, trajectory_time_pub, segment_pub, + // tracking_error_pub, velocity_pub; + + // SUBSCRIBERS + rclcpp::Subscription::SharedPtr traj_sub; + rclcpp::Subscription::SharedPtr traj_track_sub; + rclcpp::Subscription::SharedPtr odom_sub; + + // PUBLISHERS + rclcpp::Publisher::SharedPtr marker_vis_pub; + rclcpp::Publisher::SharedPtr segment_marker_vis_pub; + rclcpp::Publisher::SharedPtr tracking_point_pub; + rclcpp::Publisher::SharedPtr look_ahead_pub; + rclcpp::Publisher::SharedPtr trajectory_completion_percentage_pub; + rclcpp::Publisher::SharedPtr trajectory_time_pub; + rclcpp::Publisher::SharedPtr tracking_error_pub; + rclcpp::Publisher::SharedPtr velocity_pub; + rclcpp::Publisher::SharedPtr segment_pub; + + // tf::TransformBroadcaster* broadcaster; + // tf::TransformListener* listener; + + // LISTENERS AND BROADCATERS + std::shared_ptr tf_buffer; + std::shared_ptr tf_listener; + std::unique_ptr tf_broadcaster; + + // ros::ServiceServer traj_style_srv, traj_mode_srv; + rclcpp::Service::SharedPtr traj_style_srv; + rclcpp::Service::SharedPtr traj_mode_srv; + + nav_msgs::msg::Odometry odom; + bool got_odom; + + std::string tf_prefix; + std::string target_frame; + int trajectory_mode; + Trajectory* trajectory; + rclcpp::Time start_time; + double prev_time; + nav_msgs::msg::Odometry tracking_point, look_ahead_point; + double tracking_point_distance_limit; + double velocity_look_ahead_time; + + public: + // TrajectoryControlNode(); + + explicit TrajectoryControlNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + // virtual bool initialize(); + virtual bool execute(); + virtual ~TrajectoryControlNode(); + + void traj_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::ConstSharedPtr msg); + void traj_track_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::ConstSharedPtr msg); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + // bool set_trajectory_style_service(std_srvs::SetBool::Request& req, + // std_srvs::SetBool::Response& res); + // bool set_trajectory_mode(trajectory_controller::TrajectoryMode::Request& req, + // trajectory_controller::TrajectoryMode::Response& res); + + bool set_trajectory_mode( + const std::shared_ptr request, + std::shared_ptr response); + void set_trajectory_style_service( + const std::shared_ptr request, + std::shared_ptr response); + + std::string mode; + float velocity_target; +}; + +TrajectoryControlNode::TrajectoryControlNode(const rclcpp::NodeOptions& options) + : Node("trajectory_control_node", options), trajectory(new Trajectory(target_frame)) { + // ros::NodeHandle* nh = get_node_handle(); + // ros::NodeHandle* pnh = get_private_node_handle(); + + // init params + // tf_prefix = pnh->param("tf_prefix", std::string("")); + // target_frame = pnh->param("target_frame", std::string("world")); + // tracking_point_distance_limit = pnh->param("tracking_point_distance_limit", 0.5); + // velocity_look_ahead_time = pnh->param("velocity_look_ahead_time", 0.0); + // got_odom = false; + tf_prefix = this->declare_parameter("tf_prefix", std::string("")); + target_frame = this->declare_parameter("target_frame", std::string("world")); + tracking_point_distance_limit = this->declare_parameter("tracking_point_distance_limit", 0.5); + velocity_look_ahead_time = this->declare_parameter("velocity_look_ahead_time", 0.0); + got_odom = false; + + trajectory_mode = airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE; // TRACK; + trajectory = new Trajectory(target_frame); + // tf_buffer = std::make_shared(this->get_clock()); + // tf_listener = std::make_shared(*tf_buffer); + + // Initialize tf_broadcaster + // tf_broadcaster = std::make_unique(this); + + // init subscribers + /* + traj_sub = nh->subscribe("trajectory", 10, &TrajectoryControlNode::traj_callback, this); + traj_track_sub = + nh->subscribe("trajectory_track", 10, &TrajectoryControlNode::traj_track_callback, this); + odom_sub = nh->subscribe("odometry", 10, &TrajectoryControlNode::odom_callback, this); + */ + traj_sub = this->create_subscription( + "trajectory", 10, + std::bind(&TrajectoryControlNode::traj_callback, this, std::placeholders::_1)); + traj_track_sub = this->create_subscription( + "trajectory_track", 10, + std::bind(&TrajectoryControlNode::traj_track_callback, this, std::placeholders::_1)); + odom_sub = this->create_subscription( + "odometry", 10, + std::bind(&TrajectoryControlNode::odom_callback, this, std::placeholders::_1)); + + // init publishers + /* + segment_pub = nh->advertise("trajectory_segment", 10); + marker_vis_pub = nh->advertise("trajectory_vis", 10); + segment_marker_vis_pub = + nh->advertise("trajectory_segment_vis", 10); + tracking_point_pub = nh->advertise("tracking_point", 10); + look_ahead_pub = nh->advertise("look_ahead", 10); + trajectory_completion_percentage_pub = + nh->advertise("trajectory_completion_percentage", 10); + trajectory_time_pub = nh->advertise("trajectory_time", 10); + tracking_error_pub = nh->advertise("tracking_error", 10); + velocity_pub = nh->advertise("tracking_point_velocity_magnitude", 1); + broadcaster = new tf::TransformBroadcaster(); + listener = new tf::TransformListener(); + */ + segment_pub = + this->create_publisher("trajectory_segment", 10); + marker_vis_pub = + this->create_publisher("trajectory_vis", 10); + segment_marker_vis_pub = + this->create_publisher("trajectory_segment_vis", 10); + tracking_point_pub = this->create_publisher("tracking_point", 10); + look_ahead_pub = this->create_publisher("look_ahead", 10); + trajectory_completion_percentage_pub = + this->create_publisher("trajectory_completion_percentage", 10); + trajectory_time_pub = this->create_publisher("trajectory_time", 10); + tracking_error_pub = this->create_publisher("tracking_error", 10); + velocity_pub = + this->create_publisher("tracking_point_velocity_magnitude", 1); + + auto clock = std::make_shared(RCL_ROS_TIME); + auto node = std::make_shared("node_name"); + + // Create tf2_ros::Buffer with required arguments + tf_buffer = std::make_shared(clock); + // tf_buffer->setCacheTime(std::chrono::seconds(10)); + + // Create tf2_ros::TransformListener with the tf2_ros::Buffer + tf_listener = std::make_shared(*tf_buffer); + + // Create tf2_ros::TransformBroadcaster + tf_broadcaster = std::make_unique(node); + // Init services + traj_mode_srv = this->create_service( + "set_trajectory_mode", std::bind(&TrajectoryControlNode::set_trajectory_mode, this, + std::placeholders::_1, std::placeholders::_2)); + traj_style_srv = this->create_service( + "set_trajectory_style", std::bind(&TrajectoryControlNode::set_trajectory_style_service, + this, std::placeholders::_1, std::placeholders::_2)); + + // init services + // traj_mode_srv = nh->advertiseService("set_trajectory_mode", + // &TrajectoryControlNode::set_trajectory_mode, this); + + prev_time = 0; + + // return true; +} + +bool TrajectoryControlNode::execute() { + if (got_odom) { + // publish trajectory visualization + marker_vis_pub->publish(trajectory->get_markers(1, 1, 0)); + + // figure out what duration into the trajectory we are + // ros::Time now = ros::Time::now(); + auto now = this->now(); + double current_time = (now - start_time).seconds(); + double time_past_end = current_time - trajectory->get_duration(); + + double tracking_error = tflib::to_tf(tracking_point.pose.pose.position) + .distance(tflib::to_tf(odom.pose.pose.position)); + std_msgs::msg::Float32 tracking_error_msg; + tracking_error_msg.data = tracking_error; + tracking_error_pub->publish(tracking_error_msg); + + if (time_past_end >= 0 && + trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + std::chrono::duration duration_past_end(time_past_end); + start_time += rclcpp::Duration(duration_past_end); + current_time = trajectory->get_duration(); + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE || + tracking_error >= tracking_point_distance_limit) { + double elapsed_time = current_time - prev_time; + std::chrono::duration duration_elapsed(elapsed_time); + start_time += rclcpp::Duration(duration_elapsed); + current_time = (now - start_time).seconds(); + } + // TODO: zero tracking point velocity in rewind and pause modes + else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND && + current_time > 0) { + // start_time += rclcpp::Duration(2.0 * (current_time - prev_time)); + // current_time = (now - start_time).seconds(); + double duration_seconds = 2.0 * (current_time - prev_time); + rclcpp::Duration duration{std::chrono::duration(duration_seconds)}; + + start_time = start_time + duration; + + current_time = (now - start_time).seconds(); + } + prev_time = current_time; + + // get tracking point and look ahead point. If they aren't valid use the drone's current + // pose + // monitor.tic("get_odom"); + bool valid = trajectory->get_odom(current_time, &tracking_point); + // set the velocity of the tracking point based on the velocity from a different time + if (valid && velocity_look_ahead_time != 0) { + nav_msgs::msg::Odometry velocity_look_ahead_point; + bool vel_valid = trajectory->get_odom(current_time + velocity_look_ahead_time, + &velocity_look_ahead_point); + if (vel_valid) { + tracking_point.twist = velocity_look_ahead_point.twist; + } + } + + // double elapsed = monitor.toc("get_odom"); + // ROS_INFO_STREAM("get_odom elapsed: " << elapsed); + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE) { + // if(!valid){ + tracking_point = odom; + tracking_point.twist.twist.linear.x = 0; + tracking_point.twist.twist.linear.y = 0; + tracking_point.twist.twist.linear.z = 0; + tracking_point.twist.twist.angular.x = 0; + tracking_point.twist.twist.angular.y = 0; + tracking_point.twist.twist.angular.z = 0; + look_ahead_point = tracking_point; + } + if (valid) { + trajectory->get_odom(current_time + 1.0, &look_ahead_point); + + tf2::Vector3 tp = tflib::to_tf(tracking_point.pose.pose.position); + double start; + bool success = trajectory->get_trajectory_distance_at_closest_point(tp, &start); + if (success) { + Trajectory sub_trajectory = + trajectory->get_subtrajectory_distance(start, start + 1000.); + segment_marker_vis_pub->publish(sub_trajectory.get_markers(0, 1, 1)); + airstack_msgs::msg::TrajectoryXYZVYaw segment_msg = + sub_trajectory.get_TrajectoryXYZVYaw(); + segment_msg.header.stamp = tracking_point.header.stamp; + segment_pub->publish(segment_msg); + } + } + + tracking_point.header.stamp = now; + look_ahead_point.header.stamp = now; + + // TODO: decide whether or not this is a good idea + // When the tracking point reaches the end of the trajectory, the velocity gets set to zero + if (time_past_end >= 0 || + trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE || + trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + tracking_point.twist.twist.linear.x = 0; + tracking_point.twist.twist.linear.y = 0; + tracking_point.twist.twist.linear.z = 0; + tracking_point.twist.twist.angular.x = 0; + tracking_point.twist.twist.angular.y = 0; + tracking_point.twist.twist.angular.z = 0; + + look_ahead_point.twist.twist.linear.x = 0; + look_ahead_point.twist.twist.linear.y = 0; + look_ahead_point.twist.twist.linear.z = 0; + look_ahead_point.twist.twist.angular.x = 0; + look_ahead_point.twist.twist.angular.y = 0; + look_ahead_point.twist.twist.angular.z = 0; + } + + std_msgs::msg::Float32 velocity_msg; + velocity_msg.data = + sqrt(tracking_point.twist.twist.linear.x * tracking_point.twist.twist.linear.x + + tracking_point.twist.twist.linear.y * tracking_point.twist.twist.linear.y + + tracking_point.twist.twist.linear.z * tracking_point.twist.twist.linear.z); + velocity_pub->publish(velocity_msg); + tracking_point_pub->publish(tracking_point); + look_ahead_pub->publish(look_ahead_point); + + // create a tf for the tracking point odom + /* + tf::StampedTransform transform = + tflib::to_tf(tracking_point, tf_prefix + "/tracking_point"); + tf::StampedTransform transform_stabilized = tflib::get_stabilized(transform); + transform_stabilized.child_frame_id_ = tf_prefix + "/tracking_point_stabilized"; + tf_broadcaster->sendTransform(transform); + tf_broadcaster->sendTransform(transform_stabilized); + + // create a tf for the look ahead odom + tf::StampedTransform look_ahead_transform = + tflib::to_tf(look_ahead_point, tf_prefix + "/look_ahead_point"); + tf::StampedTransform look_ahead_transform_stabilized = + tflib::get_stabilized(look_ahead_transform); + look_ahead_transform_stabilized.child_frame_id_ = + tf_prefix + "/look_ahead_point_stabilized"; + + tf_broadcaster->sendTransform(look_ahead_transform); + tf_broadcaster->sendTransform(look_ahead_transform_stabilized); + */ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = now; + transform.header.frame_id = tf_prefix + "/base_link"; // Base frame_id + transform.child_frame_id = tf_prefix + "/tracking_point"; + transform.transform.translation.x = tracking_point.pose.pose.position.x; + transform.transform.translation.y = tracking_point.pose.pose.position.y; + transform.transform.translation.z = tracking_point.pose.pose.position.z; + transform.transform.rotation = tracking_point.pose.pose.orientation; + + auto transform_stabilized = transform; // Apply any stabilization logic here + transform_stabilized.child_frame_id = tf_prefix + "/tracking_point_stabilized"; + + tf_broadcaster->sendTransform(transform); + tf_broadcaster->sendTransform(transform_stabilized); + + // Create a transform for the look ahead odom + geometry_msgs::msg::TransformStamped look_ahead_transform; + look_ahead_transform.header.stamp = now; + look_ahead_transform.header.frame_id = tf_prefix + "/base_link"; // Base frame_id + look_ahead_transform.child_frame_id = tf_prefix + "/look_ahead_point"; + look_ahead_transform.transform.translation.x = look_ahead_point.pose.pose.position.x; + look_ahead_transform.transform.translation.y = look_ahead_point.pose.pose.position.y; + look_ahead_transform.transform.translation.z = look_ahead_point.pose.pose.position.z; + look_ahead_transform.transform.rotation = look_ahead_point.pose.pose.orientation; + + auto look_ahead_transform_stabilized = + look_ahead_transform; // Apply any stabilization logic here + look_ahead_transform_stabilized.child_frame_id = tf_prefix + "/look_ahead_point_stabilized"; + tf_broadcaster->sendTransform(look_ahead_transform); + tf_broadcaster->sendTransform(look_ahead_transform_stabilized); + + // publish completion percentage + std_msgs::msg::Float32 trajectory_completion_percentage; + trajectory_completion_percentage.data = current_time / trajectory->get_duration() * 100.f; + trajectory_completion_percentage_pub->publish(trajectory_completion_percentage); + + // publish current trajectory time + std_msgs::msg::Float32 trajectory_time; + trajectory_time.data = current_time; + trajectory_time_pub->publish(trajectory_time); + } + + return true; +} + +bool TrajectoryControlNode::set_trajectory_mode( + std::shared_ptr request, + std::shared_ptr response) { + int prev_trajectory_mode = trajectory_mode; + trajectory_mode = request->mode; + + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE) { + } + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE) { + trajectory->clear(); + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::TRACK) { + trajectory->clear(); + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::SEGMENT) { + if (prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::PAUSE && + prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::REWIND && + prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::SEGMENT) { + trajectory->clear(); + start_time = this->now(); + } + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + } + + response->success = true; + return true; +} + +void TrajectoryControlNode::traj_callback( + airstack_msgs::msg::TrajectoryXYZVYaw::ConstSharedPtr traj) { + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::SEGMENT) + trajectory->merge(Trajectory(*traj)); +} + +void TrajectoryControlNode::traj_track_callback( + airstack_msgs::msg::TrajectoryXYZVYaw::ConstSharedPtr traj) { + start_time = this->now(); + trajectory->clear(); + trajectory->merge(Trajectory(*traj)); +} + +void TrajectoryControlNode::odom_callback(nav_msgs::msg::Odometry::ConstSharedPtr odom) { + // this->odom = odom; + tf2_ros::Buffer tf_buffer(this->get_clock()); // Initialize with a clock + tf2_ros::TransformListener tf_listener(tf_buffer); + + if (tflib::transform_odometry(&tf_buffer, *odom, target_frame, target_frame, &(this->odom))) + got_odom = true; +} + +TrajectoryControlNode::~TrajectoryControlNode() {} + +// BaseNode* BaseNode::get() { +// TrajectoryControlNode* trajectory_control_node = new TrajectoryControlNode(); +// return trajectory_control_node; +// } +int main(int argc, char* argv[]) { + // Initialize the ROS 2 client library + rclcpp::init(argc, argv); + + // Create an instance of the TrajectoryControlNode + auto node = std::make_shared(rclcpp::NodeOptions()); + + // Spin the node to process callbacks + rclcpp::spin(node); + + // Clean up and shut down ROS 2 + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/CMakeLists.txt index b02c766ef..4e499df57 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/CMakeLists.txt @@ -1,189 +1,285 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(disparity_expansion) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cv_bridge - image_geometry - image_transport - roscpp - std_msgs - tf -) +# # Find catkin macros and libraries +# # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +# # is used, also find other catkin packages +# find_package(catkin REQUIRED COMPONENTS +# cv_bridge +# image_geometry +# image_transport +# roscpp +# std_msgs +# tf +# ) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) + +find_package(stereo_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +find_package(message_filters REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) # CONFIGURE OPENCV find_package(OpenCV REQUIRED) # CONFIGURE PCL -find_package(PCL 1.7 REQUIRED) +find_package(PCL 1.12 REQUIRED) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +# # System dependencies are found with CMake's conventions +message(STATUS "PCL_INCLUDE_DIRS: ${PCL_INCLUDE_DIRS}") -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# # Uncomment this if the package has a setup.py. This macro ensures +# # modules and global scripts declared therein get installed +# # See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder +# ############################################### +# # Declare ROS messages, services and actions ## +# ############################################### + +# # To declare and build messages, services or actions from within this +# # package, follow these steps: +# # * Let MSG_DEP_SET be the set of packages whose message types you use in +# # your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +# # * In the file package.xml: +# # * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +# # * If MSG_DEP_SET isn't empty the following dependencies might have been +# # pulled in transitively but can be declared for certainty nonetheless: +# # * add a build_depend tag for "message_generation" +# # * add a run_depend tag for "message_runtime" +# # * In this file (CMakeLists.txt): +# # * add "message_generation" and every package in MSG_DEP_SET to +# # find_package(catkin REQUIRED COMPONENTS ...) +# # * add "message_runtime" and every package in MSG_DEP_SET to +# # catkin_package(CATKIN_DEPENDS ...) +# # * uncomment the add_*_files sections below as needed +# # and list every .msg/.srv/.action file to be processed +# # * uncomment the generate_messages entry below +# # * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +# # Generate messages in the 'msg' folder # add_message_files( -# FILES -# Message1.msg -# Message2.msg +# FILES +# Message1.msg +# Message2.msg # ) -## Generate services in the 'srv' folder +# # Generate services in the 'srv' folder # add_service_files( -# FILES -# Service1.srv -# Service2.srv +# FILES +# Service1.srv +# Service2.srv # ) -## Generate actions in the 'action' folder +# # Generate actions in the 'action' folder # add_action_files( -# FILES -# Action1.action -# Action2.action +# FILES +# Action1.action +# Action2.action # ) -## Generate added messages and services with any dependencies listed here +# # Generate added messages and services with any dependencies listed here # generate_messages( -# DEPENDENCIES -# std_msgs +# DEPENDENCIES +# std_msgs # ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS +# ################################## +# # catkin specific configuration ## +# ################################## +# # The catkin_package macro generates cmake config files for your package +# # Declare things to be passed to dependent projects +# # INCLUDE_DIRS: uncomment this if you package contains header files +# # LIBRARIES: libraries you create in this project that dependent projects also need +# # CATKIN_DEPENDS: catkin_packages dependent projects also need +# # DEPENDS: system dependencies of this project that dependent projects also need +# catkin_package( +# INCLUDE_DIRS # include - ${catkin_INCLUDE_DIRS} +# ${catkin_INCLUDE_DIRS} +# ${OpenCV_INCLUDE_DIRS} +# ${PCL_INCLUDE_DIRS} +include_directories( + ${rclcpp_INCLUDE_DIRS} + ${rcl_interfaces_INCLUDE_DIRS} + ${cv_bridge_INCLUDE_DIRS} + ${image_geometry_INCLUDE_DIRS} + ${image_transport_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${pcl_ros_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} -# LIBRARIES disparity_expansion -# CATKIN_DEPENDS cv_bridge image_geometry image_transport roscpp std_msgs tf -# DEPENDS system_lib + ${pcl_conversions_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} ) -########### -## Build ## -########### +# LIBRARIES disparity_expansion +# CATKIN_DEPENDS cv_bridge image_geometry image_transport roscpp std_msgs tf +# DEPENDS system_lib +# ) +# ########## +# # Build ## +# ########## set(CMAKE_BUILD_TYPE Release) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# # Specify additional locations of header files +# # Your package locations should be listed before other locations # include_directories(include) include_directories( - ${catkin_INCLUDE_DIRS} + + # ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) -## Declare a cpp library +# # Declare a cpp library # add_library(disparity_expansion -# src/${PROJECT_NAME}/disparity_expansion.cpp +# src/${PROJECT_NAME}/disparity_expansion.cpp # ) -## Declare a cpp executable -add_executable (disparity_expansion src/disparity_expansion.cpp) +# # Declare a cpp executable +add_executable(disparity_expansion src/disparity_expansion.cpp) target_link_libraries(disparity_expansion ${OpenCV_LIBS} ${catkin_LIBRARIES} ) -add_executable (disparity_conv src/disparity_conv.cpp) +ament_target_dependencies(disparity_expansion + rclcpp + rcl_interfaces + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + message_filters + visualization_msgs + stereo_msgs + tf2_geometry_msgs +) +add_executable(disparity_conv src/disparity_conv.cpp) target_link_libraries(disparity_conv ${OpenCV_LIBS} ${catkin_LIBRARIES} ) -add_executable (disparity_pcd src/disparity_pcd.cpp) +ament_target_dependencies(disparity_conv + rclcpp + rcl_interfaces + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + message_filters + tf2_geometry_msgs +) + +add_executable(disparity_pcd src/disparity_pcd.cpp) target_link_libraries(disparity_pcd ${OpenCV_LIBS} ${catkin_LIBRARIES} ) -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes +ament_target_dependencies(disparity_pcd + rclcpp + rcl_interfaces + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + message_filters + tf2_geometry_msgs +) + +# install(TARGETS disparity_expansion +# RUNTIME DESTINATION bin) +install(TARGETS disparity_expansion + DESTINATION lib/${PROJECT_NAME} +) +install(TARGETS disparity_conv + DESTINATION lib/${PROJECT_NAME} +) +install(TARGETS disparity_pcd + DESTINATION lib/${PROJECT_NAME} +) +install(FILES package.xml + DESTINATION share/${PROJECT_NAME} +) + +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +# # Add cmake target dependencies of the executable/library +# # as an example, message headers may need to be generated before nodes # add_dependencies(disparity_expansion_node disparity_expansion_generate_messages_cpp) -## Specify libraries to link a library or executable target against +# # Specify libraries to link a library or executable target against # target_link_libraries(disparity_expansion_node -# ${catkin_LIBRARIES} +# ${catkin_LIBRARIES} # ) +ament_package() -############# -## Install ## -############# +# ############ +# # Install ## +# ############ # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination +# # Mark executable scripts (Python etc.) for installation +# # in contrast to setup.py, you can choose the destination # install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) -## Mark executables and/or libraries for installation +# # Mark executables and/or libraries for installation # install(TARGETS disparity_expansion disparity_expansion_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) -## Mark cpp header files for installation +# # Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE # ) -## Mark other files for installation (e.g. launch and bag files, etc.) +# # Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -############# -## Testing ## -############# +# ############ +# # Testing ## +# ############ -## Add gtest based cpp test target and link libraries +# # Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_disparity_expansion.cpp) # if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() -## Add folders to be run by python nosetests +# # Add folders to be run by python nosetests # catkin_add_nosetests(test) diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/COLCON_IGNORE b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/COLCON_IGNORE deleted file mode 100644 index 8b1378917..000000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/COLCON_IGNORE +++ /dev/null @@ -1 +0,0 @@ - diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/config/disparity_expansion_params.yaml b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/config/disparity_expansion_params.yaml new file mode 100644 index 000000000..a20abd3dd --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/config/disparity_expansion_params.yaml @@ -0,0 +1,13 @@ +disparity_expansion: + ros__parameters: + expansion_cloud_topic: "/robot1/expansion_cloud" + expanded_disparity_fg_topic: "/robot1/expanded_disparity_fg" + expanded_disparity_bg_topic: "/robot1/expanded_disparity_bg" + expansion_poly_topic: "/expansion_poly" + camera_info_topic: "/robot1/perception/camera_info" + disparity_topic: "/disparity" + depth_topic: "/robot1/perception/depth" + scale: 2.0 + downsample_scale: 2.0 + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.launch b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.launch deleted file mode 100644 index 40214a58d..000000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.xml new file mode 100644 index 000000000..056d0d24a --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_expansion.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_gazebo.launch b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_gazebo.launch index 9c1832821..1b408bfda 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_gazebo.launch +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_gazebo.launch @@ -1,34 +1,34 @@ - - - - - - - + + + + + + + - + - + - + - + - + - + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.launch b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.xml similarity index 51% rename from ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.launch rename to ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.xml index f3c6a5bbd..52ab50616 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.launch +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/launch/disparity_pcd.xml @@ -1,18 +1,18 @@ - + - + - + - + - + - + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/package.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/package.xml index f7fc7834e..2da6f7f18 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/package.xml +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/package.xml @@ -1,5 +1,6 @@ - + + disparity_expansion 0.0.0 The disparity_expansion package @@ -39,24 +40,39 @@ - catkin + ament_cmake + rclcpp cv_bridge image_geometry image_transport - roscpp + sensor_msgs std_msgs - tf - cv_bridge - image_geometry - image_transport - roscpp - std_msgs - tf + tf2 + pcl_conversions + visualization_msgs + message_filters + stereo_msgs + OpenCV + PCL + + rclcpp + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + visualization_msgs + message_filters + stereo_msgs + OpenCV + PCL - + ament_cmake - \ No newline at end of file + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_conv.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_conv.cpp index 5a715ad01..218aa7a96 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_conv.cpp +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_conv.cpp @@ -1,76 +1,106 @@ /* -* Copyright (c) 2016 Carnegie Mellon University, Author -* -* For License information please see the LICENSE file in the root directory. -* -*/ + * Copyright (c) 2016 Carnegie Mellon University, Author + * + * For License information please see the LICENSE file in the root directory. + * + */ + +// Postprocessing of disparity image. +/* -//Postprocessing of disparity image. -#include "ros/ros.h" -#include "sensor_msgs/Image.h" -#include "sensor_msgs/CameraInfo.h" -#include "std_msgs/Header.h" #include -#include "opencv2/core/core.hpp" -#include #include -#include #include -#include +#include +#include #include #include -#include -#include +#include #include + +#include +#include +#include + +#include "opencv2/core/core.hpp" +#include "ros/ros.h" +#include "sensor_msgs/CameraInfo.h" +#include "sensor_msgs/Image.h" +#include "std_msgs/Header.h" + +*/ + +#include +#include + +#include +#include +#include + +#include "opencv2/core/core.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/header.hpp" #include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include -class disparity_conv{ -public: - disparity_conv(ros::NodeHandle& nh); - ros::Subscriber cam_info_sub_; - ros::Subscriber disparity_sub_; - ros::Publisher disparity_conv_pub_; - ros::Publisher filtered_image_pub_; +class disparity_conv : public rclcpp::Node { + public: + disparity_conv(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + void callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg_disp, + const sensor_msgs::msg::Image::ConstSharedPtr &msg_image); + + private: + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr disparity_sub_; + rclcpp::Publisher::SharedPtr disparity_conv_pub_; + rclcpp::Publisher::SharedPtr filtered_image_pub_; double baseline; - void stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_disp); - void callback(const sensor_msgs::Image::ConstPtr &msg_disp, const sensor_msgs::Image::ConstPtr &msg_image); + void stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr &msg_disp); + // void callback(const sensor_msgs::msg::Image::SharedPtr msg_disp, + // const sensor_msgs::msg::Image::SharedPtr msg_image); - double fx_,fy_,cx_,cy_; - unsigned int width,height; + double fx_, fy_, cx_, cy_; + unsigned int width, height; cv::Mat prev_disp; bool first; }; -void disparity_conv::stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_disp){ - ROS_INFO_ONCE("Disparity CB"); +// ########################################################################################################### +// ########################################################################################################### +void disparity_conv::stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr &msg_disp) { + RCLCPP_INFO_ONCE(this->get_logger(), "Disparity CB"); - ros::Time start = ros::Time::now(); + rclcpp::Time start = this->get_clock()->now(); cv_bridge::CvImageConstPtr cv_ptrdisparity; - cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); - + // cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); + cv_bridge::CvImagePtr di_msg = std::make_shared(); cv::Mat disparity32F; - disparity32F = cv::Mat::zeros(msg_disp->height,msg_disp->width,CV_32FC1); - try - { + disparity32F = cv::Mat::zeros(msg_disp->height, msg_disp->width, CV_32FC1); + try { cv_ptrdisparity = cv_bridge::toCvShare(msg_disp); - cv_ptrdisparity->image.convertTo(disparity32F,CV_32F); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); + cv_ptrdisparity->image.convertTo(disparity32F, CV_32F); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } - cv::Mat mask = disparity32F == 4095.0; - disparity32F.setTo(NAN,mask); //setting invalid entries tp 0 or NAN - disparity32F = disparity32F/16.0;// actual disparities are obtained here as floats - - + disparity32F.setTo(NAN, mask); // setting invalid entries tp 0 or NAN + disparity32F = disparity32F / 16.0; // actual disparities are obtained here as floats // if(cv_ptrdisparity->image.type()!=CV_32F) // { @@ -93,156 +123,161 @@ void disparity_conv::stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_d // } int filt_mode = 2; - if(filt_mode == 1) - { + if (filt_mode == 1) { int dilation_size = 3; - cv::Mat disparity32F_filtered,disparity32F_filtered2; - cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, - cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), - cv::Point( dilation_size, dilation_size ) ); - cv::erode(disparity32F,disparity32F_filtered,element, cv::Point(-1, -1), 2); - // cv::dilate(disparity32F_filtered2,disparity32F_filtered,element, cv::Point(-1, -1), 20); - - disparity32F_filtered.convertTo(disparity32F_filtered2,CV_8U); - - - disparity32F_filtered.copyTo(di_msg->image,disparity32F_filtered2); - } - else if (filt_mode == 2) { + cv::Mat disparity32F_filtered, disparity32F_filtered2; + cv::Mat element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), + cv::Point(dilation_size, dilation_size)); + cv::erode(disparity32F, disparity32F_filtered, element, cv::Point(-1, -1), 2); + // cv::dilate(disparity32F_filtered2,disparity32F_filtered,element, cv::Point(-1, -1), + // 20); + + disparity32F_filtered.convertTo(disparity32F_filtered2, CV_8U); + + disparity32F_filtered.copyTo(di_msg->image, disparity32F_filtered2); + } else if (filt_mode == 2) { int kernel_size = 5; double sig = 1, th = 0, lm = 1.0, gm = 0.02, ps = 0; - cv::Mat kernel = cv::getGaborKernel(cv::Size(kernel_size,kernel_size), sig, th, lm, gm, ps); + cv::Mat kernel = + cv::getGaborKernel(cv::Size(kernel_size, kernel_size), sig, th, lm, gm, ps); cv::filter2D(disparity32F, di_msg->image, CV_32F, kernel); - } - else - { + } else { disparity32F.copyTo(di_msg->image); } - di_msg->header = msg_disp->header; + di_msg->header = msg_disp->header; di_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; -// ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); - disparity_conv_pub_.publish(di_msg->toImageMsg()); + // ROS_INFO("Time: %f + // \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); + disparity_conv_pub_->publish(*(di_msg->toImageMsg())); return; } -void disparity_conv::callback(const sensor_msgs::Image::ConstPtr &msg_disp, const sensor_msgs::Image::ConstPtr &msg_image) -{ - ROS_INFO_ONCE("Disparity CB 2"); - ros::Time start = ros::Time::now(); +// #################################################################################################################### +// ################################################################################################################### + +void disparity_conv::callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg_disp, + const sensor_msgs::msg::Image::ConstSharedPtr &msg_image) { + RCLCPP_INFO_ONCE(this->get_logger(), "Callback triggered"); + + rclcpp::Time start = this->get_clock()->now(); cv_bridge::CvImageConstPtr cv_ptrdisparity; cv_bridge::CvImageConstPtr cv_ptrImage; - cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); + // cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); + cv_bridge::CvImagePtr di_msg = std::make_shared(); cv::Mat disparity32F, image32F; - disparity32F = cv::Mat::zeros(msg_disp->height,msg_disp->width,CV_32FC1); - image32F = cv::Mat::zeros(msg_image->height,msg_image->width,CV_32FC1); + disparity32F = cv::Mat::zeros(msg_disp->height, msg_disp->width, CV_32FC1); + image32F = cv::Mat::zeros(msg_image->height, msg_image->width, CV_32FC1); - try - { + try { cv_ptrdisparity = cv_bridge::toCvShare(msg_disp); - if(1)//speckle filter + if (1) // speckle filter { cv::Mat speckleFiltered; - cv_ptrdisparity->image.convertTo(speckleFiltered,CV_16SC1); - cv::filterSpeckles(speckleFiltered,4095,100,4); - - speckleFiltered.convertTo(disparity32F,CV_32F); - } - else - cv_ptrdisparity->image.convertTo(disparity32F,CV_32F); - cv_ptrImage= cv_bridge::toCvShare(msg_image); - cv_ptrImage->image.convertTo(image32F,CV_32F); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); + cv_ptrdisparity->image.convertTo(speckleFiltered, CV_16SC1); + cv::filterSpeckles(speckleFiltered, 4095, 100, 4); + + speckleFiltered.convertTo(disparity32F, CV_32F); + } else + cv_ptrdisparity->image.convertTo(disparity32F, CV_32F); + cv_ptrImage = cv_bridge::toCvShare(msg_image); + cv_ptrImage->image.convertTo(image32F, CV_32F); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } - cv::Mat mask = disparity32F == 4095.0; - disparity32F.setTo(NAN,mask); //setting invalid entries to 0 or NAN - disparity32F = disparity32F/16.0;// actual disparities are obtained here as floats + disparity32F.setTo(NAN, mask); // setting invalid entries to 0 or NAN + disparity32F = disparity32F / 16.0; // actual disparities are obtained here as floats int filt_mode = 3; if (filt_mode == 2) { int kernel_size = 150; double sig = 1, th = 0, lm = 1.0, gm = 0.02, ps = 0; - cv::Mat kernel = cv::getGaborKernel(cv::Size(kernel_size,kernel_size), sig, th, lm, gm, ps); + cv::Mat kernel = + cv::getGaborKernel(cv::Size(kernel_size, kernel_size), sig, th, lm, gm, ps); cv::filter2D(image32F, di_msg->image, CV_32F, kernel); - } - else if(filt_mode == 3) - { - cv::GaussianBlur( image32F, image32F, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT ); - cv::Sobel( image32F, di_msg->image, CV_32FC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT ); -// di_msg->image.copyTo(image32F); -// cv::convertScaleAbs( image32F, di_msg->image ); + } else if (filt_mode == 3) { + cv::GaussianBlur(image32F, image32F, cv::Size(3, 3), 0, 0, cv::BORDER_DEFAULT); + cv::Sobel(image32F, di_msg->image, CV_32FC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT); + // di_msg->image.copyTo(image32F); + // cv::convertScaleAbs( image32F, di_msg->image ); di_msg->image = cv::abs(di_msg->image); - cv::Mat kernel = cv::Mat::ones(1,2,CV_32FC1);//15 + cv::Mat kernel = cv::Mat::ones(1, 2, CV_32FC1); // 15 cv::Mat acc; - cv::filter2D(di_msg->image,acc, CV_32FC1, kernel); - mask = acc > 10.0;//2 + cv::filter2D(di_msg->image, acc, CV_32FC1, kernel); + mask = acc > 10.0; // 2 di_msg->image.setTo(NAN); - if(0)//dilate + if (0) // dilate { int dilation_size = 2; - cv::Mat disparity32F_filtered,disparity32F_filtered2; - disparity32F.copyTo(disparity32F_filtered,mask); - cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, - cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), - cv::Point( dilation_size, dilation_size ) ); -// cv::erode(disparity32F_filtered2,disparity32F_filtered,element, cv::Point(-1, -1), 2); - cv::dilate(disparity32F_filtered,disparity32F_filtered2,element, cv::Point(-1, -1), 2); - disparity32F_filtered2.convertTo(disparity32F_filtered,CV_8U); - disparity32F.copyTo(di_msg->image,disparity32F_filtered); - } - else - disparity32F.copyTo(di_msg->image,mask); - } - else - { + cv::Mat disparity32F_filtered, disparity32F_filtered2; + disparity32F.copyTo(disparity32F_filtered, mask); + cv::Mat element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), + cv::Point(dilation_size, dilation_size)); + // cv::erode(disparity32F_filtered2,disparity32F_filtered,element, + // cv::Point(-1, -1), 2); + cv::dilate(disparity32F_filtered, disparity32F_filtered2, element, cv::Point(-1, -1), + 2); + disparity32F_filtered2.convertTo(disparity32F_filtered, CV_8U); + disparity32F.copyTo(di_msg->image, disparity32F_filtered); + } else + disparity32F.copyTo(di_msg->image, mask); + } else { disparity32F.copyTo(di_msg->image); } - //TEMPORAL DIFFERENCING FILTER + // TEMPORAL DIFFERENCING FILTER cv::Mat mask2; - if(0) - { - if(first) - { + if (0) { + if (first) { disparity32F.copyTo(prev_disp); first = false; } cv::Mat diff; cv::absdiff(disparity32F, prev_disp, diff); mask2 = diff < 80.0; - disparity32F.copyTo(di_msg->image,mask2); + disparity32F.copyTo(di_msg->image, mask2); disparity32F.copyTo(prev_disp); -// disparity32F.copyTo(di_msg->image,mask); + // disparity32F.copyTo(di_msg->image,mask); } - - di_msg->header = msg_disp->header; + di_msg->header = msg_disp->header; di_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; -// ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); - disparity_conv_pub_.publish(di_msg->toImageMsg()); - image32F.convertTo(image32F,CV_8U); + // ROS_INFO("Time: %f + // \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); + disparity_conv_pub_->publish(*(di_msg->toImageMsg())); + image32F.convertTo(image32F, CV_8U); di_msg->encoding = sensor_msgs::image_encodings::TYPE_8UC1; di_msg->image.setTo(0.0); - image32F.copyTo(di_msg->image,mask); - filtered_image_pub_.publish(di_msg->toImageMsg()); + image32F.copyTo(di_msg->image, mask); + filtered_image_pub_->publish(*(di_msg->toImageMsg())); return; } -disparity_conv::disparity_conv(ros::NodeHandle& nh){ - disparity_conv_pub_ = nh.advertise("/nerian_sp1/disparity_map_32F", 10); - filtered_image_pub_ = nh.advertise("/nerian_sp1/left/image_filtered_sky", 10); - ROS_INFO("into constr"); -// disparity_sub_ = nh.subscribe("/nerian_sp1/disparity_map", 1,&disparity_conv::stereoDisparityCb,this); +// ############################################################################################################################################# +// ############################################################################################################################################ +disparity_conv::disparity_conv(const rclcpp::NodeOptions &options) + : Node("disparity_conv", options) +{ + disparity_conv_pub_ = + this->create_publisher("/nerian_sp1/disparity_map_32F", 10); + filtered_image_pub_ = + this->create_publisher("/nerian_sp1/left/image_filtered_sky", 10); + + RCLCPP_INFO(this->get_logger(), "into constr"); + // disparity_conv_pub_ = nh.advertise("/nerian_sp1/disparity_map_32F", 10); + // filtered_image_pub_ = nh.advertise("/nerian_sp1/left/image_filtered_sky", + // 10); ROS_INFO("into constr"); + // disparity_sub_ = nh.subscribe("/nerian_sp1/disparity_map", + // 1,&disparity_conv::stereoDisparityCb,this); // Q-Matrix // [1.0 0.0 0.0 -317.20617294311523, @@ -259,16 +294,27 @@ disparity_conv::disparity_conv(ros::NodeHandle& nh){ first = true; } -int main(int argc, char** argv) -{ - ros::init(argc, argv, "disparity_conv"); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); // cv::initModule_nonfree();//THIS LINE IS IMPORTANT for using surf and sift features of opencv - ros::NodeHandle nh; - disparity_conv d(nh); - message_filters::Subscriber disp_sub(nh, "/nerian_sp1/disparity_map", 1); - message_filters::Subscriber image_sub(nh, "/nerian_sp1/left/image_raw", 1); - message_filters::TimeSynchronizer sync(disp_sub, image_sub, 10); - sync.registerCallback(boost::bind(&disparity_conv::callback,&d, _1, _2)); - ros::spin(); + auto node = std::make_shared(rclcpp::NodeOptions()); + + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + + auto disp_sub = std::make_shared>( + node.get(), "/nerian_sp1/disparity_map", rmw_qos_profile_default); + auto image_sub = std::make_shared>( + node.get(), "/nerian_sp1/left/image_raw", rmw_qos_profile_default); + + auto sync = std::make_shared>(SyncPolicy(10), + *disp_sub, *image_sub); + + sync->registerCallback( + std::bind(&disparity_conv::callback, node, std::placeholders::_1, std::placeholders::_2)); + + rclcpp::spin(node); + + rclcpp::shutdown(); return 0; } diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion.cpp index 997101331..547fad67c 100755 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion.cpp +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion.cpp @@ -1,52 +1,57 @@ /* -* Copyright (c) 2016 Carnegie Mellon University, Author -* -* For License information please see the LICENSE file in the root directory. -* -*/ - -//Applies C-Space expansion on disparity images. -#include "ros/ros.h" -#include "sensor_msgs/Image.h" -#include "sensor_msgs/CameraInfo.h" -#include "stereo_msgs/DisparityImage.h" -#include "geometry_msgs/PolygonStamped.h" -#include "visualization_msgs/MarkerArray.h" -#include "std_msgs/Header.h" + * Copyright (c) 2016 Carnegie Mellon University, Author + * + * For License information please see the LICENSE file in the root directory. + * + */ + +// Applies C-Space expansion on disparity images. + #include -#include "opencv2/core/core.hpp" -#include #include -#include -#include -#include -#include +#include +#include +#include #include -#include "tf/tf.h" -#include +#include + +#include +#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#define SCALE 2.0 // num_units/pixel -//#define lut_max_disparity_ 165 // units -//#define robot_radius_ 2.0 // in meters -//for 2times Robsize = 0.0173 at 40m -//15 = 0.0677 -#define DEPTH_ERROR_COEFF 0.0177 +#include "opencv2/core/core.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/header.hpp" typedef pcl::PointCloud PointCloud; -class disparity_expansion{ -public: - disparity_expansion(ros::NodeHandle& nh); +class DisparityExpansionNode : public rclcpp::Node { + public: + DisparityExpansionNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - ros::Publisher expansion_cloud_pub; - ros::Publisher expanded_disparity_fg_pub; - ros::Publisher expanded_disparity_bg_pub; - ros::Publisher expansion_poly_pub; - ros::Subscriber subs_; - ros::Subscriber cam_info_sub_; - ros::Subscriber disparity_sub_; + rclcpp::Publisher::SharedPtr expansion_cloud_pub; + rclcpp::Publisher::SharedPtr expanded_disparity_fg_pub; + rclcpp::Publisher::SharedPtr expanded_disparity_bg_pub; + rclcpp::Publisher::SharedPtr expansion_poly_pub; + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr disparity_sub_; + rclcpp::Subscription::SharedPtr depth_sub_; double baseline; double downsample_scale; @@ -54,212 +59,279 @@ class disparity_expansion{ bool got_cam_info; image_geometry::PinholeCameraModel model_; - void getCamInfo(const sensor_msgs::CameraInfo::ConstPtr &msg_info); - void stereoDisparityCb(const stereo_msgs::DisparityImage::ConstPtr &msg_disp);//sensor_msgs::Image::ConstPtr &msg_disp); + void getCamInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_info); + void depthImageCb(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + void stereoDisparityCb(const std::shared_ptr& msg_disp); void generateExpansionLUT(); + cv::Mat depthToDisparity(const cv::Mat& depth_image); + void visualizeDepthImage(const cv::Mat& depth_image); + void visualizeDisparityImage(const cv::Mat& disparity_image); - struct cell - { - unsigned int idx1; - unsigned int idx2; + struct cell { + unsigned int idx1; + unsigned int idx2; }; - int lut_max_disparity_;// 165 // units - double robot_radius_;// 2.0 // in meters + int lut_max_disparity_; + double robot_radius_; double padding_; double bg_multiplier_; double pixel_error_; - cell table_u[200][640];//[640]; - cell table_v[200][480];//[512]; + double scale_; + std::vector> table_u; + std::vector> table_v; double table_d[200]; - double fx_,fy_,cx_,cy_; - unsigned int width,height; + double fx_, fy_, cx_, cy_; + unsigned int width, height; }; -void disparity_expansion::generateExpansionLUT() -{ +void DisparityExpansionNode::generateExpansionLUT() { bool debug = false; - if(debug) - { - ROS_WARN("Expansion LUT Debug disabled creation"); + if (debug) { + RCLCPP_WARN(this->get_logger(), "Expansion LUT Debug disabled creation"); LUT_ready = true; } - if(LUT_ready) - { - ROS_ERROR("LUT all ready"); -// sleep(1); + if (LUT_ready) { + RCLCPP_ERROR(this->get_logger(), "LUT all ready"); + return; } double center_x = cx_; double center_y = cy_; double constant_x = 1.0 / fx_; double constant_y = 1.0 / fy_; - ROS_INFO("Fx Fy Cx Cy: %f %f , %f %f \nW H Baseline: %d %d %f",fx_,fy_,cx_,cy_,width,height,baseline); - double r = robot_radius_;//expansion radius in cm - int u1,u2,v1,v2; - double x,y,z; + RCLCPP_INFO(this->get_logger(), "Fx Fy Cx Cy: %f %f , %f %f \nW H Baseline: %d %d %f", fx_, fy_, + cx_, cy_, width, height, baseline); + double r = robot_radius_; // expansion radius in cm + table_u = std::vector>(lut_max_disparity_, std::vector(width)); + table_v = std::vector>(lut_max_disparity_, std::vector(height)); + int u1, u2, v1, v2; + double x, y, z; double disparity; - - for(unsigned int disp_idx = 1;disp_idx< lut_max_disparity_ ;disp_idx++) - { -// z = depth*0.01;//cm to m - disparity = disp_idx/SCALE;//1 cell = 0.5m, z is in meters - r = robot_radius_;// * exp(DEPTH_ERROR_COEFF*z); + for (unsigned int disp_idx = 1; disp_idx < lut_max_disparity_; disp_idx++) { + disparity = disp_idx / scale_; // 1 cell = 0.5m, z is in meters + r = robot_radius_; // * exp(DEPTH_ERROR_COEFF*z); z = baseline * fx_ / disparity; - double disp_new = baseline * fx_/(z - robot_radius_) + 0.5; - table_d[disp_idx] = disp_new ; -// ROS_INFO("REAL EXPANDED: %f , %f",disparity,table_d[disp_idx]); - + double disp_new = baseline * fx_ / (z - robot_radius_) + 0.5; + table_d[disp_idx] = disp_new; - for ( int v = ( int ) height - 1; v >= 0; --v ) - { - y = ( v - center_y ) * z * constant_y; + for (int v = (int)height - 1; v >= 0; --v) { + y = (v - center_y) * z * constant_y; - double beta = atan2(z,y); - double beta1 = asin(r/sqrt(z*z + y*y)); + double beta = atan2(z, y); + double beta1 = asin(r / sqrt(z * z + y * y)); - double r1 = z/tan(beta+beta1); - double r2 = z/tan(beta-beta1); - v1 = fy_*r1/z + center_y; - v2 = fy_*r2/z + center_y; + double r1 = z / tan(beta + beta1); + double r2 = z / tan(beta - beta1); + v1 = fy_ * r1 / z + center_y; + v2 = fy_ * r2 / z + center_y; - if((v2-v1)<0) - ROS_ERROR("Something MESSED %d %d %d",v1,v2,disp_idx); + if ((v2 - v1) < 0) + RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", v1, v2, disp_idx); - if(v1 < 0) v1 = 0; - if(v1 > (height-1)) v1 = height-1; + if (v1 < 0) v1 = 0; + if (v1 > (height - 1)) v1 = height - 1; - if(v2 < 0) v2 = 0; - if(v2 > (height-1)) v2 = height-1; + if (v2 < 0) v2 = 0; + if (v2 > (height - 1)) v2 = height - 1; - table_v[disp_idx][v].idx1 = v1 ; - table_v[disp_idx][v].idx2 = v2 ; + table_v[disp_idx][v].idx1 = v1; + table_v[disp_idx][v].idx2 = v2; } - for ( int u = ( int ) width - 1; u >= 0; --u ) - { - x = ( u - center_x ) * z * constant_x; - - double alpha = atan2(z,x); - double alpha1 = asin(r/sqrt(z*z + x*x)); + for (int u = (int)width - 1; u >= 0; --u) { + x = (u - center_x) * z * constant_x; - double r1 = z/tan(alpha+alpha1); - double r2 = z/tan(alpha-alpha1); - u1 = fx_*r1/z + center_x; - u2 = fx_*r2/z + center_x; + double alpha = atan2(z, x); + double alpha1 = asin(r / sqrt(z * z + x * x)); - if((u2-u1)<0) - ROS_ERROR("Something MESSED %d %d %d",u1,u2,disp_idx); + double r1 = z / tan(alpha + alpha1); + double r2 = z / tan(alpha - alpha1); + u1 = fx_ * r1 / z + center_x; + u2 = fx_ * r2 / z + center_x; - if(u1 < 0) u1 = 0; - if(u1 > (width-1)) u1 = width-1; + if ((u2 - u1) < 0) + RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", u1, u2, disp_idx); - if(u2 < 0) u2 = 0; - if(u2 > (width-1)) u2 = width-1; + if (u1 < 0) u1 = 0; + if (u1 > (width - 1)) u1 = width - 1; - table_u[disp_idx][u].idx1 = u1 ; - table_u[disp_idx][u].idx2 = u2 ; + if (u2 < 0) u2 = 0; + if (u2 > (width - 1)) u2 = width - 1; + table_u[disp_idx][u].idx1 = u1; + table_u[disp_idx][u].idx2 = u2; } } -// for ( int u = (width - 1); u >= 0; --u ) -// { -// int d = 199; -// ROS_ERROR("window disp_idx u1 u2 \t %d \t%d \t%d",u,table_u[d][u].idx1,table_u[d][u].idx2); -// } - ROS_WARN("Expansion LUT created: LUT MAX: %d , ROBOT SIZE: %f",lut_max_disparity_/2,robot_radius_); + + RCLCPP_WARN(this->get_logger(), "Expansion LUT created: LUT MAX: %d , ROBOT SIZE: %f", + lut_max_disparity_ / 2, robot_radius_); LUT_ready = true; } -void disparity_expansion::getCamInfo(const sensor_msgs::CameraInfo::ConstPtr& msg_info) -{ - if(got_cam_info) - return; - model_.fromCameraInfo ( msg_info ); - ROS_INFO_ONCE("Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f",model_.fx(),model_.fy(),model_.cx(),model_.cy()); - cx_ = model_.cx()/downsample_scale; - cy_ = model_.cy()/downsample_scale; - fx_ = fy_ = model_.fx()/downsample_scale; - width = msg_info->width/downsample_scale; - height = msg_info->height/downsample_scale; - baseline = -msg_info->P[3]/msg_info->P[0]; - baseline *=downsample_scale; +void DisparityExpansionNode::getCamInfo( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_info) { + if (got_cam_info) return; + model_.fromCameraInfo(msg_info); + RCLCPP_INFO_ONCE(this->get_logger(), "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f", model_.fx(), + model_.fy(), model_.cx(), model_.cy()); + cx_ = model_.cx() / downsample_scale; + cy_ = model_.cy() / downsample_scale; + fx_ = fy_ = model_.fx() / downsample_scale; + width = msg_info->width / downsample_scale; + height = msg_info->height / downsample_scale; + baseline = -msg_info->p[3] / msg_info->p[0]; + baseline = 0.25; + baseline *= downsample_scale; generateExpansionLUT(); got_cam_info = true; + RCLCPP_INFO(this->get_logger(), "Baseline: %.4f meters", baseline); + RCLCPP_INFO(this->get_logger(), "Focal length (fx): %.4f pixels", fx_); } +void DisparityExpansionNode::depthImageCb(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } -void disparity_expansion::stereoDisparityCb(const stereo_msgs::DisparityImage::ConstPtr &msg_disp){//sensor_msgs::Image::ConstPtr &msg_disp){ -// ROS_INFO("Disparity CB"); -// dimage = *msg_disp; - std::cout << "hello" << std::endl; + if (cv_ptr->image.empty()) { + RCLCPP_ERROR(this->get_logger(), "Received empty depth image."); + return; + } + RCLCPP_INFO(this->get_logger(), "Printing 5x5 grid of depth values:"); + for (int i = 0; i < 5 && i < cv_ptr->image.rows; ++i) { + for (int j = 0; j < 5 && j < cv_ptr->image.cols; ++j) { + float depth_val = cv_ptr->image.at(i, j); + RCLCPP_INFO(this->get_logger(), "Depth[%d,%d]: %.4f", i, j, depth_val); + } + } + cv::Mat disparity_image = depthToDisparity(cv_ptr->image); + visualizeDisparityImage(disparity_image); + // visualizeDepthImage(cv_ptr->image); + + auto disparity_msg = std::make_shared(); + disparity_msg->header = msg->header; + disparity_msg->image = + *cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_32FC1, disparity_image) + .toImageMsg(); + disparity_msg->f = fx_; + + // Call the existing disparity processing function + stereoDisparityCb(disparity_msg); +} - if(!LUT_ready) - { - ROS_INFO_THROTTLE(1,"LUT not ready yet, not processing disparity"); +cv::Mat DisparityExpansionNode::depthToDisparity(const cv::Mat& depth_image) { + if (depth_image.empty()) { + RCLCPP_ERROR(this->get_logger(), + "Depth image is empty in depthToDisparity. Raising an error."); + throw std::runtime_error("Depth image is empty in depthToDisparity."); + } + + cv::Mat depth_float; + if (depth_image.type() != CV_32F) { + depth_image.convertTo(depth_float, CV_32F); + } else { + depth_float = depth_image; + } + + cv::Mat disparity_image(depth_float.size(), CV_32F); + + // DISPARITY FORMULA + + disparity_image = (baseline * fx_) / (depth_image); // MULT BY 100 + + RCLCPP_INFO(this->get_logger(), "Baseline: %.4f", baseline); + RCLCPP_INFO(this->get_logger(), "Focal length (fx): %.4f", fx_); + cv::patchNaNs(disparity_image, 0.0f); + + disparity_image.setTo(0.0f, disparity_image == std::numeric_limits::infinity()); + + return disparity_image; +} + +void DisparityExpansionNode::visualizeDepthImage(const cv::Mat& depth_image) { + cv::Mat normalized, display; + cv::normalize(depth_image, normalized, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(normalized, display, cv::COLORMAP_JET); + cv::imshow("Depth Image", display); + cv::waitKey(1); +} + +void DisparityExpansionNode::visualizeDisparityImage(const cv::Mat& disparity_image) { + cv::imshow("Disparity Image", disparity_image); + cv::waitKey(1); +} + +void DisparityExpansionNode::stereoDisparityCb( + const std::shared_ptr& + msg_disp) { // sensor_msgs::Image::ConstPtr &msg_disp){ + + std::cout << "hello" << std::endl; + + if (!LUT_ready) { + auto& clk = *this->get_clock(); + RCLCPP_INFO_THROTTLE(this->get_logger(), clk, 1, + "LUT not ready yet, not processing disparity"); return; } -// generateExpansionLUT(); + + auto img_msg = std::make_shared(); + img_msg->header = msg_disp->header; + img_msg->height = msg_disp->image.height; + img_msg->width = msg_disp->image.width; + img_msg->encoding = msg_disp->image.encoding; + img_msg->is_bigendian = msg_disp->image.is_bigendian; + img_msg->step = msg_disp->image.step; + img_msg->data = msg_disp->image.data; cv_bridge::CvImagePtr fg_msg(new cv_bridge::CvImage()); cv_bridge::CvImagePtr bg_msg(new cv_bridge::CvImage()); - cv_bridge::CvImageConstPtr cv_ptrdisparity; - try - { - cv_ptrdisparity = cv_bridge::toCvShare(sensor_msgs::ImageConstPtr(new sensor_msgs::Image(msg_disp->image))); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; + try { + cv_ptrdisparity = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; } cv::Mat disparity32F = cv_ptrdisparity->image; + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "Received empty disparity image"); + return; + } cv::resize(disparity32F, disparity32F, cv::Size(), 0.5, 0.5, cv::INTER_AREA); + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "Resized disparity image is empty"); + return; + } cv::Mat disparity_fg; cv::Mat disparity_bg; cv::Mat disparity32F_bg; - - -// const sensor_msgs::ImageConstPtr my_ptr = &dimage; -// cv::Mat dummy(msg_disp->image.height,msg_disp->image.width,CV_32FC4); -// dummy.copyTo(free_msg->image); -// free_msg->image.zeros(msg_disp->image.height,msg_disp->image.width,CV_32FC4); -// free_msg->header = dimage.header; -// free_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC4; - try - { -// fg_msg = cv_bridge::toCvCopy(dimage, sensor_msgs::image_encodings::TYPE_32FC1); -// bg_msg = cv_bridge::toCvCopy(dimage, sensor_msgs::image_encodings::TYPE_32FC1); -// di_msg = cv_bridge::toCvCopy(dimage, sensor_msgs::image_encodings::TYPE_32FC1); - -// fg_msg->image.zeros(disparity32F.rows,disparity32F.cols,CV_32FC1);// = disparity32F; -// bg_msg->image.zeros(disparity32F.rows,disparity32F.cols,CV_32FC1);// .create(disparity32F.rows,disparity32F.cols,CV_32F);// = disparity32F; -// di_msg->image.zeros(disparity32F.rows,disparity32F.cols,CV_32FC1);// .create(disparity32F.rows,disparity32F.cols,CV_32F);// = disparity32F; - disparity32F.copyTo(fg_msg->image ); + try { + disparity32F.copyTo(fg_msg->image); disparity32F.copyTo(bg_msg->image); disparity32F.copyTo(disparity_fg); disparity32F.copyTo(disparity_bg); disparity32F.copyTo(disparity32F_bg); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } - fg_msg->header = msg_disp->header; + fg_msg->header = msg_disp->header; fg_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; - bg_msg->header = msg_disp->header; + bg_msg->header = msg_disp->header; bg_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; -// bg_msg->image = bg_msg->image + INFINITY; -// fg_msg->image = fg_msg->image - INFINITY; - if(1)//make cloud + + if (1) // make cloud { // Use correct principal point from calibration float center_x = cx_; @@ -268,71 +340,60 @@ void disparity_expansion::stereoDisparityCb(const stereo_msgs::DisparityImage::C float constant_x = 1.0 / fx_; float constant_y = 1.0 / fy_; -// if ( di_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 ) { - ROS_INFO_ONCE("IMG TYPE 32FC, GOOD"); -// int row_step = dimage.step / sizeof ( float ); -// const float* disparity_row = reinterpret_cast ( &dimage.data[0] ); - ros::Time start = ros::Time::now(); + RCLCPP_INFO_ONCE(this->get_logger(), "IMG TYPE 32FC, GOOD"); + + rclcpp::Time start = this->get_clock()->now(); float padding = padding_; - for ( int v = ( int ) height - 2; (v >= 0); v-=1 ) - { - for ( int u = ( int ) width - 1; u >= 0; u-=1 ) - { - float disparity_value = disparity32F.at(v,u);//disparity_row[v * row_step + u];// + 0.5; - if(!isnan(double(disparity_value)) && ((int(disparity_value*SCALE)+1) < lut_max_disparity_) && ((int(disparity_value*SCALE)+1) > 0))//expand + for (int v = (int)height - 2; (v >= 0); v -= 1) { + for (int u = (int)width - 1; u >= 0; u -= 1) { + float disparity_value = + disparity32F.at(v, u); // disparity_row[v * row_step + u];// + 0.5; + if (!std::isnan(double(disparity_value * scale_)) && + ((int(disparity_value * scale_) + 1) < lut_max_disparity_) && + ((int(disparity_value * scale_) + 1) > 0)) // expand { -// disparity_value = disparity32F.at(v,u)+0.5; - unsigned int u1 = table_u[int(disparity_value*SCALE)+1][u].idx1; - unsigned int u2 = table_u[int(disparity_value*SCALE)+1][u].idx2; - -// unsigned int v1 = v;//table_v[int(disparity_value*SCALE)+1][v].idx1; -// unsigned int v2 = v+1;//table_v[int(disparity_value*SCALE)+1][v].idx2; - -// if(v1 <0 || u1 <0 || v2 >=height || u2>=width || (u2-u1)<=0 || (v2-v1)<=0) -// { -// ROS_ERROR("U Expansion out of img bounds with disparity: %f",disparity_value); -// ROS_ERROR("u1 u2 v1 v2 %d %d \t %d %d",u1,u2,v1,v2); -// continue; -// } - cv::Rect roi = cv::Rect(u1,v,(u2-u1),1); + // disparity_value = disparity32F.at(v,u)+0.5; + unsigned int u1 = table_u[int(disparity_value * scale_) + 1][u].idx1; + unsigned int u2 = table_u[int(disparity_value * scale_) + 1][u].idx2; + + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "disparity32F matrix is empty."); + return; + } + + cv::Rect roi = cv::Rect(u1, v, (u2 - u1), 1); + cv::Mat submat_t = disparity32F(roi).clone(); double min, max; - cv::Point p1,p2; - int min_idx,max_idx; -// cv::minMaxIdx(disparity32F(roi), &min, &max,&min_idx,&max_idx); - cv::minMaxLoc(disparity32F(roi), &min, &max,&p1,&p2); + cv::Point p1, p2; + int min_idx, max_idx; + + cv::minMaxLoc(disparity32F(roi), &min, &max, &p1, &p2); min_idx = p1.x; max_idx = p2.x; - float disp_new_fg = max;// = table_d[int(max*SCALE)]; - float disp_new_bg = min;// = table_d[int(min*SCALE)]; - float disp_to_depth = baseline * fx_/max; + float disp_new_fg = max; // = table_d[int(max*SCALE)]; + float disp_new_bg = min; // = table_d[int(min*SCALE)]; + float disp_to_depth = baseline * fx_ / max; cv::Mat submat; - cv::divide(baseline *fx_,submat_t,submat); - submat = (submat - disp_to_depth);///robot_radius_; -// ROS_INFO("submat at : %f",disp_to_depth); -// std::cout<(0,j); - if(std::isfinite(val)) - { - if(val < ctr*range && val > max_depth) - { + for (int j = 0; j < submat.cols; j++) { + float val = submat.at(0, j); + if (std::isfinite(val)) { + if (val < ctr * range && val > max_depth) { found = true; max_depth = val; } @@ -341,77 +402,75 @@ void disparity_expansion::stereoDisparityCb(const stereo_msgs::DisparityImage::C ctr++; } disp_to_depth += max_depth; - } - else - disp_to_depth +=padding;// baseline * fx_/min; - + } else + disp_to_depth += padding; // baseline * fx_/min; - disp_new_bg = baseline * fx_/(disp_to_depth /*+ robot_radius_*/) /*- 0.5*/; + disp_new_bg = + baseline * fx_ / (disp_to_depth /*+ robot_radius_*/) /*- 0.5*/; disparity_fg(roi).setTo(disp_new_fg); - disparity_bg(roi).setTo(/*min*/disp_new_bg); + disparity_bg(roi).setTo(/*min*/ disp_new_bg); - int u_temp = u1+max_idx; + int u_temp = u1 + max_idx; if (u_temp >= u) u = u1; else - u = u_temp+1; + u = u_temp + 1; + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "BAD disparity during expansion for u: " + << disparity_value << " lut_max_disparity_: " << lut_max_disparity_ + << " SCALE: " << scale_); } - else - ROS_ERROR_THROTTLE(1,"BAD disparity during expansion: %f",disparity_value); } } -// fg_msg->image.copyTo(disparity32F); disparity_fg.copyTo(disparity32F); disparity_bg.copyTo(disparity32F_bg); - for ( int u = ( int ) width - 2; (u >= 0) && true; u-=1 ) - { - for ( int v = ( int ) height - 1; v >= 0; v-=1 ) - { - float disparity_value = disparity32F.at(v,u) + pixel_error_;//disparity_row[v * row_step + u];// + 0.5; - if(!isnan(double(disparity_value)) && ((int(disparity_value*SCALE)+1) < lut_max_disparity_) && ((int(disparity_value*SCALE)+1) > 0))//expand + for (int u = (int)width - 2; (u >= 0) && true; u -= 1) { + for (int v = (int)height - 1; v >= 0; v -= 1) { + float disparity_value = + disparity32F.at(v, u) + + pixel_error_; // disparity_row[v * row_step + u];// + 0.5; + if (!std::isnan(double(disparity_value)) && + ((int(disparity_value * scale_) + 1) < lut_max_disparity_) && + ((int(disparity_value * scale_) + 1) > 0)) // expand { - unsigned int v1 = table_v[int(disparity_value*SCALE)+1][v].idx1; - unsigned int v2 = table_v[int(disparity_value*SCALE)+1][v].idx2; + unsigned int v1 = table_v[int(disparity_value * scale_) + 1][v].idx1; + unsigned int v2 = table_v[int(disparity_value * scale_) + 1][v].idx2; - cv::Rect roi = cv::Rect(u,v1,1,(v2-v1)); + cv::Rect roi = cv::Rect(u, v1, 1, (v2 - v1)); cv::Mat submat_t = disparity32F_bg(roi).clone(); double min, max; - cv::Point p1,p2; - int min_idx,max_idx; - cv::minMaxLoc(disparity32F(roi), &min, &max,&p1,&p2); + cv::Point p1, p2; + int min_idx, max_idx; + cv::minMaxLoc(disparity32F(roi), &min, &max, &p1, &p2); min_idx = p1.y; max_idx = p2.y; - float disp_new_fg;// = max;// = table_d[int(max*SCALE)]; - float disp_new_bg;// = min;// = table_d[int(min*SCALE)]; - float disp_to_depth = baseline * fx_/max; - disp_new_fg = baseline * fx_/(disp_to_depth - robot_radius_) + pixel_error_; + float disp_new_fg; // = max;// = table_d[int(max*SCALE)]; + float disp_new_bg; // = min;// = table_d[int(min*SCALE)]; + float disp_to_depth = baseline * fx_ / max; + disp_new_fg = + baseline * fx_ / (disp_to_depth - robot_radius_) + pixel_error_; cv::Mat submat; - cv::divide(baseline *fx_,submat_t,submat); - cv::minMaxLoc(disparity32F_bg(roi), &min, &max,&p1,&p2); - disp_to_depth = baseline * fx_/max; - submat = (submat - disp_to_depth);///robot_radius_; -// ROS_INFO("submat at : %f",disp_to_depth); -// std::cout<(j,0); - if(std::isfinite(val)) - { - if(val < ctr*range && val > max_depth) - { + for (int j = 0; j < submat.rows; j++) { + float val = submat.at(j, 0); + if (std::isfinite(val)) { + if (val < ctr * range && val > max_depth) { found = true; max_depth = val; } @@ -420,94 +479,94 @@ void disparity_expansion::stereoDisparityCb(const stereo_msgs::DisparityImage::C ctr++; } disp_to_depth += max_depth; - } - else - disp_to_depth +=padding;// baseline * fx_/min; + } else + disp_to_depth += padding; // baseline * fx_/min; - disp_new_bg = baseline * fx_/(disp_to_depth + robot_radius_) - pixel_error_; + disp_new_bg = + baseline * fx_ / (disp_to_depth + robot_radius_) - pixel_error_; disp_new_bg = disp_new_bg < 0.0 ? 0.0001 : disp_new_bg; disparity_fg(roi).setTo(disp_new_fg); disparity_bg(roi).setTo(disp_new_bg); - int v_temp = v1+max_idx; + int v_temp = v1 + max_idx; if (v_temp >= v) v = v1; else - v = v_temp+1; + v = v_temp + 1; + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "BAD disparity during expansion for v: " + << disparity_value << " lut_max_disparity_: " << lut_max_disparity_ + << " SCALE: " << scale_); } - else - ROS_ERROR_THROTTLE(1,"BAD disparity during expansion: %f",disparity_value); } } fg_msg->image = disparity_fg; bg_msg->image = disparity_bg; + expanded_disparity_fg_pub->publish(*(fg_msg->toImageMsg())); + expanded_disparity_bg_pub->publish(*(bg_msg->toImageMsg())); - ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); - - - expanded_disparity_fg_pub.publish(fg_msg->toImageMsg()); - expanded_disparity_bg_pub.publish(bg_msg->toImageMsg()); - - if(expansion_poly_pub.getNumSubscribers() > 0 )//create expansion PCD + if (expansion_poly_pub->get_subscription_count() > 0) // create expansion PCD { - visualization_msgs::MarkerArray marker_arr; - visualization_msgs::Marker marker; + visualization_msgs::msg::MarkerArray marker_arr; + visualization_msgs::msg::Marker marker; marker.header = msg_disp->header; marker.ns = "occ_space"; marker.id = 0; - marker.type = visualization_msgs::Marker::LINE_STRIP; -// marker.mesh_resource = "package://gazebo_robot_description/meshes/octorotor/riverine_octo_m.dae"; - marker.lifetime = ros::Duration(0.5); - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x =0;//xyz_centroid[0]; - marker.pose.position.y =0;//xyz_centroid[1]; - marker.pose.position.z =0;//xyz_centroid[2]; - - tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0,0.0,0.0), marker.pose.orientation); -// marker.pose.orientation.w = 1; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; // xyz_centroid[0]; + marker.pose.position.y = 0; // xyz_centroid[1]; + marker.pose.position.z = 0; // xyz_centroid[2]; + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, 0.0); + marker.pose.orientation = tf2::toMsg(q); + float marker_scale = 0.51; marker.scale.x = marker_scale; marker.scale.y = marker_scale; marker.scale.z = marker_scale; - marker.color.a = 0.3; // Don't forget to set the alpha! + marker.color.a = 0.3; // Don't forget to set the alpha! marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 0.0; - geometry_msgs::PolygonStamped poly; + geometry_msgs::msg::PolygonStamped poly; poly.header = msg_disp->header; int v = 120; float prev_depth = 0.0; - for ( int v = ( int ) 0; v <= 239; v+=10 ) - { - for ( int u = ( int ) width - 1; u >= 0; u-- ) - { - float depth_value = baseline * fx_ / fg_msg->image.at(v,u);//free_msg->image.at(v,u)[0]; - float depth_diff = fabs(depth_value-prev_depth); + for (int v = (int)0; v <= 239; v += 10) { + for (int u = (int)width - 1; u >= 0; u--) { + float depth_value = baseline * fx_ / + fg_msg->image.at( + v, u); // free_msg->image.at(v,u)[0]; + float depth_diff = fabs(depth_value - prev_depth); prev_depth = depth_value; - if(!isnan(depth_value) && !isinf(depth_value) && depth_diff < 0.5) - { - marker.color.r = 1.0 * (fg_msg->image.at(v,u)-pixel_error_)/fg_msg->image.at(v,u); - marker.color.g = 1.0 * (pixel_error_)/fg_msg->image.at(v,u); - geometry_msgs::Point gm_p; - gm_p.x = ( u - center_x ) * depth_value * constant_x; - gm_p.y = ( v - center_y ) * depth_value * constant_y; + if (!std::isnan(depth_value) && !std::isinf(depth_value) && + depth_diff < 0.5) { + marker.color.r = 1.0 * (fg_msg->image.at(v, u) - pixel_error_) / + fg_msg->image.at(v, u); + marker.color.g = 1.0 * (pixel_error_) / fg_msg->image.at(v, u); + geometry_msgs::msg::Point gm_p; + gm_p.x = (u - center_x) * depth_value * constant_x; + gm_p.y = (v - center_y) * depth_value * constant_y; gm_p.z = depth_value; marker.points.push_back(gm_p); - depth_value = baseline * fx_ / bg_msg->image.at(v,u); - gm_p.x = ( u - center_x ) * depth_value * constant_x; - gm_p.y = ( v - center_y ) * depth_value * constant_y; + depth_value = baseline * fx_ / bg_msg->image.at(v, u); + gm_p.x = (u - center_x) * depth_value * constant_x; + gm_p.y = (v - center_y) * depth_value * constant_y; gm_p.z = depth_value; marker.points.push_back(gm_p); - // poly.polygon.points.push_back(gm_p); - } - else - { + } else { marker_arr.markers.push_back(marker); marker.points.clear(); marker.id++; @@ -515,127 +574,138 @@ void disparity_expansion::stereoDisparityCb(const stereo_msgs::DisparityImage::C } } marker_arr.markers.push_back(marker); - expansion_poly_pub.publish(marker_arr); + expansion_poly_pub->publish(marker_arr); } - if(expansion_cloud_pub.getNumSubscribers() > 0 )//create expansion PCD + if (expansion_cloud_pub->get_subscription_count() > 0) // create expansion PCD { - PointCloud::Ptr cloud (new PointCloud); + PointCloud::Ptr cloud(new PointCloud); cloud->header.frame_id = msg_disp->header.frame_id; cloud->height = 1; - cloud->width =1; + cloud->width = 1; cloud->is_dense = false; - int point_counter=0; - pcl::PointXYZI pt_fg,pt_bg,pt_free1,pt_free2,pt_real; - - for ( int v = ( int ) height - 1; v >= 0; v-=4 ) - { - for ( int u = ( int ) width - 1; u >= 0; u-=4 ) - { - - // Fill in XYZ -// float depth_value = baseline * fx_ / new_disparity_bridgePtr->image.at(v,u);//fg_msg->image.at(v,u);//free_msg->image.at(v,u)[0]; - float depth_value = baseline * fx_ / fg_msg->image.at(v,u);//free_msg->image.at(v,u)[0]; - pt_fg.x = ( u - center_x ) * depth_value * constant_x; - pt_fg.y = ( v - center_y ) * depth_value * constant_y; + int point_counter = 0; + pcl::PointXYZI pt_fg, pt_bg, pt_free1, pt_free2, pt_real; + + for (int v = (int)height - 1; v >= 0; v -= 4) { + for (int u = (int)width - 1; u >= 0; u -= 4) { + float depth_value = baseline * fx_ / + fg_msg->image.at( + v, u); // free_msg->image.at(v,u)[0]; + pt_fg.x = (u - center_x) * depth_value * constant_x; + pt_fg.y = (v - center_y) * depth_value * constant_y; pt_fg.z = depth_value; pt_fg.intensity = 220; - depth_value = baseline * fx_ / bg_msg->image.at(v,u);//free_msg->image.at(v,u)[1]; - pt_bg.x = ( u - center_x ) * depth_value * constant_x; - pt_bg.y = ( v - center_y ) * depth_value * constant_y; + depth_value = baseline * fx_ / + bg_msg->image.at( + v, u); // free_msg->image.at(v,u)[1]; + pt_bg.x = (u - center_x) * depth_value * constant_x; + pt_bg.y = (v - center_y) * depth_value * constant_y; pt_bg.z = depth_value; pt_bg.intensity = 120; - //ACtual PCD - depth_value = baseline * fx_ / cv_ptrdisparity->image.at(v,u);//new_disparity_bridgePtr->image.at(v,u); - pt_real.x = ( u - center_x ) * depth_value * constant_x; - pt_real.y = ( v - center_y ) * depth_value * constant_y; + // ACtual PCD + depth_value = baseline * fx_ / + cv_ptrdisparity->image.at( + v, u); // new_disparity_bridgePtr->image.at(v,u); + pt_real.x = (u - center_x) * depth_value * constant_x; + pt_real.y = (v - center_y) * depth_value * constant_y; pt_real.z = depth_value; - pt_real.intensity = 170;//*disparity32F.at(v,u)/200; - -// depth_value = baseline * fx_ / free_msg->image.at(v,u)[2]; -// pt_free1.x = ( u - center_x ) * depth_value * constant_x; -// pt_free1.y = ( v - center_y ) * depth_value * constant_y; -// pt_free1.z = depth_value; -// pt_free1.intensity = 170; -// depth_value = baseline * fx_ / free_msg->image.at(v,u)[3]; -// pt_free2.x = ( u - center_x ) * depth_value * constant_x; -// pt_free2.y = ( v - center_y ) * depth_value * constant_y; -// pt_free2.z = depth_value; -// pt_free2.intensity = 170; - -// if(fg_msg->image.at(v,u) >= 0.1 && bg_msg->image.at(v,u) >= 0.1 ) -// if(new_disparity_bridgePtr->image.at(v,u) > 0.1) + pt_real.intensity = 170; //*disparity32F.at(v,u)/200; + { -// ROS_INFO("depth: %f",new_disparity_bridgePtr->image.at(v,u)); point_counter++; - cloud->points.push_back ( pt_fg ); + cloud->points.push_back(pt_fg); point_counter++; - cloud->points.push_back ( pt_bg ); + cloud->points.push_back(pt_bg); point_counter++; - cloud->points.push_back ( pt_real ); - -// point_counter++; -// cloud->points.push_back ( pt_free1 ); -// point_counter++; -// cloud->points.push_back ( pt_free2 ); - + cloud->points.push_back(pt_real); } } } - cloud->width = point_counter; - cloud->header.seq = msg_disp->header.seq; - cloud->header.stamp = pcl_conversions::toPCL(msg_disp->header.stamp); + cloud->width = point_counter; - sensor_msgs::PointCloud2 cloud_PC2; - pcl::toROSMsg(*cloud,cloud_PC2); - expansion_cloud_pub.publish(cloud_PC2); + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); + + sensor_msgs::msg::PointCloud2 cloud_PC2; + pcl::toROSMsg(*cloud, cloud_PC2); + expansion_cloud_pub->publish(cloud_PC2); } } } return; } -disparity_expansion::disparity_expansion(ros::NodeHandle& nh){ - expansion_cloud_pub = nh.advertise("/expansion/cloud_fg", 10); - expansion_poly_pub = nh.advertise("/expansion/occ_marker", 10); - expanded_disparity_fg_pub = nh.advertise("/narrow_stereo/left/expanded_disparity_fg", 10); - expanded_disparity_bg_pub = nh.advertise("/narrow_stereo/left/expanded_disparity_bg", 10); - ROS_INFO("into constr with nodehandle %s",nh.getNamespace().c_str()); +DisparityExpansionNode::DisparityExpansionNode(const rclcpp::NodeOptions& options) + : Node("DisparityExpansionNode", options) { + this->declare_parameter("expansion_cloud_topic", "expansion_cloud"); + this->declare_parameter("expanded_disparity_fg_topic", "expanded_disparity_fg"); + this->declare_parameter("expanded_disparity_bg_topic", "expanded_disparity_bg"); + this->declare_parameter("expansion_poly_topic", "expansion_poly"); + this->declare_parameter("camera_info_topic", "camera_info"); + this->declare_parameter("disparity_topic", "disparity"); + this->declare_parameter("depth_topic", "depth"); + this->declare_parameter("scale", 2.0); + this->declare_parameter("robot_radius", 2.0); + this->declare_parameter("lut_max_disparity", 164); + this->declare_parameter("padding", 2.0); + this->declare_parameter("bg_multiplier", 5.0); + this->declare_parameter("sensor_pixel_error", 0.5); + this->declare_parameter("downsample_scale", 2.0); + + std::string expansion_cloud_topic = this->get_parameter("expansion_cloud_topic").as_string(); + // log to console + RCLCPP_INFO(this->get_logger(), "expansion_cloud_topic: %s", expansion_cloud_topic.c_str()); + std::string expanded_disparity_fg_topic = + this->get_parameter("expanded_disparity_fg_topic").as_string(); + RCLCPP_INFO(this->get_logger(), "expanded_disparity_fg_topic: %s", + expanded_disparity_fg_topic.c_str()); + std::string expanded_disparity_bg_topic = + this->get_parameter("expanded_disparity_bg_topic").as_string(); + RCLCPP_INFO(this->get_logger(), "expanded_disparity_bg_topic: %s", + expanded_disparity_bg_topic.c_str()); + std::string expansion_poly_topic = this->get_parameter("expansion_poly_topic").as_string(); + std::string camera_info_topic = this->get_parameter("camera_info_topic").as_string(); + RCLCPP_INFO(this->get_logger(), "camera_info_topic: %s", camera_info_topic.c_str()); + std::string disparity_topic = this->get_parameter("disparity_topic").as_string(); + std::string depth_topic = this->get_parameter("depth_topic").as_string(); + RCLCPP_INFO(this->get_logger(), "depth_topic: %s", depth_topic.c_str()); + scale_ = this->get_parameter("scale").as_double(); + downsample_scale = this->get_parameter("downsample_scale").as_double(); + this->get_parameter("robot_radius", robot_radius_); + this->get_parameter("lut_max_disparity", lut_max_disparity_); + this->get_parameter("padding", padding_); + this->get_parameter("bg_multiplier", bg_multiplier_); + this->get_parameter("sensor_pixel_error", pixel_error_); + + expansion_cloud_pub = + this->create_publisher(expansion_cloud_topic, 10); + expansion_poly_pub = + this->create_publisher(expansion_poly_topic, 10); + expanded_disparity_fg_pub = + this->create_publisher(expanded_disparity_fg_topic, 10); + expanded_disparity_bg_pub = + this->create_publisher(expanded_disparity_bg_topic, 10); + + RCLCPP_INFO(this->get_logger(), "into constr with node name %s", this->get_name()); LUT_ready = false; got_cam_info = false; - cam_info_sub_ = nh.subscribe("/narrow_stereo/right/camera_info", 1,&disparity_expansion::getCamInfo,this); - disparity_sub_ = nh.subscribe("/narrow_stereo/disparity", 1,&disparity_expansion::stereoDisparityCb,this); - - nh.param("robot_radius", robot_radius_, 2.0); - nh.param("lut_max_disparity", lut_max_disparity_, 164); - nh.param("padding", padding_, 2.0); - nh.param("bg_multiplier", bg_multiplier_, 5.0); - nh.param("sensor_pixel_error", pixel_error_, 0.5); - -// Q-Matrix -// [1.0 0.0 0.0 -317.20617294311523, -// 0.0 1.0 0.0 -233.2914752960205, -// 0.0 0.0 0.0 307.4838344732113, -// 0.0 0.0 1.7930881143612616 -0.0] - - cx_ = 317.20617294311523; - cy_ = 233.2914752960205; - fx_ = fy_ = 307.4838344732113; - baseline = 0.5576007548439765; - width = 640; - height = 480; - downsample_scale = 2.0; -// generateExpansionLUT(); + + cam_info_sub_ = this->create_subscription( + camera_info_topic, 1, + std::bind(&DisparityExpansionNode::getCamInfo, this, std::placeholders::_1)); + + depth_sub_ = this->create_subscription( + depth_topic, 1, + std::bind(&DisparityExpansionNode::depthImageCb, this, std::placeholders::_1)); } -int main(int argc, char** argv) -{ - ros::init(argc, argv, "disparity_expansion"); - // cv::initModule_nonfree();//THIS LINE IS IMPORTANT for using surf and sift features of opencv - ros::NodeHandle nh("~"); - disparity_expansion d(nh); - ros::spin(); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); return 0; } diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion_working.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion_working.cpp deleted file mode 100644 index ffad0131a..000000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_expansion_working.cpp +++ /dev/null @@ -1,494 +0,0 @@ -/* -* Copyright (c) 2016 Carnegie Mellon University, Author -* -* For License information please see the LICENSE file in the root directory. -* -*/ - -//Outputs an image in the form of a point cloud in 3d space. -#include "ros/ros.h" -#include "message_filters/subscriber.h" -#include -#include "sensor_msgs/Image.h" -#include "sensor_msgs/CameraInfo.h" -#include "std_msgs/Header.h" -#include -#include "opencv2/core/core.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define SCALE 2.0 // num_units/pixel -#define LUT_MAX_DISPARITY 104 // units -#define ROB_RADIUS 1.0 // in meters -//for 2times Robsize = 0.0173 at 40m -//15 = 0.0677 -#define DEPTH_ERROR_COEFF 0.0177 - -typedef pcl::PointCloud PointCloud; - -class disparity_expansion{ -public: - disparity_expansion(ros::NodeHandle& nh); - - ros::Publisher expansion_cloud_pub; - ros::Publisher expanded_disparity_fg_pub; - ros::Publisher expanded_disparity_bg_pub; - ros::Subscriber subs_; - ros::Subscriber cam_info_sub_; - ros::Subscriber disparity_sub_; - -// message_filters::Subscriber disp_sub; -// message_filters::Subscriber depth_sub; -// message_filters::Subscriber image_sub; -// message_filters::Subscriber info_sub; -// typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; -// typedef message_filters::sync_policies::ApproximateTime MySyncPolicy2; -// message_filters::Synchronizer* sync; -// message_filters::Synchronizer* sync2; - image_geometry::PinholeCameraModel model_; - sensor_msgs::Image dimage; - float baseline; - bool LUT_ready; - bool got_cam_info; - - void getCamInfo(const sensor_msgs::CameraInfo::ConstPtr &msg_info); - void stereoDisparityCb(const stereo_msgs::DisparityImage::ConstPtr &msg_disp); - void generateExpansionLUT(); - - struct cell - { - unsigned int idx1; - unsigned int idx2; - }; - cell table_u[LUT_MAX_DISPARITY][640];//[640]; - cell table_v[LUT_MAX_DISPARITY][480];//[512]; - float table_d[LUT_MAX_DISPARITY]; -}; - -void disparity_expansion::generateExpansionLUT() -{ - bool debug = false; - if(debug) - { - ROS_WARN("Expansion LUT Debug disabled creation"); - LUT_ready = true; - } - if(LUT_ready) - { - ROS_ERROR("LUT all ready"); -// sleep(1); - return; - } - float center_x = model_.cx(); - float center_y = model_.cy(); - float constant_x = 1.0 / model_.fx(); - float constant_y = 1.0 / model_.fy(); - ROS_INFO("Fx Fy Cx Cy: %f %f , %f %f",model_.fx(),model_.fy(),model_.cx(),model_.cy()); - double r = ROB_RADIUS;//expansion radius in cm - int u1,u2,v1,v2; - double x,y,z; - double disparity; - - - for(unsigned int disp_idx = 1;disp_idx<= LUT_MAX_DISPARITY ;disp_idx++) - { -// z = depth*0.01;//cm to m - disparity = disp_idx/SCALE;//1 cell = 0.5m, z is in meters - r = ROB_RADIUS;// * exp(DEPTH_ERROR_COEFF*z); - z = baseline * model_.fx() / disparity; - - float disp_new = baseline * model_.fx()/(z - ROB_RADIUS) + 0.5; - table_d[disp_idx] = disp_new ; - ROS_INFO("REAL EXPANDED: %f , %f",disparity,table_d[disp_idx]); - - - for ( int v = ( int ) dimage.height - 1; v >= 0; --v ) - { - y = ( v - center_y ) * z * constant_y; - - double beta = atan2(z,y); - double beta1 = asin(r/sqrt(z*z + y*y)); - - double r1 = z/tan(beta+beta1); - double r2 = z/tan(beta-beta1); - v1 = model_.fy()*r1/z + center_y; - v2 = model_.fy()*r2/z + center_y; - - if((v2-v1)<0) - ROS_ERROR("Something MESSED %d %d %d",v1,v2,disp_idx); - - if(v1 < 0) v1 = 0; - if(v1 > (dimage.height-1)) v1 = dimage.height-1; - - if(v2 < 0) v2 = 0; - if(v2 > (dimage.height-1)) v2 = dimage.height-1; - - table_v[disp_idx][v].idx1 = v1 ; - table_v[disp_idx][v].idx2 = v2 ; -// ROS_ERROR("d v1 v2 %d %d \t %d",z,v1,v2); -// ROS_ERROR("d beta beta1 %f %f \t %f",z,beta*180/M_PI, beta1*180/M_PI); - } - - for ( int u = ( int ) dimage.width - 1; u >= 0; --u ) - { - x = ( u - center_x ) * z * constant_x; - - double alpha = atan2(z,x); - double alpha1 = asin(r/sqrt(z*z + x*x)); - - double r1 = z/tan(alpha+alpha1); - double r2 = z/tan(alpha-alpha1); - u1 = model_.fx()*r1/z + center_x; - u2 = model_.fx()*r2/z + center_x; - - if((u2-u1)<0) - ROS_ERROR("Something MESSED %d %d %d",u1,u2,disp_idx); - - if(u1 < 0) u1 = 0; - if(u1 > (dimage.width-1)) u1 = dimage.width-1; - - if(u2 < 0) u2 = 0; - if(u2 > (dimage.width-1)) u2 = dimage.width-1; - - table_u[disp_idx][u].idx1 = u1 ; - table_u[disp_idx][u].idx2 = u2 ; -// ROS_ERROR("d u1 u2 %d %d \t %d",u1,u2,depth); - - } - } - ROS_WARN("Expansion LUT created"); - for(int i =1;i<=LUT_MAX_DISPARITY;i++) - { - disparity = i/SCALE;//1 cell = 0.5m, z is in meters - r = ROB_RADIUS;// * exp(DEPTH_ERROR_COEFF*z); - z = baseline * model_.fx() / disparity; - - float disp_new = baseline * model_.fx()/(z - ROB_RADIUS) + 0.5; - table_d[i] = disp_new ; - ROS_INFO("expanded disparity: %f %d",table_d[i],table_u[i][1].idx1); - } - LUT_ready = true; -// exit(0); -} - - -void disparity_expansion::getCamInfo(const sensor_msgs::CameraInfo::ConstPtr& msg_info) -{ - model_.fromCameraInfo ( msg_info ); - ROS_INFO_ONCE("Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f",model_.fx(),model_.fy(),model_.cx(),model_.cy()); - got_cam_info = true; -} - -void disparity_expansion::stereoDisparityCb(const stereo_msgs::DisparityImage::ConstPtr &msg_disp){ - ROS_INFO("Disparity CB"); - - - - - if(!got_cam_info) - return; - dimage = msg_disp->image; - // Update camera model - baseline = msg_disp->T; - - if(!LUT_ready) - generateExpansionLUT(); - - cv_bridge::CvImagePtr fg_msg; - cv_bridge::CvImagePtr bg_msg; - cv_bridge::CvImagePtr free_msg(new cv_bridge::CvImage()); - cv_bridge::CvImagePtr temp_msg; - cv_bridge::CvImagePtr di_msg; - - //DEBUG READ FROM FILE - temp_msg = cv_bridge::toCvCopy(msg_disp->image, sensor_msgs::image_encodings::TYPE_32FC1); - temp_msg->image = temp_msg->image *0; - temp_msg->image(cv::Rect(0 ,0, temp_msg->image.cols/2, temp_msg->image.rows/2)) = baseline * model_.fx()/10.0 ;//TL QUAD - temp_msg->image(cv::Rect(temp_msg->image.cols/2, 0,temp_msg->image.cols/2-1, temp_msg->image.rows/2-1)) = baseline * model_.fx()/15.0 ;//BR QUAD - temp_msg->image(cv::Rect(temp_msg->image.cols/2, temp_msg->image.rows/2,temp_msg->image.cols/2-1, temp_msg->image.rows/2-1)) = baseline * model_.fx()/55.0 ;//BR QUAD -// temp_msg->image(cv::Rect(dimage.width/2, dimage.height/2,dimage.width-1, dimage.height-1)) = baseline * model_.fx()/15.0 ; - -// temp_msg->toImageMsg(dimage);//UNCOMMENT TO DEBUG - - - - -// const sensor_msgs::ImageConstPtr my_ptr = &dimage; - cv::Mat dummy(msg_disp->image.height,msg_disp->image.width,CV_32FC4); - dummy.copyTo(free_msg->image); -// free_msg->image.zeros(msg_disp->image.height,msg_disp->image.width,CV_32FC4); - free_msg->header = dimage.header; - free_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC4; - try - { - fg_msg = cv_bridge::toCvCopy(dimage, sensor_msgs::image_encodings::TYPE_32FC1); - bg_msg = cv_bridge::toCvCopy(dimage, sensor_msgs::image_encodings::TYPE_32FC1); - di_msg = cv_bridge::toCvCopy(dimage, sensor_msgs::image_encodings::TYPE_32FC1); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - - -// cv_bridge::CvImage cv_image; -// try{ -// cv_image.image = cv::imread("/home/aeroscout/gd_mav/src/ompl_planner3D/launch/img/DispImage.jpeg",CV_LOAD_IMAGE_GRAYSCALE); -// } -// catch (cv::Exception& e) -// { -// ROS_ERROR("cv_bridge exception: %s", e.what()); -// return; -// } -//// cv_image.encoding = "bgr8"; - -// cv::Mat test; -//// test = fg_msg->image; -//// cv::imshow("OPENCV_WINDOW", cv_image.image); -//// cv::waitKey(3); -// test = cv::imread("/home/aeroscout/gd_mav/src/ompl_planner3D/launch/img/DispImage.jpeg", 0); - - - fg_msg->header = dimage.header; - fg_msg->encoding = dimage.encoding;; - bg_msg->header = dimage.header; - bg_msg->encoding = dimage.encoding; - - bg_msg->image = bg_msg->image + INFINITY; - fg_msg->image = fg_msg->image - INFINITY; - if(1)//make cloud - { - // Use correct principal point from calibration - float center_x = model_.cx(); - float center_y = model_.cy(); - - float constant_x = 1.0 / model_.fx(); - float constant_y = 1.0 / model_.fy(); - - if ( dimage.encoding == sensor_msgs::image_encodings::TYPE_32FC1 ) - { - ROS_INFO_ONCE("IMG TYPE 32FC, GOOD"); - int row_step = dimage.step / sizeof ( float ); - const float* disparity_row = reinterpret_cast ( &dimage.data[0] ); - -// cv::Mat cv_exp_disp_foregnd_mat(dimage.height,dimage.width,CV_32F,cv::Scalar(0)); -// cv::Mat cv_exp_disp_backgnd_mat(dimage.height,dimage.width,CV_32F,cv::Scalar(9999)); - for ( int v = ( int ) dimage.height - 1; (v >= 0) ; v-=10 ) - { - for ( int u = ( int ) dimage.width - 1; u >= 0; u-=10 ) - { - float disparity_value = disparity_row[v * row_step + u] + 0.5; - if(!isnan(double(disparity_value)) && ((int(disparity_value*SCALE)+1) < LUT_MAX_DISPARITY) && ((int(disparity_value*SCALE)+1) > 0))//expand - { - unsigned int u1 = table_u[int(disparity_value*SCALE)+1][u].idx1; - unsigned int u2 = table_u[int(disparity_value*SCALE)+1][u].idx2; - - unsigned int v1 = table_v[int(disparity_value*SCALE)+1][v].idx1; - unsigned int v2 = table_v[int(disparity_value*SCALE)+1][v].idx2; - - if(v1 <0 || u1 <0 || v2 >=dimage.height || u2>=dimage.width || (u2-u1)<0 || (v2-v1)<0) - { - ROS_ERROR("Expansion out of img bounds with disparity: %f",disparity_value); - ROS_ERROR("u1 u2 v1 v2 %d %d \t %d %d",u1,u2,v1,v2); - continue; - } - cv::Rect roi = cv::Rect(u1,v1,(u2-u1),(v2-v1)); - cv::Mat submat = di_msg->image(roi).clone(); - double min, max; - cv::minMaxLoc(di_msg->image(roi), &min, &max); - float disp_new_fg = table_d[int(max*SCALE)]; - float disp_new_bg = table_d[int(min*SCALE)]; - float disp_to_depth = baseline * model_.fx()/max; - disp_new_fg = baseline * model_.fx()/(disp_to_depth - ROB_RADIUS) /*+ 0.5*/; -// disp_to_depth = baseline * model_.fx()/min; - disp_new_bg = baseline * model_.fx()/(disp_to_depth + ROB_RADIUS) /*- 0.5*/; - submat.setTo(disp_new_fg); -// submat = cv::max(fg_msg->image(roi),submat); - cv::Mat tMat(fg_msg->image(roi)); - submat.copyTo(tMat); - tMat = (bg_msg->image(roi)); - submat.setTo(disp_new_bg); - submat.copyTo(tMat); - -// v -= abs(v2-v1)/4; - u -= abs(u2-u1)/2; -// u -= (u-u1)/(max-min)+1; - - /* - for(int i = v1;i<=v2;i++) - for(int j = u1; j<=u2;j++) - { - float old_fg,old_bg; - old_fg = fg_msg->image.at(i,j); - old_bg = bg_msg->image.at(i,j); - float disp_to_depth = baseline * model_.fx()/disparity_value ; - float disp_new_fg = baseline * model_.fx()/(disp_to_depth - ROB_RADIUS) + 0.5; - if((disp_new_fg) > fg_msg->image.at(i,j)) - { - fg_msg->image.at(i,j) = disp_new_fg; - free_msg->image.at(i,j)[0] = disp_new_fg; - } - - float disp_new_bg = baseline * model_.fx()/(disp_to_depth + ROB_RADIUS) - 0.5; - if((disp_new_bg) < bg_msg->image.at(i,j)) - { - bg_msg->image.at(i,j) = disp_new_bg; - free_msg->image.at(i,j)[1] = disp_new_bg; - } - - if(disp_new_fg < old_bg && disparity_value != 0 && old_bg < 500) - { - if(free_msg->image.at(i,j)[2] < old_bg) - free_msg->image.at(i,j)[2] = old_bg-1; - if(free_msg->image.at(i,j)[3] < disp_new_fg) - free_msg->image.at(i,j)[3] = disp_new_fg+1; - } - - if(disp_new_bg > old_fg && disparity_value != 0 && old_bg < 500) - { - if(free_msg->image.at(i,j)[2] < disp_new_bg) - free_msg->image.at(i,j)[2] = disp_new_bg-1; - if(free_msg->image.at(i,j)[3] < old_fg) - free_msg->image.at(i,j)[3] = old_fg+1; - } - } - */ - } - else - ROS_ERROR_THROTTLE(1,"BAD disparity during expansion: %f",disparity_value); - } - } - -// double min, max; -// cv::minMaxLoc(fg_msg->image, &min, &max); -// ROS_INFO("FG minmax: \t%f \t%f",min,max); -// cv::minMaxLoc(bg_msg->image, &min, &max); -// ROS_INFO("BG minmax: \t%f \t%f",min,max); - - expanded_disparity_fg_pub.publish(fg_msg->toImageMsg()); - expanded_disparity_bg_pub.publish(bg_msg->toImageMsg()); - - if(expansion_cloud_pub.getNumSubscribers() > 0 )//create expansion PCD - { - PointCloud::Ptr cloud (new PointCloud); - cloud->header.frame_id = msg_disp->header.frame_id; - cloud->height = 1; - cloud->width =1; - cloud->is_dense = false; - int point_counter=0; - pcl::PointXYZI pt_fg,pt_bg,pt_free1,pt_free2; - for ( int v = ( int ) dimage.height - 1; v >= 0; v-=4 ) - { - for ( int u = ( int ) dimage.width - 1; u >= 0; u-=4 ) - { - - // Fill in XYZ - float depth_value = msg_disp->T * model_.fx() / fg_msg->image.at(v,u);//free_msg->image.at(v,u)[0]; - pt_fg.x = ( u - center_x ) * depth_value * constant_x; - pt_fg.y = ( v - center_y ) * depth_value * constant_y; - pt_fg.z = depth_value; - pt_fg.intensity = 220; - - depth_value = msg_disp->T * model_.fx() / bg_msg->image.at(v,u);//free_msg->image.at(v,u)[1]; - pt_bg.x = ( u - center_x ) * depth_value * constant_x; - pt_bg.y = ( v - center_y ) * depth_value * constant_y; - pt_bg.z = depth_value; - pt_bg.intensity = 120; - - depth_value = msg_disp->T * model_.fx() / free_msg->image.at(v,u)[2]; - pt_free1.x = ( u - center_x ) * depth_value * constant_x; - pt_free1.y = ( v - center_y ) * depth_value * constant_y; - pt_free1.z = depth_value; - pt_free1.intensity = 170; - depth_value = msg_disp->T * model_.fx() / free_msg->image.at(v,u)[3]; - pt_free2.x = ( u - center_x ) * depth_value * constant_x; - pt_free2.y = ( v - center_y ) * depth_value * constant_y; - pt_free2.z = depth_value; - pt_free2.intensity = 170; - - if(fg_msg->image.at(v,u) >= 0.1 && bg_msg->image.at(v,u) >= 0.1 ) - { - point_counter++; - cloud->points.push_back ( pt_fg ); - point_counter++; - cloud->points.push_back ( pt_bg ); - -// point_counter++; -// cloud->points.push_back ( pt_free1 ); -// point_counter++; -// cloud->points.push_back ( pt_free2 ); - - } - } - } - cloud->width = point_counter; - cloud->header.seq = msg_disp->header.seq; - cloud->header.stamp = pcl_conversions::toPCL(msg_disp->header.stamp); - - sensor_msgs::PointCloud2 cloud_PC2; - pcl::toROSMsg(*cloud,cloud_PC2); - expansion_cloud_pub.publish(cloud_PC2); - } - } - } - return; -} - -disparity_expansion::disparity_expansion(ros::NodeHandle& nh){ - //the depth image stream and color image stream have different framerates and therefore are not aligned. So we use a time synchronizer message filter (http://www.ros.org/wiki/message_filters) -// depth_sub.subscribe(nh,"/camera/depth/image", 1); -// image_sub.subscribe(nh, "/camera/rgb/image_rect_color", 1); -// info_sub.subscribe(nh, "/camera/depth/camera_info", 1); - -// disp_sub.subscribe(nh, "/ueye_resized/disparity", 1); -// image_sub.subscribe(nh, "/ueye_resized/left/image_rect_color", 1); -// info_sub.subscribe(nh, "/ueye_resized/left/camera_info", 1); - -// disp_sub.subscribe(nh, "/ceye/disparity", 1); -// image_sub.subscribe(nh, "/ceye/left/image_rect_color", 1); -// info_sub.subscribe(nh, "/ceye/left/camera_info", 1); - -// sync = new message_filters::Synchronizer(2); -// sync->connectInput(depth_sub,image_sub, info_sub); -// sync->registerCallback(boost::bind(&depth_proc::callback,this, _1, _2,_3)); - -// sync2 = new message_filters::Synchronizer(2); -// sync2->connectInput(disp_sub,image_sub, info_sub); -// sync2->registerCallback(boost::bind(&disparity_expansion::stereoDisparityCb,this, _1, _2,_3)); - expansion_cloud_pub = nh.advertise("/expansion/cloud_fg", 10); - expanded_disparity_fg_pub = nh.advertise("/ceye/left/expanded_disparity_fg", 10); - expanded_disparity_bg_pub = nh.advertise("/ceye/left/expanded_disparity_bg", 10); - ROS_INFO("into constr"); - LUT_ready = false; - got_cam_info = false; - - -// cam_info_sub_ = nh.subscribe("/ceye/left/camera_info", 1,&disparity_expansion::getCamInfo,this); -// disparity_sub_ = nh.subscribe("/ceye/disparity", 1,&disparity_expansion::stereoDisparityCb,this); - cam_info_sub_ = nh.subscribe("/camera/depth/camera_info", 1,&disparity_expansion::getCamInfo,this); - disparity_sub_ = nh.subscribe("/camera/disparity/image_raw", 1,&disparity_expansion::stereoDisparityCb,this); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "disparity_expansion"); - // cv::initModule_nonfree();//THIS LINE IS IMPORTANT for using surf and sift features of opencv - ros::NodeHandle nh; - disparity_expansion d(nh); - ros::spin(); - return 0; -} - diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_pcd.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_pcd.cpp index 85d6f4184..29d6dbe77 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_pcd.cpp +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_expansion/src/disparity_pcd.cpp @@ -6,6 +6,7 @@ */ //Outputs a point cloud using disparity images. +/* #include "ros/ros.h" #include "sensor_msgs/Image.h" #include "sensor_msgs/CameraInfo.h" @@ -24,48 +25,84 @@ #include #include #include +*/ + +#include +#include +#include +#include +//#include +#include +#include + +#include "opencv2/core/core.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/header.hpp" +#include +#include +#include +#include + +//#include +//#include +//#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include typedef pcl::PointCloud PointCloud; -class disparity_pcd{ +class disparity_pcd : public rclcpp::Node +{ public: - disparity_pcd(ros::NodeHandle& nh); - ros::Subscriber cam_info_sub_; - ros::Subscriber disparity_sub_; - ros::Publisher disparity_pcd_pub_; + disparity_pcd(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + //ros::Subscriber cam_info_sub_; + //ros::Subscriber disparity_sub_; + //ros::Publisher disparity_pcd_pub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr disparity_sub_; + rclcpp::Publisher::SharedPtr disparity_pcd_pub_; double baseline; double downsample_scale; bool got_cam_info; - void stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_disp); - void callback(const sensor_msgs::Image::ConstPtr &msg_disp, const sensor_msgs::Image::ConstPtr &msg_image); - void getCamInfo(const sensor_msgs::CameraInfo::ConstPtr &msg_info); + void stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp); + void callback(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp, const sensor_msgs::msg::Image::ConstSharedPtr msg_image); + void getCamInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg_info); double fx_,fy_,cx_,cy_; unsigned int width,height; }; -void disparity_pcd::getCamInfo(const sensor_msgs::CameraInfo::ConstPtr& msg_info) +void disparity_pcd::getCamInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg_info) { if(got_cam_info) return; image_geometry::PinholeCameraModel model_;model_.fromCameraInfo ( msg_info ); - ROS_INFO_ONCE("Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f",model_.fx(),model_.fy(),model_.cx(),model_.cy()); + RCLCPP_INFO_ONCE(this->get_logger(), "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f",model_.fx(),model_.fy(),model_.cx(),model_.cy()); cx_ = model_.cx()/downsample_scale; cy_ = model_.cy()/downsample_scale; fx_ = fy_ = model_.fx()/downsample_scale; width = msg_info->width/downsample_scale; height = msg_info->height/downsample_scale; - baseline = -msg_info->P[3]/msg_info->P[0]; + baseline = -msg_info->p[3]/msg_info->p[0]; baseline *=downsample_scale; got_cam_info = true; } -void disparity_pcd::stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_disp){ - ROS_INFO_ONCE("Disparity CB"); +void disparity_pcd::stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp){ + RCLCPP_INFO_ONCE(this->get_logger(),"Disparity CB"); - ros::Time start = ros::Time::now(); + rclcpp::Time start = this->get_clock()->now(); cv_bridge::CvImageConstPtr cv_ptrdisparity; cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); @@ -78,11 +115,11 @@ void disparity_pcd::stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_di } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } - if(disparity_pcd_pub_.getNumSubscribers() > 0 )//create expansion PCD + if(disparity_pcd_pub_->get_subscription_count() > 0 )//create expansion PCD { PointCloud::Ptr cloud (new PointCloud); cloud->header.frame_id = msg_disp->header.frame_id; @@ -112,28 +149,30 @@ void disparity_pcd::stereoDisparityCb(const sensor_msgs::Image::ConstPtr &msg_di } } cloud->width = point_counter; - cloud->header.seq = msg_disp->header.seq; - cloud->header.stamp = pcl_conversions::toPCL(msg_disp->header.stamp); + //cloud->header.seq = msg_disp->header.seq; + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); + - sensor_msgs::PointCloud2 cloud_PC2; + sensor_msgs::msg::PointCloud2 cloud_PC2; pcl::toROSMsg(*cloud,cloud_PC2); - disparity_pcd_pub_.publish(cloud_PC2); + disparity_pcd_pub_->publish(cloud_PC2); } // ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); return; } -void disparity_pcd::callback(const sensor_msgs::Image::ConstPtr &msg_disp, const sensor_msgs::Image::ConstPtr &msg_image) +void disparity_pcd::callback(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp, const sensor_msgs::msg::Image::ConstSharedPtr msg_image) { if(!got_cam_info) { - ROS_INFO_THROTTLE(1,"Cam Info not received, not processing"); + RCLCPP_INFO_THROTTLE(this->get_logger(),*this->get_clock(),1,"Cam Info not received, not processing"); return; } - ROS_INFO_ONCE("Disparity CB"); + RCLCPP_INFO_ONCE(this->get_logger(),"Disparity CB"); - ros::Time start = ros::Time::now(); + rclcpp::Time start = this->get_clock()->now(); cv_bridge::CvImageConstPtr cv_ptrdisparity; cv_bridge::CvImageConstPtr cv_ptrImage; cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); @@ -151,11 +190,11 @@ void disparity_pcd::callback(const sensor_msgs::Image::ConstPtr &msg_disp, const } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(),"cv_bridge exception: %s", e.what()); return; } - if(disparity_pcd_pub_.getNumSubscribers() > 0 )//create expansion PCD + if(disparity_pcd_pub_->get_subscription_count() > 0 )//create expansion PCD { PointCloud::Ptr cloud (new PointCloud); cloud->header.frame_id = msg_disp->header.frame_id; @@ -185,23 +224,33 @@ void disparity_pcd::callback(const sensor_msgs::Image::ConstPtr &msg_disp, const } } cloud->width = point_counter; - cloud->header.seq = msg_disp->header.seq; - cloud->header.stamp = pcl_conversions::toPCL(msg_disp->header.stamp); + //cloud->header.seq = msg_disp->header.seq; + //cloud->header.stamp = pcl_conversions::toPCL(msg_disp->header.stamp); + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); - sensor_msgs::PointCloud2 cloud_PC2; + sensor_msgs::msg::PointCloud2 cloud_PC2; pcl::toROSMsg(*cloud,cloud_PC2); - disparity_pcd_pub_.publish(cloud_PC2); + disparity_pcd_pub_->publish(cloud_PC2); } // ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); return; } -disparity_pcd::disparity_pcd(ros::NodeHandle& nh){ - disparity_pcd_pub_ = nh.advertise("/pcd", 10); - cam_info_sub_ = nh.subscribe("/nerian_sp1/right/camera_info", 1,&disparity_pcd::getCamInfo,this); - nh.param("downsample_scale", downsample_scale, 1.0); - ROS_INFO("into constr"); +disparity_pcd::disparity_pcd(const rclcpp::NodeOptions & options) +: Node("disparity_pcd", options) +{ + disparity_pcd_pub_ = this->create_publisher("/pcd", 10); + //cam_info_sub_ = nh.subscribe("/nerian_sp1/right/camera_info", 1,&disparity_pcd::getCamInfo,this); + cam_info_sub_ = this->create_subscription("/nerian_sp1/right/camera_info", 10, std::bind(&disparity_pcd::getCamInfo, this, std::placeholders::_1)); + + //nh.param("downsample_scale", downsample_scale, 1.0); + //ROS_INFO("into constr"); + this->declare_parameter("downsample_scale", 1.0); + this->get_parameter("downsample_scale", downsample_scale); + + RCLCPP_INFO(this->get_logger(), "into constr"); // disparity_sub_ = nh.subscribe("/disparity", 1,&disparity_pcd::stereoDisparityCb,this); // Q-Matrix @@ -218,18 +267,43 @@ disparity_pcd::disparity_pcd(ros::NodeHandle& nh){ got_cam_info = false; // width = 640; // height = 480; + +// auto disp_sub_ = std::make_shared>(node.get(), "/disparity", 10); +// auto image_sub_ = std::make_shared>(shared_from_this(), "/nerian_sp1/left/image_raw", 10); + +// auto sync = std::make_shared>(*disp_sub_, *image_sub_, 10); +// sync->registerCallback(std::bind(&disparity_pcd::callback, this, std::placeholders::_1, std::placeholders::_2)); } int main(int argc, char** argv) { - ros::init(argc, argv, "disparity_pcd"); + rclcpp::init(argc, argv); // cv::initModule_nonfree();//THIS LINE IS IMPORTANT for using surf and sift features of opencv - ros::NodeHandle nh("~"); - disparity_pcd d(nh); - message_filters::Subscriber disp_sub(nh, "/disparity", 1); - message_filters::Subscriber image_sub(nh, "/nerian_sp1/left/image_raw", 1); - message_filters::TimeSynchronizer sync(disp_sub, image_sub, 10); - sync.registerCallback(boost::bind(&disparity_pcd::callback,&d, _1, _2)); - ros::spin(); + //ros::NodeHandle nh("~"); + auto node = std::make_shared(rclcpp::NodeOptions()); + + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + auto disp_sub = std::make_shared>( + node.get(), "/nerian_sp1/disparity_map", rmw_qos_profile_default); + auto image_sub = std::make_shared>( + node.get(), "/nerian_sp1/left/image_raw", rmw_qos_profile_default); + + auto sync = std::make_shared>(SyncPolicy(10), + *disp_sub, *image_sub); + + sync->registerCallback( + std::bind(&disparity_pcd::callback, node, std::placeholders::_1, std::placeholders::_2)); + + + //disparity_pcd d(nh); + //message_filters::Subscriber disp_sub(nh, "/disparity", 1); + //message_filters::Subscriber image_sub(nh, "/nerian_sp1/left/image_raw", 1); + //message_filters::TimeSynchronizer sync(disp_sub, image_sub, 10); + //sync.registerCallback(boost::bind(&disparity_pcd::callback,&d, _1, _2)); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; } diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/CMakeLists.txt new file mode 100644 index 000000000..85503b6be --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.8) +project(disparity_graph) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_geometry REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED) + +add_library(disparity_graph src/disparity_graph.cpp) +target_compile_features(disparity_graph PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +target_include_directories(disparity_graph PUBLIC + $ + $) + +ament_target_dependencies( + disparity_graph + "rclcpp" + "sensor_msgs" + "std_msgs" + "tf2" + "tf2_ros" + "geometry_msgs" + "cv_bridge" + "image_geometry" + "nav_msgs" + "image_transport" + "OpenCV" + "visualization_msgs" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(disparity_graph PRIVATE "DISPARITY_GRAPH_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS disparity_graph + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + disparity_graph +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp new file mode 100644 index 000000000..ce577172c --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp @@ -0,0 +1,350 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace disparity_graph { + +class DisparityGraph : rclcpp::Node { + struct DisparityGraphNode { + cv_bridge::CvImagePtr Im_fg; + cv_bridge::CvImagePtr Im_bg; + std_msgs::msg::Header header; + tf2::Transform w2s_tf; + tf2::Transform s2w_tf; + }; + std::deque disp_graph_; + + size_t graph_size_; + double thresh_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + std::string sensor_frame_, fixed_frame_, stabilized_frame_; + visualization_msgs::msg::Marker marker_; + rclcpp::Publisher::SharedPtr disparity_graph_marker_pub_; + double angle_tol, displacement_tol; + bool first; + bool got_cam_info; + double fx_, fy_, cx_, cy_, baseline_, downsample_scale; + unsigned int width_, height_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + + std::mutex io_mutex; + + message_filters::Subscriber disp_fg_sub_, disp_bg_sub_; + typedef message_filters::sync_policies::ExactTime + ExactPolicy; + typedef message_filters::Synchronizer ExactSync; + + std::shared_ptr exact_sync; + + public: + DisparityGraph() + : Node("disparity_graph"), + graph_size_(10), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + fixed_frame_("world"), + stabilized_frame_("base_frame_stabilized"), + angle_tol(30.0), + displacement_tol(1.5), + first(true), + got_cam_info(false) { + disparity_graph_marker_pub_ = + create_publisher("disparity_marker", 10); + marker_.header.frame_id = fixed_frame_; + marker_.header.stamp = this->get_clock()->now(); + marker_.ns = "disparityGraph"; + marker_.id = 0; + marker_.type = visualization_msgs::msg::Marker::ARROW; + marker_.action = visualization_msgs::msg::Marker::ADD; + marker_.pose.position.x = 1; + marker_.pose.position.y = 1; + marker_.pose.position.z = 1; + marker_.pose.orientation.x = 0.0; + marker_.pose.orientation.y = 0.0; + marker_.pose.orientation.z = 0.0; + marker_.pose.orientation.w = 1.0; + marker_.scale.x = 1; + marker_.scale.y = 0.1; + marker_.scale.z = 0.1; + marker_.color.a = 1.0; // Don't forget to set the alpha! + marker_.color.r = 0.0; + marker_.color.g = 1.0; + marker_.color.b = 0.0; + + declare_parameter("baseline", 0.10); + baseline_ = get_parameter("baseline").as_double(); + declare_parameter("downsample_scale", 1.0); + downsample_scale = get_parameter("downsample_scale").as_double(); + declare_parameter("low_occ_thresh", 0.9); + thresh_ = get_parameter("low_occ_thresh").as_double(); + declare_parameter("graph_size", 10); + graph_size_ = get_parameter("graph_size").as_int(); + declare_parameter("displacement_tolerance", 1.5); + displacement_tol = get_parameter("displacement_tolerance").as_double(); + declare_parameter("angle_tolerance", 30.0); + angle_tol = get_parameter("angle_tolerance").as_double() * M_PI / 180.0; + + disp_fg_sub_.subscribe(this, "/ceye/left/expanded_disparity_fg"); + disp_bg_sub_.subscribe(this, "/ceye/left/expanded_disparity_bg"); + + exact_sync.reset(new ExactSync(ExactPolicy(5), disp_fg_sub_, disp_bg_sub_)); + exact_sync->registerCallback(std::bind(&DisparityGraph::disp_cb, this, + std::placeholders::_1, std::placeholders::_2)); + + cam_info_sub_ = create_subscription( + "/nerian_sp1/right/camera_info", 1, + std::bind(&DisparityGraph::get_cam_info, this, std::placeholders::_1)); + } + + virtual ~DisparityGraph(); + + void disp_cb(const sensor_msgs::msg::Image::ConstSharedPtr &disp_fg, + const sensor_msgs::msg::Image::ConstSharedPtr &disp_bg) { + RCLCPP_INFO(this->get_logger(), "Recvd disp stamp: %lf", + (this->get_clock()->now() - disp_fg->header.stamp).seconds()); + sensor_frame_ = disp_fg->header.frame_id; + + tf2::Stamped transform; + geometry_msgs::msg::TransformStamped tf_msg; + try { + tf_msg = tf_buffer_.lookupTransform(sensor_frame_, fixed_frame_, disp_fg->header.stamp); + tf2::fromMsg(tf_msg, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "DG disp_cb %s", ex.what()); + return; + } + + std::scoped_lock lock(io_mutex); + + if (first) { + first = false; + DisparityGraphNode n; + try { + n.Im_fg = cv_bridge::toCvCopy(disp_fg, sensor_msgs::image_encodings::TYPE_32FC1); + n.Im_bg = cv_bridge::toCvCopy(disp_bg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (std::exception ex) { + RCLCPP_ERROR(this->get_logger(), "\n\n\n\n\n\t\t\tDEQUE ERR\n\n\n\n: %s", + ex.what()); + return; + } + + n.header = disp_bg->header; + n.w2s_tf = transform; + n.s2w_tf = transform.inverse(); + disp_graph_.push_front(n); + disp_graph_.push_back(n); + + } else { + tf2::Vector3 curr_p = transform.inverse().getOrigin(); + std::deque::iterator it, end; + it = disp_graph_.begin(); + end = disp_graph_.end() - 1; + tf2::Vector3 graph_p = it->s2w_tf.getOrigin(); + tf2Scalar diff_a = + tf2::angleShortestPath(transform.inverse().getRotation(), it->s2w_tf.getRotation()); + tf2Scalar diff_p = curr_p.distance(graph_p); + + DisparityGraphNode n; + try { + n.Im_fg = cv_bridge::toCvCopy(disp_fg, sensor_msgs::image_encodings::TYPE_32FC1); + n.Im_bg = cv_bridge::toCvCopy(disp_bg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (std::exception ex) { + RCLCPP_ERROR(this->get_logger(), "\n\n\n\n\n\t\t\tDEQUE ERR\n\n\n\n: %s", + ex.what()); + return; + } + + n.header = disp_bg->header; + n.w2s_tf = transform; + n.s2w_tf = transform.inverse(); + + if (fabs(diff_a) >= angle_tol || fabs(diff_p) > displacement_tol) { + RCLCPP_ERROR(this->get_logger(), "Adding new node size %d", + (int)disp_graph_.size()); + if (disp_graph_.size() >= graph_size_) { + disp_graph_.erase(end); + } + + if (disp_graph_.size() <= graph_size_) { + disp_graph_.push_front(n); + } + } + disp_graph_.pop_back(); + disp_graph_.push_back(n); + } + } + + void get_cam_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr &cam_info_msg) { + if (got_cam_info) { + return; + } + image_geometry::PinholeCameraModel model; + model.fromCameraInfo(cam_info_msg); + // print + RCLCPP_INFO(this->get_logger(), "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f Baseline: %f", + model.fx(), model.fy(), model.cx(), model.cy(), baseline_); + cx_ = model.cx() / downsample_scale; + cy_ = model.cy() / downsample_scale; + fx_ = fy_ = model.fx() / downsample_scale; + width_ = cam_info_msg->width / downsample_scale; + height_ = cam_info_msg->height / downsample_scale; + double baseline = -cam_info_msg->p[3] / cam_info_msg->p[0]; + if (baseline != 0.0) { + baseline_ = baseline; + } + baseline_ *= downsample_scale; + RCLCPP_INFO(this->get_logger(), + "Transformed Cam Info Recvd Fx Fy Cx Cy: %f %f, %f %f Baseline: %f with " + "downsamplescale: %f", + model.fx(), model.fy(), model.cx(), model.cy(), baseline_, downsample_scale); + + RCLCPP_WARN(this->get_logger(), + "Cam Info Constants - Fx Fy Cx Cy: %f %f, %f %f / width=%d - height=%d", fx_, + fy_, cx_, cy_, width_, height_); + + got_cam_info = true; + } + + bool is_state_valid_depth_pose(geometry_msgs::msg::PoseStamped checked_state, double thresh, + double &occupancy) { + occupancy = 0.0; + geometry_msgs::msg::PointStamped checked_point_stamped; + checked_point_stamped.point = checked_state.pose.position; + checked_point_stamped.header = checked_state.header; + + geometry_msgs::msg::PointStamped world_point_stamped; + + try { + tf_buffer_.transform(checked_point_stamped, world_point_stamped, fixed_frame_); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s", + fixed_frame_.c_str(), checked_state.header.frame_id.c_str(), ex.what()); + return false; + } + + tf2::Vector3 optical_point; + tf2::fromMsg(world_point_stamped.point, optical_point); + + std::scoped_lock lock(io_mutex); + + bool seen = false; + for (uint i = 0; i < disp_graph_.size(); i++) { + tf2::Vector3 local_point = disp_graph_.at(i).w2s_tf * optical_point; + + float x, y, z; + + x = local_point.getX(); + y = local_point.getY(); + z = local_point.getZ(); + + if (local_point.length() < 1.0) { + seen = true; + continue; + } + + int u = x / z * fx_ + cx_; + int v = y / z * fy_ + cy_; + + if (u >= 0 && u < width_ && v >= 0 && v < height_ && z > 0.0) { + seen = true; + double state_disparity = baseline_ * fx_ / z; + if ((disp_graph_.at(i).Im_fg->image.at(v, u) > state_disparity) && + (disp_graph_.at(i).Im_bg->image.at(v, u) < state_disparity)) { + occupancy += (state_disparity - 0.5) / state_disparity; + } else { + occupancy -= 0.5 * (state_disparity - 0.5) / state_disparity; + occupancy = occupancy < 0.0 ? 0.0 : occupancy; + } + } + if (occupancy >= thresh) { + return false; + } + } + return seen; + } + + bool get_state_cost(geometry_msgs::msg::Pose checked_state, double &cost) { + geometry_msgs::msg::PointStamped checked_point_stamped; + checked_point_stamped.header.frame_id = "world"; + checked_point_stamped.header.stamp = this->now();// - rclcpp::Duration(0.1); + checked_point_stamped.point = checked_state.position; + + geometry_msgs::msg::PointStamped world_point_stamped; + + try { + tf_buffer_.transform(checked_point_stamped, world_point_stamped, fixed_frame_); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "TF to fixed frame failed: %s", ex.what()); + return false; + } + + tf2::Vector3 optical_point; + tf2::fromMsg(world_point_stamped.point, optical_point); + + std::lock_guard lock(io_mutex); + for (uint i = 0; i < disp_graph_.size(); i++) { + tf2::Vector3 local_point = disp_graph_.at(i).w2s_tf * optical_point; + + float x, y, z; + + x = local_point.getX(); + y = local_point.getY(); + z = local_point.getZ(); + + if (z < 0.0) { + return false; + } + + int u = x / z * fx_ + cx_; + int v = y / z * fy_ + cy_; + + if (u >= 0 && u < width_ && v >= 0 && v < height_ && z > 0.0) { + double state_disparity = baseline_ * fx_ / z; + if ((disp_graph_.at(i).Im_fg->image.at(v, u) > state_disparity) && + (disp_graph_.at(i).Im_bg->image.at(v, u) < state_disparity)) { + cost += (state_disparity - 0.5) / state_disparity; + } + cost = cost < 0.0 ? 0.0 : cost; + + if (cost > 100.0) { + cost = 100.0; + return false; + } + } + } + return true; + } + + void clear_graph(void) { + std::lock_guard lock(io_mutex); + disp_graph_.clear(); + first = true; + RCLCPP_INFO_STREAM(this->get_logger(), "<<< DISP GRAPH CLEARED >>>"); + }; + + rclcpp::Publisher::SharedPtr pcdPub, occPub_; + + rclcpp::Publisher::SharedPtr expansion_poly_pub; + float orig_z; +}; + +} // namespace disparity_graph diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/package.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/package.xml new file mode 100644 index 000000000..774d51d38 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/package.xml @@ -0,0 +1,29 @@ + + + + disparity_graph + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake_ros + + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros + geometry_msgs + cv_bridge + image_geometry + nav_msgs + image_transport + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph/src/disparity_graph.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/src/disparity_graph.cpp new file mode 100644 index 000000000..2e1006d0b --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph/src/disparity_graph.cpp @@ -0,0 +1,6 @@ +#include "disparity_graph/disparity_graph.hpp" + +namespace disparity_graph +{ + +} // namespace disparity_graph diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/include/disparity_graph/disparity_graph.h b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/include/disparity_graph/disparity_graph.h deleted file mode 100644 index 129bec471..000000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/include/disparity_graph/disparity_graph.h +++ /dev/null @@ -1,108 +0,0 @@ -/** - * @attention Copyright (C) 2017 - * @attention Carnegie Mellon University - * @attention All rights reserved - * - * @author: AirLab / Field Robotics Center - * @author: Geetesh Dubey - * - * @attention This code was modified under award #A018532. - * @attention LIMITED RIGHTS: - * @attention The US Government is granted Limited Rights to this Data. - * Use, duplication, or disclosure is subject to the - * restrictions as stated in award #A014692 - * @author: Geetesh Dubey - * - */ -#include -#include "sensor_msgs/Image.h" -#include "sensor_msgs/image_encodings.h" -#include "visualization_msgs/MarkerArray.h" -#include -#include "opencv2/core/core.hpp" -#include -#include -#include "deque" -#include "tf/transform_listener.h" -#include "tf/transform_datatypes.h" -#include -#include -#include -#include -#include "image_geometry/pinhole_camera_model.h" -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "nav_msgs/OccupancyGrid.h" -#include "visualization_msgs/MarkerArray.h" - -#include - -typedef pcl::PointCloud PointCloud; - -namespace nabla { namespace disparity_graph { - -class disparity_graph { - -private: - struct node { - //sensor_msgs::Image Im_fg; - //sensor_msgs::Image Im_bg; - cv_bridge::CvImagePtr Im_fg; - cv_bridge::CvImagePtr Im_bg; - std_msgs::Header header; - tf::Transform w2s_tf; - tf::Transform s2w_tf; - }; - -public: - void disp_cb(const sensor_msgs::Image::ConstPtr &disp_fg, - const sensor_msgs::Image::ConstPtr &disp_bg); - - std::deque disp_graph; - size_t graph_size; - double thresh_; - tf::TransformListener listener; - std::string sensor_frame, fixed_frame, stabilized_frame; - visualization_msgs::Marker marker; - ros::Publisher disparity_graph_marker_pub; - disparity_graph(); - ros::Timer timer1; - double angle_tol, displacement_tol; - bool first; - bool got_cam_info; - double fx_,fy_,cx_,cy_,baseline_,downsample_scale; - unsigned int width_,height_; - ros::Subscriber cam_info_sub_; - boost::mutex io_mutex; - message_filters::Subscriber disp_fg_sub_, disp_bg_sub_; - typedef message_filters::sync_policies::ExactTime ExactPolicy; - typedef message_filters::Synchronizer ExactSync; - boost::shared_ptr exact_sync_; - void getCamInfo(const sensor_msgs::CameraInfo::ConstPtr &msg_info); - bool isStateValidDepth_pose(geometry_msgs::PoseStamped checked_state, double thresh, double& occupancy); - bool getStateCost(geometry_msgs::Pose checked_state, double &cost); - - //TEST PCD - PointCloud pcd_checked_states; - void pcd_test(const ros::TimerEvent &event); - //void pcd_test2(const ros::TimerEvent &event); - void pcd_test3(const ros::TimerEvent& event); - void pcd_test4(const ros::TimerEvent &event); - void clear_graph(void) { boost::mutex::scoped_lock lock(io_mutex); disp_graph.clear(); first = true; ROS_ERROR_STREAM("<<< DISP GRAPH CLEARED >>>"); }; - ros::Publisher pcdPub, occPub_; - //nav_msgs::OccupancyGrid occ_map; - ros::Publisher expansion_poly_pub; - float orig_z; -}; - -} } diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/src/disparity_graph.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/src/disparity_graph.cpp deleted file mode 100644 index 39415db72..000000000 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/src/disparity_graph.cpp +++ /dev/null @@ -1,661 +0,0 @@ -/** - * @attention Copyright (C) 2017 - * @attention Carnegie Mellon University - * @attention All rights reserved - * - * @author: AirLab / Field Robotics Center - * @author: Geetesh Dubey - * - * @attention This code was modified under award #A018532. - * @attention LIMITED RIGHTS: - * @attention The US Government is granted Limited Rights to this Data. - * Use, duplication, or disclosure is subject to the - * restrictions as stated in award #A014692 - * @author: Geetesh Dubey - * - */ -/* -* Copyright (c) 2016 Carnegie Mellon University, Author -* -* For License information please see the LICENSE file in the root directory. -* -*/ - -#include "disparity_graph/disparity_graph.h" - -#include - -using namespace nabla::disparity_graph; - -disparity_graph::disparity_graph() { - ros::NodeHandle priv_nh("~/disparity_graph"); - ros::NodeHandle global_nh("/oa"); - graph_size = 10; - angle_tol = /*cos*/(30.0); - displacement_tol = 01.5; - first = true; - got_cam_info = false; - fixed_frame = "world"; - stabilized_frame = "base_frame_stabilized"; - disparity_graph_marker_pub = priv_nh.advertise("disparity_marker", 10); - marker.header.frame_id = fixed_frame; - marker.header.stamp = ros::Time::now(); - marker.ns = "disparityGraph"; - marker.id = 0; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 1; - marker.pose.position.y = 1; - marker.pose.position.z = 1; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 1; - marker.scale.y = 0.1; - marker.scale.z = 0.1; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - //downsample_scale =4.0; - //baseline_ = 0.5576007548439765; - global_nh.param("baseline", baseline_, 0.10);//0.5576007548439765; - global_nh.param("downsample_scale", downsample_scale, 1.0); - global_nh.param("low_occ_thresh", thresh_, 0.9); - int igraph_size = graph_size; - priv_nh.param("graph_size", igraph_size, igraph_size); - graph_size = igraph_size; - priv_nh.param("displacement_tolerance", displacement_tol, displacement_tol); - priv_nh.param("angle_tolerance", angle_tol, angle_tol); - angle_tol = angle_tol * M_PI/180.0; - - ROS_ERROR("disparity_graph NS: %s",priv_nh.getNamespace().c_str()); - - // right_info_sub_.subscribe(nh, right_info_topic, 1); - disp_fg_sub_.subscribe(priv_nh,"/ceye/left/expanded_disparity_fg",1); - disp_bg_sub_.subscribe(priv_nh,"/ceye/left/expanded_disparity_bg",1); - - exact_sync_.reset(new ExactSync(ExactPolicy(5),disp_fg_sub_,disp_bg_sub_) ); - exact_sync_->registerCallback(boost::bind(&disparity_graph::disp_cb, this, _1, _2)); - cam_info_sub_ = priv_nh.subscribe("/nerian_sp1/right/camera_info", 1,&disparity_graph::getCamInfo,this); - timer1 = priv_nh.createTimer(ros::Duration(0.20), &disparity_graph::pcd_test4,this); - - pcd_checked_states.header.frame_id=fixed_frame; - pcd_checked_states.height = 1; - pcd_checked_states.width =0; - pcd_checked_states.is_dense = false; - pcdPub = priv_nh.advertise("/graph_pcd",10); - expansion_poly_pub = priv_nh.advertise("/expansion/graph_occ_marker", 10); - -/* - //Occupancy Map - occ_map.header.frame_id=fixed_frame; - occ_map.info.resolution = 1.0;//0.5; - occ_map.info.width = 2*50/occ_map.info.resolution; - occ_map.info.height = 2*50/occ_map.info.resolution; - occ_map.data.resize(occ_map.info.width*occ_map.info.height,-1); - orig_z = -1.0; - occ_map.info.origin.position.x = -double(occ_map.info.width)/2.0*occ_map.info.resolution; - occ_map.info.origin.position.y = -double(occ_map.info.height)/2.0*occ_map.info.resolution; - occ_map.info.origin.position.z = orig_z; - occ_map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0); - occPub_ = priv_nh.advertise("/graph_occ_map",10); -*/ -} - -void disparity_graph::pcd_test(const ros::TimerEvent& event) { - if (pcdPub.getNumSubscribers() == 0) { - return; - } - - for(uint i =0; iimage.at(v,u)); - z_bg = (baseline_*fx_/disp_graph.at(i).Im_bg->image.at(v,u)); - } - - for (double z_sample = z_fg; - z_sample <= z_bg && std::isfinite(z_sample)/*&& z_sample>5.0*/; - z_sample += 0.5) { - pt_optical_fg.setZ(z_sample); - pt_optical_fg.setX((u - cx_) * pt_optical_fg.getZ()/fx_); - pt_optical_fg.setY((v - cy_) * pt_optical_fg.getZ()/fy_); - - pt_world_fg = s2w_tf * pt_optical_fg; - - pcl::PointXYZI pt; - //FG - pt.x = pt_world_fg.getX(); - pt.y = pt_world_fg.getY(); - pt.z = pt_world_fg.getZ(); - - double state_disparity = baseline_ * fx_ / z_sample; - double confidence = (state_disparity - 0.5)/state_disparity; - - if(confidence >= thresh_) - pt.intensity = confidence;//*200.0;//i*20+10; - else - pt.intensity = 0.0; - if(confidence >= thresh_) - { - pcd_checked_states.points.push_back(pt); - pcd_checked_states.width++; - } - z_sample = ((z_sample+0.5) > z_bg ) ? z_bg : z_sample; - } - - //pt_optical_fg.setZ(baseline_*fx_/disp_graph.at(i).Im_fg->image.at(v,u)); - //pt_optical_fg.setX((u - cx_) * pt_optical_fg.getZ()/fx_); - //pt_optical_fg.setY((v - cy_) * pt_optical_fg.getZ()/fy_); - - //pt_optical_bg.setZ(baseline_*fx_/disp_graph.at(i).Im_bg->image.at(v,u)); - //pt_optical_bg.setX((u - cx_) * pt_optical_bg.getZ()/fx_); - //pt_optical_bg.setY((v - cy_) * pt_optical_bg.getZ()/fy_); - - //pt_world_fg = s2w_tf * pt_optical_fg; - //pt_world_bg = s2w_tf * pt_optical_bg; - - //pcl::PointXYZI pt; - ////FG - //pt.x = pt_world_fg.getX(); - //pt.y = pt_world_fg.getY(); - //pt.z = pt_world_fg.getZ(); - //pt.intensity = 200;//i*20+10; - //pcd_checked_states.points.push_back(pt); - //pcd_checked_states.width++; - - //double x1,y1,x2,y2,z1,z2; - //x1 = pt.x;y1=pt.y;z1=pt.z; - ////BG - //pt.x = pt_world_bg.getX(); - //pt.y = pt_world_bg.getY(); - //pt.z = pt_world_bg.getZ(); - //pt.intensity = 100;//i*20+10; - //pcd_checked_states.points.push_back(pt); - //pcd_checked_states.width++; - } - } - } - - // Create the filtering object - pcd_checked_states.header.stamp = pcl_conversions::toPCL(ros::Time::now()); - pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ()); - pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); - pcl::toPCLPointCloud2(pcd_checked_states,*cloud); - pcl::VoxelGrid sor; - sor.setInputCloud (cloud); - double voxel_dim = 0.5; - sor.setLeafSize (voxel_dim,voxel_dim,voxel_dim); - sor.filter (*cloud_filtered); - pcd_checked_states.clear(); - pcl::fromPCLPointCloud2(*cloud_filtered,pcd_checked_states); - - sensor_msgs::PointCloud2 cloud_PC2; - pcl::toROSMsg(pcd_checked_states,cloud_PC2); - pcdPub.publish(cloud_PC2); - pcd_checked_states.points.clear(); - pcd_checked_states.width = 0; -} - -void disparity_graph::pcd_test4(const ros::TimerEvent& event) { - if (pcdPub.getNumSubscribers() == 0) { - return; - } - - pcl::PointXYZI pt; - - ros::Time stamp = ros::Time::now(); - listener.waitForTransform(fixed_frame,stabilized_frame,stamp,ros::Duration(0.1)); - tf::StampedTransform transform; - try { - listener.lookupTransform(fixed_frame, - stabilized_frame, - stamp, transform); - } catch (tf::TransformException &ex) { - ROS_ERROR("DG graph_pcd %s",ex.what()); - // ros::Duration(1.0).sleep(); - // continue; - return; - } -// checked_point_stmpd.header.stamp = stamp;//ros::Time::now();//(0); -// checked_point_stmpd.header.frame_id = fixed_frame; - - pcd_checked_states.header.frame_id = fixed_frame; - - geometry_msgs::PoseStamped checked_pose; - checked_pose.header.frame_id = pcd_checked_states.header.frame_id; - checked_pose.header.stamp = stamp; - float delta = .5; - for(float i =-50; i<=50;i+=delta) { - for(float u=-50;u<=50;u+=delta) { - for(float v=-10;v<=10;v+=delta) { - -// checked_point_stmpd.point.x = i; -// checked_point_stmpd.point.y = u; -// checked_point_stmpd.point.z = v; - - tf::Point local_point(i,u,v); -// tf::pointMsgToTF(checked_point_stmpd.point,local_point); - tf::Point world_point = transform * local_point; - tf::pointTFToMsg(world_point,checked_pose.pose.position); - double occupancy = 0.0; - bool valid = isStateValidDepth_pose(checked_pose, thresh_, occupancy); - if(!valid) - { - pt.x = world_point.getX(); - pt.y = world_point.getY(); - pt.z = world_point.getZ(); - pt.intensity = occupancy; - - - if(occupancy >= thresh_) - { - pcd_checked_states.points.push_back(pt); - pcd_checked_states.width++; - } - } - - } - } - } - - // Create the filtering object - pcd_checked_states.header.stamp = pcl_conversions::toPCL(stamp); - /*pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ()); - pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); - pcl::toPCLPointCloud2(pcd_checked_states,*cloud); - pcl::VoxelGrid sor; - sor.setInputCloud (cloud); - double voxel_dim = 0.5; - sor.setLeafSize (voxel_dim,voxel_dim,voxel_dim); - sor.filter (*cloud_filtered); - pcd_checked_states.clear(); - pcl::fromPCLPointCloud2(*cloud_filtered,pcd_checked_states);*/ - - sensor_msgs::PointCloud2 cloud_PC2; - pcl::toROSMsg(pcd_checked_states,cloud_PC2); - pcdPub.publish(cloud_PC2); - pcd_checked_states.points.clear(); - pcd_checked_states.width = 0; -} - -/* -void disparity_graph::pcd_test2(const ros::TimerEvent& event) { - pcd_test(event); - if(occPub_.getNumSubscribers() == 0) - return; - - occ_map.info.map_load_time = ros::Time::now(); - occ_map.data.clear(); - occ_map.data.resize(occ_map.info.width*occ_map.info.height,-1); - - for(uint u=0; u=0 && index < occ_map.info.width*occ_map.info.height) - { - getStateCost(chk_pose,cost); - occ_map.data[index] = uint64(cost); - } - - } - } - occ_map.header.stamp = ros::Time::now(); - occPub_.publish(occ_map); -} -*/ - -void disparity_graph::pcd_test3(const ros::TimerEvent& event) { - /* create expansion PCD */ - if (expansion_poly_pub.getNumSubscribers() > 0 ) { - cv_bridge::CvImagePtr cv_depth_fg,cv_depth_bg; - visualization_msgs::MarkerArray marker_arr; - visualization_msgs::Marker marker; - //marker.header = msg_disp->header; - marker.ns = "occ_space"; - marker.id = 0; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.lifetime = ros::Duration(0.5); - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x =0;//xyz_centroid[0]; - marker.pose.position.y =0;//xyz_centroid[1]; - marker.pose.position.z =0;//xyz_centroid[2]; - - tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0,0.0,0.0), marker.pose.orientation); - // marker.pose.orientation.w = 1; - float marker_scale = 0.1; - marker.scale.x = marker_scale; - marker.scale.y = marker_scale; - marker.scale.z = marker_scale; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - - int v = 60; - for (uint i = 0; i < disp_graph.size(); i++) { - float prev_depth = 0.0; - // { - // boost::mutex::scoped_lock lock(io_mutex); - // cv_depth_fg = cv_bridge::toCvCopy(disp_graph.at(i).Im_fg,sensor_msgs::image_encodings::TYPE_32FC1); - // cv_depth_bg = cv_bridge::toCvCopy(disp_graph.at(i).Im_bg,sensor_msgs::image_encodings::TYPE_32FC1); - // } - marker.header = disp_graph.at(i).header; - for (int u = (int) width_ - 1; u >= 0; u--) { - float depth_value = baseline_ * fx_ / disp_graph.at(i).Im_fg->image.at(v,u);//free_msg->image.at(v,u)[0]; - float depth_diff = fabs(depth_value-prev_depth); - prev_depth = depth_value; - if (!std::isnan(depth_value) && !std::isinf(depth_value) && depth_diff < 0.5) { - geometry_msgs::Point gm_p; - gm_p.x = ( u - cx_ ) * depth_value / fx_; - gm_p.y = ( v - cy_ ) * depth_value / fy_; - gm_p.z = depth_value; - marker.points.push_back(gm_p); - - depth_value = baseline_ * fx_ / disp_graph.at(i).Im_bg->image.at(v,u); - gm_p.x = ( u - cx_ ) * depth_value / fx_; - gm_p.y = ( v - cy_ ) * depth_value / fy_; - gm_p.z = depth_value; - marker.points.push_back(gm_p); - // poly.polygon.points.push_back(gm_p); - } else { - marker_arr.markers.push_back(marker); - marker.points.clear(); - marker.id++; - } - } - } - marker_arr.markers.push_back(marker); - expansion_poly_pub.publish(marker_arr); - } -} - -void disparity_graph::disp_cb(const sensor_msgs::Image::ConstPtr &disp_fg, - const sensor_msgs::Image::ConstPtr &disp_bg) { -// ROS_INFO("Recvd Disp,size %d",disp_graph.size()); - ROS_INFO("Recvd disp stamp: %lf",ros::Duration(ros::Time::now() - disp_fg->header.stamp).toSec()); - sensor_frame = disp_fg->header.frame_id; - - listener.waitForTransform(sensor_frame, - fixed_frame, - disp_fg->header.stamp, ros::Duration(0.10)); - - tf::StampedTransform transform; - try { - listener.lookupTransform(sensor_frame, - fixed_frame, - disp_fg->header.stamp, transform); - } catch (tf::TransformException &ex) { - ROS_ERROR("DG disp_cb %s",ex.what()); - // ros::Duration(1.0).sleep(); - // continue; - return; - } - - boost::mutex::scoped_lock lock(io_mutex); - if (first) { - first = false; - node n; - try { - n.Im_fg = cv_bridge::toCvCopy(disp_fg,sensor_msgs::image_encodings::TYPE_32FC1); - n.Im_bg = cv_bridge::toCvCopy(disp_bg,sensor_msgs::image_encodings::TYPE_32FC1); - } catch (std::exception ex) { - ROS_ERROR("\n\n\n\n\n\t\t\tDEQUE ERR\n\n\n\n: %s",ex.what()); - return; - } - // n.Im_fg = *disp_fg; - // n.Im_bg = *disp_bg; - n.header = disp_bg->header; - n.w2s_tf= transform; - n.s2w_tf= transform.inverse(); - disp_graph.push_front(n); - disp_graph.push_back(n); - } else { - //Check for similar tf nodes - //tf::Vector3 curr_a =transform.inverse().getRotation().normalize().getAxis().normalize(); - tf::Vector3 curr_p =transform.inverse().getOrigin(); - std::deque::iterator it,end; - it = disp_graph.begin(); - end = disp_graph.end()-1; - // while(it!=end && disp_graph.size()) - // { - //tf::Vector3 graph_a = it->s2w_tf.getRotation().normalize().getAxis().normalize(); - tf::Vector3 graph_p = it->s2w_tf.getOrigin(); - // tfScalar diff_a = curr_a.dot(graph_a); - tfScalar diff_a = tf::angleShortestPath(transform.inverse().getRotation(),it->s2w_tf.getRotation()); - tfScalar diff_p = curr_p.distance(graph_p); - // ROS_INFO_STREAM("diff_a ; diff_p:"<header; - n.w2s_tf= transform; - n.s2w_tf= transform.inverse(); - //pose in graph is too close in angle to the current graph - if (fabs(diff_a)>=angle_tol || fabs(diff_p)>displacement_tol) { - ROS_ERROR("Adding new node size %d", (int)disp_graph.size()); - if (disp_graph.size() >= graph_size) { - disp_graph.erase(end); - } - - //add new data - if(disp_graph.size() <= graph_size) { - disp_graph.push_front(n); - } - // it = disp_graph.erase(it); - } - disp_graph.pop_back(); - disp_graph.push_back(n); - // pcd_test(); - } - - visualization_msgs::MarkerArray marker_arr; - - for(uint i =0; iwidth/downsample_scale;// this is already downsampled from, disp_expansion - height_ = msg_info->height/downsample_scale; - double baseline = -msg_info->P[3]/msg_info->P[0]; - if (baseline != 0.0) { - baseline_ = baseline; - } - baseline_ *=downsample_scale; - ROS_WARN("Transformed Cam Info Recvd Fx Fy Cx Cy: %f %f, %f %f Baseline: %f with downsamplescale: %f", - model_.fx(), - model_.fy(), - model_.cx(), - model_.cy(), - baseline_, - downsample_scale); - - ROS_WARN("Cam Info Constants - Fx Fy Cx Cy: %f %f, %f %f / width=%d - height=%d", - fx_, fy_, cx_, cy_, width_, height_); - - got_cam_info = true; -} - - -bool disparity_graph::isStateValidDepth_pose(geometry_msgs::PoseStamped checked_state, - double thresh, double &occupancy) { - //unsigned int invalid_cntr=0; - occupancy = 0.0; - geometry_msgs::PointStamped checked_point_stamped; - checked_point_stamped.point = checked_state.pose.position; - checked_point_stamped.header = checked_state.header; - - ////check the pose using the footprint_cost check - - geometry_msgs::PointStamped world_point_stamped; - - try { - listener.transformPoint(fixed_frame,checked_point_stamped,world_point_stamped); - } catch (tf::TransformException ex){ - ROS_ERROR("TF to fixed_frame failed: %s",ex.what()); - return false; - } - - tf::Point optical_point; - tf::pointMsgToTF(world_point_stamped.point, optical_point); - - boost::mutex::scoped_lock lock(io_mutex); - bool seen = false; //true; - for (uint i = 0; i < disp_graph.size(); i++) { - - tf::Point local_point = disp_graph.at(i).w2s_tf * optical_point; - - float x,y,z; - - x = local_point.getX(); - y = local_point.getY(); - z = local_point.getZ(); - - if(local_point.length() < 1.0) - { - seen = true; - continue; - } -// if((-0.20=0 && u< width_ && v>=0 && v0.0) { - seen = true; - double state_disparity = baseline_ * fx_ / z; - if ((disp_graph.at(i).Im_fg->image.at(v,u) > state_disparity) && - (disp_graph.at(i).Im_bg->image.at(v,u) < state_disparity)) { - - occupancy += (state_disparity - 0.5)/state_disparity; - } else { - occupancy -= 0.5*(state_disparity - 0.5)/state_disparity; - occupancy = occupancy < 0.0 ? 0.0 : occupancy; - } - } - if (/*invalid_cntr>=invalid_thresh*/ occupancy >= thresh) { - return false; - } - } - if (seen) { - return true; - } else { - return false; - } -} - - -bool disparity_graph::getStateCost(geometry_msgs::Pose checked_state, - double &cost) { - -// uint invalid_thresh = 2; -// uint validity = 2; -// unsigned int invalid_cntr=0; - geometry_msgs::PointStamped checked_point_stamped; - checked_point_stamped.header.frame_id = "world"; - checked_point_stamped.header.stamp = ros::Time::now() - ros::Duration(0.1); - checked_point_stamped.point = checked_state.position; - - geometry_msgs::PointStamped world_point_stamped; -// listener.waitForTransform(fixed_frame,checked_point_stamped.header.frame_id,checked_point_stamped.header.stamp,ros::Duration(1.0)); - try { - listener.transformPoint(fixed_frame,checked_point_stamped,world_point_stamped); - } catch (tf::TransformException ex) { - ROS_ERROR("TF to fixed_frame failed: %s",ex.what()); - return false; - } - - tf::Point optical_point; - tf::pointMsgToTF(world_point_stamped.point, optical_point); - - boost::mutex::scoped_lock lock(io_mutex); - for (uint i = 0; i < disp_graph.size(); i++) { - tf::Point local_point = disp_graph.at(i).w2s_tf * optical_point; - - float x,y,z; - x = local_point.getX(); - y = local_point.getY(); - z = local_point.getZ(); - - //point behind camera - if (z<0) { - continue; - } - - int u = x/z*fx_ + cx_; - int v = y/z*fy_ + cy_; - - if (u>=0 && u< width_ && v>=0 && vimage.at(v,u) > state_disparity) && (disp_graph.at(i).Im_bg->image.at(v,u) < state_disparity)) { -// cost += (fabs(bg_msg->image.at(v,u) - state_disparity)/fabs(fg_msg->image.at(v,u) - bg_msg->image.at(v,u)) + 1) * 10; - cost +=state_disparity * 10.0; -// invalid_cntr++; - } - cost = cost < 0.0 ? 0.0 : cost; - ROS_ERROR_THROTTLE(1,"CHECKING POINT at \t%f\t%f\t%f : cost = %f", - checked_state.position.x, - checked_state.position.y, - checked_state.position.z, - cost); - - if (cost > 100.0) { - cost = 100.0; - return false; - } - } - } - - return true; -} diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/.gitignore b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/.gitignore similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/.gitignore rename to ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/.gitignore diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/CMakeLists.txt similarity index 99% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/CMakeLists.txt rename to ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/CMakeLists.txt index 8803b2a0d..58f0ec5b6 100644 --- a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS tf_conversions tictoc_profiler pcl_conversions - pcl_ros ) # CONFIGURE OPENCV diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/LICENSE b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/LICENSE similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/LICENSE rename to ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/LICENSE diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/include/disparity_graph/disparity_graph.h b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/include/disparity_graph/disparity_graph.h new file mode 100644 index 000000000..5ff32b818 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/include/disparity_graph/disparity_graph.h @@ -0,0 +1,113 @@ +/** + * @attention Copyright (C) 2017 + * @attention Carnegie Mellon University + * @attention All rights reserved + * + * @author: AirLab / Field Robotics Center + * @author: Geetesh Dubey + * + * @attention This code was modified under award #A018532. + * @attention LIMITED RIGHTS: + * @attention The US Government is granted Limited Rights to this Data. + * Use, duplication, or disclosure is subject to the + * restrictions as stated in award #A014692 + * @author: Geetesh Dubey + * + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "deque" +#include "image_geometry/pinhole_camera_model.h" +#include "nav_msgs/OccupancyGrid.h" +#include "opencv2/core/core.hpp" +#include "sensor_msgs/Image.h" +#include "sensor_msgs/image_encodings.h" +#include "tf/transform_datatypes.h" +#include "tf/transform_listener.h" +#include "visualization_msgs/MarkerArray.h" + +typedef pcl::PointCloud PointCloud; + +namespace nabla { +namespace disparity_graph { + +class disparity_graph { + private: + struct node { + // sensor_msgs::Image Im_fg; + // sensor_msgs::Image Im_bg; + cv_bridge::CvImagePtr Im_fg; + cv_bridge::CvImagePtr Im_bg; + std_msgs::Header header; + tf::Transform w2s_tf; + tf::Transform s2w_tf; + }; + + public: + void disp_cb(const sensor_msgs::Image::ConstPtr &disp_fg, + const sensor_msgs::Image::ConstPtr &disp_bg); + + std::deque disp_graph; + size_t graph_size; + double thresh_; + tf::TransformListener listener; + std::string sensor_frame, fixed_frame, stabilized_frame; + visualization_msgs::Marker marker; + ros::Publisher disparity_graph_marker_pub; + disparity_graph(); + ros::Timer timer1; + double angle_tol, displacement_tol; + bool first; + bool got_cam_info; + double fx_, fy_, cx_, cy_, baseline_, downsample_scale; + unsigned int width_, height_; + ros::Subscriber cam_info_sub_; + boost::mutex io_mutex; + message_filters::Subscriber disp_fg_sub_, disp_bg_sub_; + typedef message_filters::sync_policies::ExactTime + ExactPolicy; + typedef message_filters::Synchronizer ExactSync; + boost::shared_ptr exact_sync_; + void getCamInfo(const sensor_msgs::CameraInfo::ConstPtr &msg_info); + bool is_state_valid_depth_pose(geometry_msgs::PoseStamped checked_state, double thresh, + double &occupancy); + bool getStateCost(geometry_msgs::Pose checked_state, double &cost); + + // TEST PCD + PointCloud pcd_checked_states; + void pcd_test(const ros::TimerEvent &event); + // void pcd_test2(const ros::TimerEvent &event); + void pcd_test3(const ros::TimerEvent &event); + void pcd_test4(const ros::TimerEvent &event); + void clear_graph(void) { + boost::mutex::scoped_lock lock(io_mutex); + disp_graph.clear(); + first = true; + ROS_ERROR_STREAM("<<< DISP GRAPH CLEARED >>>"); + }; + ros::Publisher pcdPub, occPub_; + // nav_msgs::OccupancyGrid occ_map; + ros::Publisher expansion_poly_pub; + float orig_z; +}; + +} // namespace disparity_graph +} // namespace nabla diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/launch/talker.launch b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/launch/talker.launch similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/launch/talker.launch rename to ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/launch/talker.launch diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/package.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/package.xml similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/package.xml rename to ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/package.xml diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/readme.md b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/readme.md similarity index 100% rename from ros_ws/src/robot/autonomy/planning/local/disparity_graph_bkp/readme.md rename to ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/readme.md diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/src/disparity_graph.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/src/disparity_graph.cpp new file mode 100644 index 000000000..a661fec8e --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_graph_ros1/src/disparity_graph.cpp @@ -0,0 +1,653 @@ +/** + * @attention Copyright (C) 2017 + * @attention Carnegie Mellon University + * @attention All rights reserved + * + * @author: AirLab / Field Robotics Center + * @author: Geetesh Dubey + * + * @attention This code was modified under award #A018532. + * @attention LIMITED RIGHTS: + * @attention The US Government is granted Limited Rights to this Data. + * Use, duplication, or disclosure is subject to the + * restrictions as stated in award #A014692 + * @author: Geetesh Dubey + * + */ +/* + * Copyright (c) 2016 Carnegie Mellon University, Author + * + * For License information please see the LICENSE file in the root directory. + * + */ + +#include "disparity_graph/disparity_graph.h" + +#include + +using namespace nabla::disparity_graph; + +disparity_graph::disparity_graph() { + ros::NodeHandle priv_nh("~/disparity_graph"); + ros::NodeHandle global_nh("/oa"); + graph_size = 10; + angle_tol = /*cos*/ (30.0); + displacement_tol = 01.5; + first = true; + got_cam_info = false; + fixed_frame = "world"; + stabilized_frame = "base_frame_stabilized"; + disparity_graph_marker_pub = + priv_nh.advertise("disparity_marker", 10); + marker.header.frame_id = fixed_frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "disparityGraph"; + marker.id = 0; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = 1; + marker.pose.position.y = 1; + marker.pose.position.z = 1; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + // downsample_scale =4.0; + // baseline_ = 0.5576007548439765; + global_nh.param("baseline", baseline_, 0.10); // 0.5576007548439765; + global_nh.param("downsample_scale", downsample_scale, 1.0); + global_nh.param("low_occ_thresh", thresh_, 0.9); + int igraph_size = graph_size; + priv_nh.param("graph_size", igraph_size, igraph_size); + graph_size = igraph_size; + priv_nh.param("displacement_tolerance", displacement_tol, displacement_tol); + priv_nh.param("angle_tolerance", angle_tol, angle_tol); + angle_tol = angle_tol * M_PI / 180.0; + + ROS_ERROR("disparity_graph NS: %s", priv_nh.getNamespace().c_str()); + + // right_info_sub_.subscribe(nh, right_info_topic, 1); + disp_fg_sub_.subscribe(priv_nh, "/ceye/left/expanded_disparity_fg", 1); + disp_bg_sub_.subscribe(priv_nh, "/ceye/left/expanded_disparity_bg", 1); + + exact_sync_.reset(new ExactSync(ExactPolicy(5), disp_fg_sub_, disp_bg_sub_)); + exact_sync_->registerCallback(boost::bind(&disparity_graph::disp_cb, this, _1, _2)); + cam_info_sub_ = + priv_nh.subscribe("/nerian_sp1/right/camera_info", 1, &disparity_graph::getCamInfo, this); + timer1 = priv_nh.createTimer(ros::Duration(0.20), &disparity_graph::pcd_test4, this); + + pcd_checked_states.header.frame_id = fixed_frame; + pcd_checked_states.height = 1; + pcd_checked_states.width = 0; + pcd_checked_states.is_dense = false; + pcdPub = priv_nh.advertise("/graph_pcd", 10); + expansion_poly_pub = + priv_nh.advertise("/expansion/graph_occ_marker", 10); + + /* + //Occupancy Map + occ_map.header.frame_id=fixed_frame; + occ_map.info.resolution = 1.0;//0.5; + occ_map.info.width = 2*50/occ_map.info.resolution; + occ_map.info.height = 2*50/occ_map.info.resolution; + occ_map.data.resize(occ_map.info.width*occ_map.info.height,-1); + orig_z = -1.0; + occ_map.info.origin.position.x = -double(occ_map.info.width)/2.0*occ_map.info.resolution; + occ_map.info.origin.position.y = -double(occ_map.info.height)/2.0*occ_map.info.resolution; + occ_map.info.origin.position.z = orig_z; + occ_map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0); + occPub_ = priv_nh.advertise("/graph_occ_map",10); + */ +} + +void disparity_graph::pcd_test(const ros::TimerEvent &event) { + if (pcdPub.getNumSubscribers() == 0) { + return; + } + + for (uint i = 0; i < disp_graph.size(); i++) { + for (uint u = 0; u < width_; u += 1) { + for (uint v = 0; v < height_; v += 1) { + tf::Point pt_optical_fg, pt_optical_bg, pt_world_fg, pt_world_bg, curr_pt; + tf::Transform w2s_tf; + tf::Transform s2w_tf; + double z_fg, z_bg; + cv_bridge::CvImagePtr cv_depth_fg, cv_depth_bg; + { + boost::mutex::scoped_lock lock(io_mutex); + // cv_depth_fg = + // cv_bridge::toCvCopy(disp_graph.at(i).Im_fg,sensor_msgs::image_encodings::TYPE_32FC1); + // cv_depth_bg = + // cv_bridge::toCvCopy(disp_graph.at(i).Im_bg,sensor_msgs::image_encodings::TYPE_32FC1); + w2s_tf = disp_graph.at(i).w2s_tf; + s2w_tf = disp_graph.at(i).s2w_tf; + + z_fg = (baseline_ * fx_ / disp_graph.at(i).Im_fg->image.at(v, u)); + z_bg = (baseline_ * fx_ / disp_graph.at(i).Im_bg->image.at(v, u)); + } + + for (double z_sample = z_fg; + z_sample <= z_bg && std::isfinite(z_sample) /*&& z_sample>5.0*/; + z_sample += 0.5) { + pt_optical_fg.setZ(z_sample); + pt_optical_fg.setX((u - cx_) * pt_optical_fg.getZ() / fx_); + pt_optical_fg.setY((v - cy_) * pt_optical_fg.getZ() / fy_); + + pt_world_fg = s2w_tf * pt_optical_fg; + + pcl::PointXYZI pt; + // FG + pt.x = pt_world_fg.getX(); + pt.y = pt_world_fg.getY(); + pt.z = pt_world_fg.getZ(); + + double state_disparity = baseline_ * fx_ / z_sample; + double confidence = (state_disparity - 0.5) / state_disparity; + + if (confidence >= thresh_) + pt.intensity = confidence; //*200.0;//i*20+10; + else + pt.intensity = 0.0; + if (confidence >= thresh_) { + pcd_checked_states.points.push_back(pt); + pcd_checked_states.width++; + } + z_sample = ((z_sample + 0.5) > z_bg) ? z_bg : z_sample; + } + + // pt_optical_fg.setZ(baseline_*fx_/disp_graph.at(i).Im_fg->image.at(v,u)); + // pt_optical_fg.setX((u - cx_) * pt_optical_fg.getZ()/fx_); + // pt_optical_fg.setY((v - cy_) * pt_optical_fg.getZ()/fy_); + + // pt_optical_bg.setZ(baseline_*fx_/disp_graph.at(i).Im_bg->image.at(v,u)); + // pt_optical_bg.setX((u - cx_) * pt_optical_bg.getZ()/fx_); + // pt_optical_bg.setY((v - cy_) * pt_optical_bg.getZ()/fy_); + + // pt_world_fg = s2w_tf * pt_optical_fg; + // pt_world_bg = s2w_tf * pt_optical_bg; + + // pcl::PointXYZI pt; + ////FG + // pt.x = pt_world_fg.getX(); + // pt.y = pt_world_fg.getY(); + // pt.z = pt_world_fg.getZ(); + // pt.intensity = 200;//i*20+10; + // pcd_checked_states.points.push_back(pt); + // pcd_checked_states.width++; + + // double x1,y1,x2,y2,z1,z2; + // x1 = pt.x;y1=pt.y;z1=pt.z; + ////BG + // pt.x = pt_world_bg.getX(); + // pt.y = pt_world_bg.getY(); + // pt.z = pt_world_bg.getZ(); + // pt.intensity = 100;//i*20+10; + // pcd_checked_states.points.push_back(pt); + // pcd_checked_states.width++; + } + } + } + + // Create the filtering object + pcd_checked_states.header.stamp = pcl_conversions::toPCL(ros::Time::now()); + pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2()); + pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2()); + pcl::toPCLPointCloud2(pcd_checked_states, *cloud); + pcl::VoxelGrid sor; + sor.setInputCloud(cloud); + double voxel_dim = 0.5; + sor.setLeafSize(voxel_dim, voxel_dim, voxel_dim); + sor.filter(*cloud_filtered); + pcd_checked_states.clear(); + pcl::fromPCLPointCloud2(*cloud_filtered, pcd_checked_states); + + sensor_msgs::PointCloud2 cloud_PC2; + pcl::toROSMsg(pcd_checked_states, cloud_PC2); + pcdPub.publish(cloud_PC2); + pcd_checked_states.points.clear(); + pcd_checked_states.width = 0; +} + +void disparity_graph::pcd_test4(const ros::TimerEvent &event) { + if (pcdPub.getNumSubscribers() == 0) { + return; + } + + pcl::PointXYZI pt; + + ros::Time stamp = ros::Time::now(); + listener.waitForTransform(fixed_frame, stabilized_frame, stamp, ros::Duration(0.1)); + tf::StampedTransform transform; + try { + listener.lookupTransform(fixed_frame, stabilized_frame, stamp, transform); + } catch (tf::TransformException &ex) { + ROS_ERROR("DG graph_pcd %s", ex.what()); + // ros::Duration(1.0).sleep(); + // continue; + return; + } + // checked_point_stmpd.header.stamp = stamp;//ros::Time::now();//(0); + // checked_point_stmpd.header.frame_id = fixed_frame; + + pcd_checked_states.header.frame_id = fixed_frame; + + geometry_msgs::PoseStamped checked_pose; + checked_pose.header.frame_id = pcd_checked_states.header.frame_id; + checked_pose.header.stamp = stamp; + float delta = .5; + for (float i = -50; i <= 50; i += delta) { + for (float u = -50; u <= 50; u += delta) { + for (float v = -10; v <= 10; v += delta) { + // checked_point_stmpd.point.x = i; + // checked_point_stmpd.point.y = u; + // checked_point_stmpd.point.z = v; + + tf::Point local_point(i, u, v); + // tf::pointMsgToTF(checked_point_stmpd.point,local_point); + tf::Point world_point = transform * local_point; + tf::pointTFToMsg(world_point, checked_pose.pose.position); + double occupancy = 0.0; + bool valid = is_state_valid_depth_pose(checked_pose, thresh_, occupancy); + if (!valid) { + pt.x = world_point.getX(); + pt.y = world_point.getY(); + pt.z = world_point.getZ(); + pt.intensity = occupancy; + + if (occupancy >= thresh_) { + pcd_checked_states.points.push_back(pt); + pcd_checked_states.width++; + } + } + } + } + } + + // Create the filtering object + pcd_checked_states.header.stamp = pcl_conversions::toPCL(stamp); + /*pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ()); + pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); + pcl::toPCLPointCloud2(pcd_checked_states,*cloud); + pcl::VoxelGrid sor; + sor.setInputCloud (cloud); + double voxel_dim = 0.5; + sor.setLeafSize (voxel_dim,voxel_dim,voxel_dim); + sor.filter (*cloud_filtered); + pcd_checked_states.clear(); + pcl::fromPCLPointCloud2(*cloud_filtered,pcd_checked_states);*/ + + sensor_msgs::PointCloud2 cloud_PC2; + pcl::toROSMsg(pcd_checked_states, cloud_PC2); + pcdPub.publish(cloud_PC2); + pcd_checked_states.points.clear(); + pcd_checked_states.width = 0; +} + +/* +void disparity_graph::pcd_test2(const ros::TimerEvent& event) { + pcd_test(event); + if(occPub_.getNumSubscribers() == 0) + return; + + occ_map.info.map_load_time = ros::Time::now(); + occ_map.data.clear(); + occ_map.data.resize(occ_map.info.width*occ_map.info.height,-1); + + for(uint u=0; u=0 && index < occ_map.info.width*occ_map.info.height) + { + getStateCost(chk_pose,cost); + occ_map.data[index] = uint64(cost); + } + + } + } + occ_map.header.stamp = ros::Time::now(); + occPub_.publish(occ_map); +} +*/ + +void disparity_graph::pcd_test3(const ros::TimerEvent &event) { + /* create expansion PCD */ + if (expansion_poly_pub.getNumSubscribers() > 0) { + cv_bridge::CvImagePtr cv_depth_fg, cv_depth_bg; + visualization_msgs::MarkerArray marker_arr; + visualization_msgs::Marker marker; + // marker.header = msg_disp->header; + marker.ns = "occ_space"; + marker.id = 0; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.lifetime = ros::Duration(0.5); + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = 0; // xyz_centroid[0]; + marker.pose.position.y = 0; // xyz_centroid[1]; + marker.pose.position.z = 0; // xyz_centroid[2]; + + tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, 0.0), marker.pose.orientation); + // marker.pose.orientation.w = 1; + float marker_scale = 0.1; + marker.scale.x = marker_scale; + marker.scale.y = marker_scale; + marker.scale.z = marker_scale; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + int v = 60; + for (uint i = 0; i < disp_graph.size(); i++) { + float prev_depth = 0.0; + // { + // boost::mutex::scoped_lock lock(io_mutex); + // cv_depth_fg = + // cv_bridge::toCvCopy(disp_graph.at(i).Im_fg,sensor_msgs::image_encodings::TYPE_32FC1); + // cv_depth_bg = + // cv_bridge::toCvCopy(disp_graph.at(i).Im_bg,sensor_msgs::image_encodings::TYPE_32FC1); + // } + marker.header = disp_graph.at(i).header; + for (int u = (int)width_ - 1; u >= 0; u--) { + float depth_value = baseline_ * fx_ / + disp_graph.at(i).Im_fg->image.at( + v, u); // free_msg->image.at(v,u)[0]; + float depth_diff = fabs(depth_value - prev_depth); + prev_depth = depth_value; + if (!std::isnan(depth_value) && !std::isinf(depth_value) && depth_diff < 0.5) { + geometry_msgs::Point gm_p; + gm_p.x = (u - cx_) * depth_value / fx_; + gm_p.y = (v - cy_) * depth_value / fy_; + gm_p.z = depth_value; + marker.points.push_back(gm_p); + + depth_value = baseline_ * fx_ / disp_graph.at(i).Im_bg->image.at(v, u); + gm_p.x = (u - cx_) * depth_value / fx_; + gm_p.y = (v - cy_) * depth_value / fy_; + gm_p.z = depth_value; + marker.points.push_back(gm_p); + // poly.polygon.points.push_back(gm_p); + } else { + marker_arr.markers.push_back(marker); + marker.points.clear(); + marker.id++; + } + } + } + marker_arr.markers.push_back(marker); + expansion_poly_pub.publish(marker_arr); + } +} + +void disparity_graph::disp_cb(const sensor_msgs::Image::ConstPtr &disp_fg, + const sensor_msgs::Image::ConstPtr &disp_bg) { + // ROS_INFO("Recvd Disp,size %d",disp_graph.size()); + ROS_INFO("Recvd disp stamp: %lf", + ros::Duration(ros::Time::now() - disp_fg->header.stamp).toSec()); + sensor_frame = disp_fg->header.frame_id; + + listener.waitForTransform(sensor_frame, fixed_frame, disp_fg->header.stamp, + ros::Duration(0.10)); + + tf::StampedTransform transform; + try { + listener.lookupTransform(sensor_frame, fixed_frame, disp_fg->header.stamp, transform); + } catch (tf::TransformException &ex) { + ROS_ERROR("DG disp_cb %s", ex.what()); + // ros::Duration(1.0).sleep(); + // continue; + return; + } + + boost::mutex::scoped_lock lock(io_mutex); + if (first) { + first = false; + node n; + try { + n.Im_fg = cv_bridge::toCvCopy(disp_fg, sensor_msgs::image_encodings::TYPE_32FC1); + n.Im_bg = cv_bridge::toCvCopy(disp_bg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (std::exception ex) { + ROS_ERROR("\n\n\n\n\n\t\t\tDEQUE ERR\n\n\n\n: %s", ex.what()); + return; + } + // n.Im_fg = *disp_fg; + // n.Im_bg = *disp_bg; + n.header = disp_bg->header; + n.w2s_tf = transform; + n.s2w_tf = transform.inverse(); + disp_graph.push_front(n); + disp_graph.push_back(n); + } else { + // Check for similar tf nodes + // tf::Vector3 curr_a =transform.inverse().getRotation().normalize().getAxis().normalize(); + tf::Vector3 curr_p = transform.inverse().getOrigin(); + std::deque::iterator it, end; + it = disp_graph.begin(); + end = disp_graph.end() - 1; + // while(it!=end && disp_graph.size()) + // { + // tf::Vector3 graph_a = it->s2w_tf.getRotation().normalize().getAxis().normalize(); + tf::Vector3 graph_p = it->s2w_tf.getOrigin(); + // tfScalar diff_a = curr_a.dot(graph_a); + tfScalar diff_a = + tf::angleShortestPath(transform.inverse().getRotation(), it->s2w_tf.getRotation()); + tfScalar diff_p = curr_p.distance(graph_p); + // ROS_INFO_STREAM("diff_a ; diff_p:"<header; + n.w2s_tf = transform; + n.s2w_tf = transform.inverse(); + // pose in graph is too close in angle to the current graph + if (fabs(diff_a) >= angle_tol || fabs(diff_p) > displacement_tol) { + ROS_ERROR("Adding new node size %d", (int)disp_graph.size()); + if (disp_graph.size() >= graph_size) { + disp_graph.erase(end); + } + + // add new data + if (disp_graph.size() <= graph_size) { + disp_graph.push_front(n); + } + // it = disp_graph.erase(it); + } + disp_graph.pop_back(); + disp_graph.push_back(n); + // pcd_test(); + } + + visualization_msgs::MarkerArray marker_arr; + + for (uint i = 0; i < disp_graph.size(); i++) { + // ROS_INFO_STREAM(disp_graph.at(i).tf.inverse().getOrigin().x()); + tf::pointTFToMsg(disp_graph.at(i).s2w_tf.getOrigin(), marker.pose.position); + tf::quaternionTFToMsg(disp_graph.at(i).s2w_tf.getRotation(), marker.pose.orientation); + marker.header.stamp = ros::Time::now(); + marker.id = i; + marker_arr.markers.push_back(marker); + } + + disparity_graph_marker_pub.publish(marker_arr); + // marker_arr.markers.clear(); +} + +void disparity_graph::getCamInfo(const sensor_msgs::CameraInfo::ConstPtr &msg_info) { + if (got_cam_info) { + return; + } + image_geometry::PinholeCameraModel model_; + model_.fromCameraInfo(msg_info); + ROS_WARN("Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f Baseline: %f", model_.fx(), model_.fy(), + model_.cx(), model_.cy(), baseline_); + cx_ = model_.cx() / downsample_scale; + cy_ = model_.cy() / downsample_scale; + fx_ = fy_ = model_.fx() / downsample_scale; + width_ = + msg_info->width / downsample_scale; // this is already downsampled from, disp_expansion + height_ = msg_info->height / downsample_scale; + double baseline = -msg_info->P[3] / msg_info->P[0]; + if (baseline != 0.0) { + baseline_ = baseline; + } + baseline_ *= downsample_scale; + ROS_WARN( + "Transformed Cam Info Recvd Fx Fy Cx Cy: %f %f, %f %f Baseline: %f with downsamplescale: " + "%f", + model_.fx(), model_.fy(), model_.cx(), model_.cy(), baseline_, downsample_scale); + + ROS_WARN("Cam Info Constants - Fx Fy Cx Cy: %f %f, %f %f / width=%d - height=%d", fx_, fy_, cx_, + cy_, width_, height_); + + got_cam_info = true; +} + +bool disparity_graph::is_state_valid_depth_pose(geometry_msgs::PoseStamped checked_state, + double thresh, double &occupancy) { + // unsigned int invalid_cntr=0; + occupancy = 0.0; + geometry_msgs::PointStamped checked_point_stamped; + checked_point_stamped.point = checked_state.pose.position; + checked_point_stamped.header = checked_state.header; + + ////check the pose using the footprint_cost check + + geometry_msgs::PointStamped world_point_stamped; + + try { + listener.transformPoint(fixed_frame, checked_point_stamped, world_point_stamped); + } catch (tf::TransformException ex) { + ROS_ERROR("TF to fixed_frame failed: %s", ex.what()); + return false; + } + + tf::Point optical_point; + tf::pointMsgToTF(world_point_stamped.point, optical_point); + + boost::mutex::scoped_lock lock(io_mutex); + bool seen = false; // true; + for (uint i = 0; i < disp_graph.size(); i++) { + tf::Point local_point = disp_graph.at(i).w2s_tf * optical_point; + + float x, y, z; + + x = local_point.getX(); + y = local_point.getY(); + z = local_point.getZ(); + + if (local_point.length() < 1.0) { + seen = true; + continue; + } + // if((-0.20= 0 && u < width_ && v >= 0 && v < height_ && z > 0.0) { + seen = true; + double state_disparity = baseline_ * fx_ / z; + if ((disp_graph.at(i).Im_fg->image.at(v, u) > state_disparity) && + (disp_graph.at(i).Im_bg->image.at(v, u) < state_disparity)) { + occupancy += (state_disparity - 0.5) / state_disparity; + } else { + occupancy -= 0.5 * (state_disparity - 0.5) / state_disparity; + occupancy = occupancy < 0.0 ? 0.0 : occupancy; + } + } + if (/*invalid_cntr>=invalid_thresh*/ occupancy >= thresh) { + return false; + } + } + if (seen) { + return true; + } else { + return false; + } +} + +bool disparity_graph::getStateCost(geometry_msgs::Pose checked_state, double &cost) { + // uint invalid_thresh = 2; + // uint validity = 2; + // unsigned int invalid_cntr=0; + geometry_msgs::PointStamped checked_point_stamped; + checked_point_stamped.header.frame_id = "world"; + checked_point_stamped.header.stamp = ros::Time::now() - ros::Duration(0.1); + checked_point_stamped.point = checked_state.position; + + geometry_msgs::PointStamped world_point_stamped; + // listener.waitForTransform(fixed_frame,checked_point_stamped.header.frame_id,checked_point_stamped.header.stamp,ros::Duration(1.0)); + try { + listener.transformPoint(fixed_frame, checked_point_stamped, world_point_stamped); + } catch (tf::TransformException ex) { + ROS_ERROR("TF to fixed_frame failed: %s", ex.what()); + return false; + } + + tf::Point optical_point; + tf::pointMsgToTF(world_point_stamped.point, optical_point); + + boost::mutex::scoped_lock lock(io_mutex); + for (uint i = 0; i < disp_graph.size(); i++) { + tf::Point local_point = disp_graph.at(i).w2s_tf * optical_point; + + float x, y, z; + x = local_point.getX(); + y = local_point.getY(); + z = local_point.getZ(); + + // point behind camera + if (z < 0) { + continue; + } + + int u = x / z * fx_ + cx_; + int v = y / z * fy_ + cy_; + + if (u >= 0 && u < width_ && v >= 0 && v < height_) { + double state_disparity = baseline_ * fx_ / z; + if ((disp_graph.at(i).Im_fg->image.at(v, u) > state_disparity) && + (disp_graph.at(i).Im_bg->image.at(v, u) < state_disparity)) { + // cost += (fabs(bg_msg->image.at(v,u) - + // state_disparity)/fabs(fg_msg->image.at(v,u) - + // bg_msg->image.at(v,u)) + 1) * 10; + cost += state_disparity * 10.0; + // invalid_cntr++; + } + cost = cost < 0.0 ? 0.0 : cost; + ROS_ERROR_THROTTLE(1, "CHECKING POINT at \t%f\t%f\t%f : cost = %f", + checked_state.position.x, checked_state.position.y, + checked_state.position.z, cost); + + if (cost > 100.0) { + cost = 100.0; + return false; + } + } + } + + return true; +} diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt new file mode 100644 index 000000000..b5776ffa5 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.5) +project(disparity_map_representation) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(stereo_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(core_map_representation_interface REQUIRED) +find_package(disparity_graph REQUIRED) +find_package(tflib REQUIRED) +find_package(pluginlib REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(image_geometry REQUIRED) +find_package(pcl_msgs REQUIRED) + +ament_export_include_directories(include) +ament_export_libraries(disparity_graph) + +include_directories( + ${ament_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${core_map_representation_interface_INCLUDE_DIRS} + ${disparity_graph_INCLUDE_DIRS} + ${pluginlib_INCLUDE_DIRS} + + # ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +add_library(disparity_map_representation + src/disparity_map_representation.cpp +) + +target_link_libraries(disparity_map_representation + disparity_graph + ${pluginlib_LIBRARIES} # Ensure this variable is set correctly +) + +ament_target_dependencies(disparity_map_representation + rclcpp + tf2 + tf2_ros + disparity_graph + core_map_representation_interface + airstack_msgs + cv_bridge + tflib + pluginlib +) + +ament_export_targets(disparity_map_representationTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp + tf2 + tf2_ros + disparity_graph + core_map_representation_interface + airstack_msgs + cv_bridge + tflib + pluginlib +) + +install( + TARGETS disparity_map_representation + EXPORT disparity_map_representationTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(FILES package.xml + DESTINATION share/${PROJECT_NAME} +) +target_link_libraries(disparity_map_representation + ${disparity_graph_LIBRARIES} +) + +# ament_export_include_directories(include) +# ament_export_libraries(disparity_map_representation) +# ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml new file mode 100644 index 000000000..a261b304d --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml @@ -0,0 +1,5 @@ + + + Disparity map representation plugin. + + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp new file mode 100644 index 000000000..18829bc6a --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp @@ -0,0 +1,49 @@ +#ifndef _DISPARITY_MAP_REPRESENTATION_ +#define _DISPARITY_MAP_REPRESENTATION_ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class DisparityMapRepresentation : public MapRepresentation { + private: + std::unique_ptr disp_graph; + + visualization_msgs::msg::MarkerArray markers; + visualization_msgs::msg::Marker points; + + // tf::TransformListener* listener; + std::shared_ptr listener; + + // ros::Publisher debug_pub; + rclcpp::Publisher::SharedPtr debug_pub; + + int obstacle_check_points; + double obstacle_check_radius; + + public: + DisparityMapRepresentation(); + virtual double distance_to_obstacle(geometry_msgs::msg::PoseStamped pose, + tf2::Vector3 direction); + virtual void publish_debug(); + + // virtual std::vector< std::vector > + // get_values(std::vector trajectories); + virtual std::vector > get_values( + std::vector > trajectories); +}; + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml new file mode 100644 index 000000000..077cd3b2b --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml @@ -0,0 +1,37 @@ + + + + disparity_map_representation + 0.0.0 + The disparity_map_representation package + + john + TODO + + ament_cmake + + geometry_msgs + rclcpp + rclpy + sensor_msgs + std_msgs + stereo_msgs + tf2 + tf2_ros + visualization_msgs + nav_msgs + cv_bridge + core_map_representation_interface + disparity_graph + tflib + pluginlib + airstack_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp new file mode 100644 index 000000000..bca030448 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp @@ -0,0 +1,260 @@ +#include +#include + +DisparityMapRepresentation::DisparityMapRepresentation() + : MapRepresentation(), disp_graph(std::make_unique()) { + points.ns = "obstacles"; + points.id = 0; + points.type = visualization_msgs::msg::Marker::SPHERE_LIST; + points.action = visualization_msgs::msg::Marker::ADD; + points.scale.x = 0.1; + points.scale.y = 0.1; + points.scale.z = 0.1; + + // debug_pub = nh->advertise("disparity_map_debug", 1); + debug_pub = + this->create_publisher("disparity_map_debug", 1); + + this->declare_parameter("disparity_map/obstacle_check_points", 5); + this->declare_parameter("disparity_map/obstacle_check_radius", 2.0); + + this->get_parameter("disparity_map/obstacle_check_points", obstacle_check_points); + this->get_parameter("disparity_map/obstacle_check_radius", obstacle_check_radius); + + // listener = new tf::TransformListener(); + std::shared_ptr tf_buffer_; + std::shared_ptr listener_; + tf_buffer_ = std::make_shared(this->get_clock()); + listener_ = std::make_shared(*tf_buffer_); +} + +std::vector > DisparityMapRepresentation::get_values( + std::vector > trajectories) { + std::vector > values(trajectories.size()); + + for (int i = 0; i < trajectories.size(); i++) { + for (int j = 0; j < trajectories[i].size(); j++) { + values[i].push_back(0); + } + } + + for (int i = 0; i < trajectories.size(); i++) { + // airstack_msgs::TrajectoryXYZVYaw trajectory = trajectories[i]; + for (int j = 0; j < trajectories[i].size(); j++) { + tf2::Vector3 wp; + tf2::fromMsg(trajectories[i][j].point, wp); + + // find the direction of the current trajectory segment between waypoints + // this direction will be used to check for obstacles in a perpendicular direction + tf2::Vector3 direction; + tf2::Vector3 wp2 = wp; + if (trajectories[i].size() < 2) { + direction = tf2::Vector3( + 1, 0, + 0); // TODO: make this point in the direction of the waypoints quaternion + } else { + if (j == 0) { + tf2::fromMsg(trajectories[i][1].point, wp2); + direction = wp2 - wp; + } else { + tf2::fromMsg(trajectories[i][j - 1].point, wp); + direction = wp - wp2; + } + } + + points.header.frame_id = trajectories[i][j].header.frame_id; + std_msgs::msg::ColorRGBA green; + green.r = 0; + green.b = 0; + green.g = 1; + green.a = 1; + std_msgs::msg::ColorRGBA red; + red.r = 1; + red.b = 0; + red.g = 0; + red.a = 1; + + tf2::Quaternion q_up, q_down, q_left, q_right; + q_up.setRPY(0, -M_PI / 2, 0); + q_down.setRPY(0, M_PI / 2, 0); + q_left.setRPY(0, 0, M_PI / 2); + q_right.setRPY(0, 0, -M_PI / 2); + std::vector directions; + directions.push_back(q_up); + directions.push_back(q_down); + directions.push_back(q_left); + directions.push_back(q_right); + + tf2::Vector3 position = wp; + tf2::Quaternion q = tf2::Quaternion(0, 0, 0, + 1); // TODO: figure out if this makes sense + // + tf2::Vector3 unit(1, 0, 0); + double closest_obstacle_distance = obstacle_check_radius; + + direction.normalize(); + tf2::Vector3 up = tf2::Transform(q_up * q) * unit; + up.normalize(); + + tf2::Vector3 side = up.cross(direction); + side.normalize(); + + up = side.cross(direction); + up.normalize(); + + std::vector direction_vectors; + direction_vectors.push_back(up); + direction_vectors.push_back(side); + direction_vectors.push_back(-up); + direction_vectors.push_back(-side); + + for (int m = 0; m < directions.size(); m++) { + tf2::Quaternion q_curr = directions[m]; + for (int k = 1; k < obstacle_check_points + 1; k++) { + double dist = + (double)k * obstacle_check_radius / (double)(obstacle_check_points + 1); + + tf2::Vector3 point = + position + + dist * direction_vectors[m]; // tf::Transform(q_curr)*(dist*direction); + geometry_msgs::msg::PoseStamped check_pose; + check_pose.header = trajectories[i][j].header; + check_pose.pose.position = trajectories[i][j].point; + check_pose.pose.orientation.w = 1.0; + + double occupancy; + bool collision = + !disp_graph->is_state_valid_depth_pose(check_pose, 0.9, occupancy); + + std_msgs::msg::ColorRGBA c; + if (collision) { + closest_obstacle_distance = std::min(dist, closest_obstacle_distance); + c = red; + } else + c = green; + + points.points.push_back(check_pose.pose.position); + points.colors.push_back(c); + } + } + + points.points.push_back(trajectories[i][j].point); + + geometry_msgs::msg::PoseStamped pose; + pose.header = trajectories[i][j].header; // trajectory.header; + pose.pose.position = trajectories[i][j].point; + pose.pose.orientation.w = 1.0; + + double occupancy; + if (!disp_graph->is_state_valid_depth_pose(pose, 0.9, occupancy)) { + values[i][j] = 0.f; + points.colors.push_back(red); + } else { + points.colors.push_back(green); + } + + values[i][j] = closest_obstacle_distance; + } + } + + return values; +} + +double DisparityMapRepresentation::distance_to_obstacle(geometry_msgs::msg::PoseStamped pose, + tf2::Vector3 direction) { + points.header.frame_id = pose.header.frame_id; + std_msgs::msg::ColorRGBA green; + green.r = 0; + green.b = 0; + green.g = 1; + green.a = 1; + std_msgs::msg::ColorRGBA red; + red.r = 1; + red.b = 0; + red.g = 0; + red.a = 1; + + tf2::Quaternion q_up, q_down, q_left, q_right; + q_up.setRPY(0, -M_PI / 2, 0); + q_down.setRPY(0, M_PI / 2, 0); + q_left.setRPY(0, 0, M_PI / 2); + q_right.setRPY(0, 0, -M_PI / 2); + std::vector directions; + directions.push_back(q_up); + directions.push_back(q_down); + directions.push_back(q_left); + directions.push_back(q_right); + + tf2::Vector3 position; + tf2::fromMsg(pose.pose.position, position); + tf2::Quaternion q; + tf2::fromMsg(pose.pose.orientation, q); + tf2::Vector3 unit(1, 0, 0); + double closest_obstacle_distance = obstacle_check_radius; + + direction.normalize(); + tf2::Vector3 up = tf2::Transform(q_up * q) * unit; + up.normalize(); + + tf2::Vector3 side = up.cross(direction); + side.normalize(); + + up = side.cross(direction); + up.normalize(); + + std::vector direction_vectors; + direction_vectors.push_back(up); + direction_vectors.push_back(side); + direction_vectors.push_back(-up); + direction_vectors.push_back(-side); + + for (int m = 0; m < directions.size(); m++) { + tf2::Quaternion q_curr = directions[m]; + for (int k = 1; k < obstacle_check_points + 1; k++) { + double dist = (double)k * obstacle_check_radius / (double)(obstacle_check_points + 1); + + tf2::Vector3 point = + position + dist * direction_vectors[m]; // tf::Transform(q_curr)*(dist*direction); + geometry_msgs::msg::PoseStamped check_pose; + check_pose.header = pose.header; + check_pose.pose.position.x = point.x(); + check_pose.pose.position.y = point.y(); + check_pose.pose.position.z = point.z(); + check_pose.pose.orientation.w = 1.0; + + double occupancy; + bool collision = !disp_graph->is_state_valid_depth_pose(check_pose, 0.9, occupancy); + + std_msgs::msg::ColorRGBA c; + if (collision) { + closest_obstacle_distance = std::min(dist, closest_obstacle_distance); + c = red; + } else + c = green; + + points.points.push_back(check_pose.pose.position); + points.colors.push_back(c); + } + } + + points.points.push_back(pose.pose.position); + points.colors.push_back(green); + + double occupancy; + if (!disp_graph->is_state_valid_depth_pose(pose, 0.9, occupancy)) return 0.f; + + return closest_obstacle_distance; +} + +void DisparityMapRepresentation::publish_debug() { + points.header.stamp = this->now(); + markers.markers.push_back(points); + + debug_pub->publish(markers); + + markers.markers.clear(); + points.points.clear(); + points.colors.clear(); +} + +PLUGINLIB_EXPORT_CLASS(DisparityMapRepresentation, MapRepresentation) diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt new file mode 100644 index 000000000..5048c8b84 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(droan_local_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) +find_package(core_map_representation_interface REQUIRED) +find_package(trajectory_controller REQUIRED) +find_package(trajectory_library REQUIRED) +find_package(tflib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +add_executable(droan_local_planner src/droan_local_planner.cpp) +target_include_directories(droan_local_planner PUBLIC + $ + $) +target_compile_features(droan_local_planner PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + droan_local_planner + "airstack_msgs" + "airstack_common" + "core_map_representation_interface" + "trajectory_controller" + "trajectory_library" + "tflib" + "nav_msgs" + "pluginlib" + "rclcpp" + "std_msgs" + "tf2" + "tf2_ros" +) + +install(TARGETS droan_local_planner + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp new file mode 100644 index 000000000..9f57c5a3d --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp @@ -0,0 +1,666 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +// #include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class LocalPlanner : rclcpp::Node { + private: + std::unique_ptr traj_lib; + + std::string map_representation; + bool got_global_plan; + airstack_msgs::msg::TrajectoryXYZVYaw global_plan; + double global_plan_trajectory_distance; + bool got_look_ahead, got_tracking_point; + nav_msgs::msg::Odometry look_ahead_odom, tracking_point_odom; + + std::vector static_trajectories; + + double waypoint_spacing, obstacle_check_radius, obstacle_penalty_weight, + forward_progress_penalty_weight; + double robot_radius; + int obstacle_check_points; + + double look_past_distance; + + float waypoint_buffer_duration, waypoint_spacing_threshold, waypoint_angle_threshold; + std::list waypoint_buffer; + + // bool use_fixed_height; + const int GLOBAL_PLAN_HEIGHT = 0; + const int FIXED_HEIGHT = 1; + const int RANGE_SENSOR_HEIGHT = 2; + int height_mode; + double height_above_ground; + double fixed_height; + bool got_range_up, got_range_down; + sensor_msgs::msg::Range range_up, range_down; + + const int TRAJECTORY_YAW = 0; + const int SMOOTH_YAW = 1; + int yaw_mode; + + // custom waypoint params + enum GoalMode { CUSTOM_WAYPOINT, AUTO_WAYPOINT, TRAJECTORY }; + GoalMode goal_mode; + double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold; + + // MapRepresentationDeprecated* map; + // MapRepresentation* pc_map; + std::shared_ptr pc_map; + + rclcpp::Subscription::SharedPtr global_plan_sub; + rclcpp::Subscription::SharedPtr waypoint_sub; + rclcpp::Subscription::SharedPtr look_ahead_sub; + rclcpp::Subscription::SharedPtr tracking_point_sub; + rclcpp::Subscription::SharedPtr range_up_sub; + rclcpp::Subscription::SharedPtr range_down_sub; + rclcpp::Subscription::SharedPtr custom_waypoint_sub; + // subscribers + // ros::Subscriber global_plan_sub, waypoint_sub, look_ahead_sub, tracking_point_sub, + // range_up_sub, + // range_down_sub, custom_waypoint_sub; + + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener; + + // publishers + // ros::Publisher vis_pub, traj_pub, traj_track_pub, obst_vis_pub, global_plan_vis_pub, + // look_past_vis_pub; + rclcpp::Publisher::SharedPtr vis_pub; + rclcpp::Publisher::SharedPtr traj_pub; + rclcpp::Publisher::SharedPtr traj_track_pub; + rclcpp::Publisher::SharedPtr obst_vis_pub; + rclcpp::Publisher::SharedPtr global_plan_vis_pub; + rclcpp::Publisher::SharedPtr look_past_vis_pub; + + // services + // ros::ServiceClient traj_mode_client; + rclcpp::Client::SharedPtr traj_mode_client; + + public: + LocalPlanner(const std::string node_name) + : Node("droan_local_planner"), + tf_listener(tf_buffer), + goal_mode(TRAJECTORY) + + { + // subscribers + global_plan_sub = this->create_subscription( + "global_plan", 10, std::bind(&LocalPlanner::global_plan_callback, this, _1)); + waypoint_sub = this->create_subscription( + "way_point", 10, std::bind(&LocalPlanner::waypoint_callback, this, _1)); + look_ahead_sub = this->create_subscription( + "look_ahead", 10, std::bind(&LocalPlanner::look_ahead_callback, this, _1)); + tracking_point_sub = this->create_subscription( + "tracking_point", 10, std::bind(&LocalPlanner::tracking_point_callback, this, _1)); + range_up_sub = this->create_subscription( + "range_up", 1, std::bind(&LocalPlanner::range_up_callback, this, _1)); + range_down_sub = this->create_subscription( + "range_down", 1, std::bind(&LocalPlanner::range_down_callback, this, _1)); + custom_waypoint_sub = this->create_subscription( + "custom_waypoint", 1, std::bind(&LocalPlanner::custom_waypoint_callback, this, _1)); + + // publishers + + vis_pub = + this->create_publisher("trajectory_library_vis", 10); + obst_vis_pub = this->create_publisher("obstaccle_vis", 10); + global_plan_vis_pub = this->create_publisher( + "local_planner_global_plan_vis", 10); + look_past_vis_pub = + this->create_publisher("look_past", 10); + traj_pub = this->create_publisher("trajectory", 10); + traj_track_pub = + this->create_publisher("trajectory_track", 10); + + // services + traj_mode_client = + this->create_client("set_trajectory_mode"); + + // init parameters + this->declare_parameter("waypoint_spacing", 0.5); + waypoint_spacing = this->get_parameter("waypoint_spacing").as_double(); + this->declare_parameter("obstacle_penalty_weight", 1.); + obstacle_penalty_weight = this->get_parameter("obstacle_penalty_weight").as_double(); + this->declare_parameter("forward_progress_penalty_weight", 0.5); + forward_progress_penalty_weight = + this->get_parameter("forward_progress_penalty_weight").as_double(); + this->declare_parameter("robot_radius", 0.75); + robot_radius = this->get_parameter("robot_radius").as_double(); + this->declare_parameter("look_past_distance", 0); + look_past_distance = this->get_parameter("look_past_distance").as_double(); + this->declare_parameter("height_mode", 0); + height_mode = this->get_parameter("height_mode").as_int(); + this->declare_parameter("height_above_ground", 1.); + height_above_ground = this->get_parameter("height_above_ground").as_double(); + this->declare_parameter("fixed_height", 1.); + fixed_height = this->get_parameter("fixed_height").as_double(); + this->declare_parameter("yaw_mode", 0); + yaw_mode = this->get_parameter("yaw_mode").as_int(); + this->declare_parameter("map_representation", std::string("PointCloudMapRepresentation")); + map_representation = this->get_parameter("map_representation").as_string(); + this->declare_parameter("waypoint_buffer_duration", 30.); + waypoint_buffer_duration = this->get_parameter("waypoint_buffer_duration").as_double(); + this->declare_parameter("waypoint_spacing_threshold", 0.5); + waypoint_spacing_threshold = this->get_parameter("waypoint_spacing_threshold").as_double(); + this->declare_parameter("waypoint_angle_threshold", 30. * M_PI / 180.); + waypoint_angle_threshold = this->get_parameter("waypoint_angle_threshold").as_double(); + + this->declare_parameter("custom_waypoint_timeout_factor", 0.3); + custom_waypoint_timeout_factor = + this->get_parameter("custom_waypoint_timeout_factor").as_double(); + this->declare_parameter("custom_waypoint_distance_threshold", 0.5); + custom_waypoint_distance_threshold = + this->get_parameter("custom_waypoint_distance_threshold").as_double(); + + this->declare_parameter("trajectory_library_config", std::string("")); + traj_lib = std::make_unique( + this->get_parameter("trajectory_library_config").as_string(), tf_buffer); + } + virtual ~LocalPlanner(); + + virtual bool execute() { + update_waypoint_mode(); + + if (!got_global_plan) return true; + + Trajectory gp(global_plan); + + // set the height of the global plan + if (height_mode == FIXED_HEIGHT) { + gp.set_fixed_height(fixed_height); + } else if (height_mode == RANGE_SENSOR_HEIGHT) { + if (!got_range_up || !got_range_down) return true; + + try { + tf2::Stamped transform_up, transform_down; + + geometry_msgs::msg::TransformStamped tf_up_msg, tf_down_msg; + tf_up_msg = tf_buffer.lookupTransform(range_up.header.frame_id, range_up.header_id, + range_up.header.stamp); + tf2::fromMsg(tf_up_msg, transform_up); + tf_down_msg = tf_buffer.lookupTransform( + range_down.header.frame_id, range_down.header_id, range_down.header.stamp); + tf2::fromMsg(tf_down_msg, transform_down); + + tf2::Vector3 range_up_gp_frame = transform_up * tf2::Vector3(range_up.range, 0, 0); + tf2::Vector3 range_down_gp_frame = + transform_down * tf2::Vector3(range_down.range, 0, 0); + + double tunnel_height = range_up_gp_frame.z() - range_down_gp_frame.z(); + double z_setpoint = (range_up_gp_frame.z() + range_down_gp_frame.z()) / 2.0; + if (tunnel_height / 2.0 >= height_above_ground) { + z_setpoint = range_down_gp_frame.z() + height_above_ground; + } + + gp.set_fixed_height(z_setpoint); + + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR("LocalPlanner", "Failed to get transform: %s", ex.what()); + return true; + } + } + + // transform the look ahead point to the global plan frame + nav_msgs::msg::Odometry look_ahead_odom_global; + // translate to global frame + tf_buffer.transform(look_ahead_odom, look_ahead_odom_global, gp.get_frame_id()); + + tf2::Vector3 look_ahead_position_global; // global frame + tf2::fromMsg(look_ahead_odom.pose.pose.position, look_ahead_position_global); + + // increment how far along the global plan we are + double trajectory_distance; + bool valid = gp.get_trajectory_distance_at_closest_point(look_ahead_position_global, + &trajectory_distance); + if (valid) { + global_plan_trajectory_distance += trajectory_distance; + gp = gp.get_subtrajectory_distance(global_plan_trajectory_distance, + global_plan_trajectory_distance + 10.0); + } else { + RCLCPP_INFO(this->get_logger(), "invalid"); + } + + // publish the segment of the global plan currently being used, for visualization + visualization_msgs::msg::MarkerArray global_markers = gp.get_markers(0, 0, 1); + global_plan_vis_pub->publish(global_markers); + + // get the dynamic trajectories + std::vector dynamic_trajectories = + traj_lib->get_dynamic_trajectories(look_ahead_odom); + + // pick the best trajectory + Trajectory best_traj; + bool all_in_collision = this->get_best_trajectory(dynamic_trajectories, gp, &best_traj); + + // publish the trajectory + if (!all_in_collision) { + airstack_msgs::msg::TrajectoryXYZVYaw path = best_traj.get_TrajectoryXYZVYaw(); + + // set yaw + if (yaw_mode == SMOOTH_YAW && path.waypoints.size() > 0) { + bool found_initial_heading = false; + double initial_heading = 0; + try { + // get the initial heading of the look_ahead_odom in best_traj frame + best_traj.get_frame_id(); + + nav_msgs::msg::Odometry look_ahead_odom_best_traj_frame = + tf_buffer.transform(look_ahead_odom, best_traj.get_frame_id()); + + initial_heading = + tf2::getYaw(look_ahead_odom_best_traj_frame.pose.pose.orientation); + found_initial_heading = true; + + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR("LocalPlanner", "Failed to get transform: %s", ex.what()); + } + + if (found_initial_heading) { + path.waypoints[0].yaw = initial_heading; + double alpha = 0.1; + double sin_yaw_prev = sin(path.waypoints[0].yaw); + double cos_yaw_prev = cos(path.waypoints[0].yaw); + + for (int i = 1; i < path.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw wp_prev = path.waypoints[i - 1]; + airstack_msgs::msg::WaypointXYZVYaw& wp_curr = path.waypoints[i]; + + double yaw = atan2(wp_curr.position.y - wp_prev.position.y, + wp_curr.position.x - wp_prev.position.x); + double cos_yaw = alpha * cos(yaw) + (1 - alpha) * cos_yaw_prev; + double sin_yaw = alpha * sin(yaw) + (1 - alpha) * sin_yaw_prev; + yaw = atan2(sin_yaw, cos_yaw); + + sin_yaw_prev = sin_yaw; + cos_yaw_prev = cos_yaw; + + wp_curr.yaw = yaw; + } + } + } + path.header.stamp = this->now(); + traj_pub->publish(path); + } + return true; + } + + bool get_best_trajectory(std::vector trajs, Trajectory global_plan, + Trajectory* best_traj_ret) { + double min_cost = std::numeric_limits::max(); + int best_traj_index = 0; + bool all_in_collision = true; + + auto now = this->now(); + + visualization_msgs::msg::MarkerArray traj_lib_markers, look_past_markers; + + std::vector> trajectories; + for (int i = 0; i < trajs.size(); i++) { + trajectories.push_back(trajs[i].get_vector_PointStamped()); + } + + std::vector> values = pc_map->get_values(trajectories); + + for (int i = 0; i < trajectories.size(); ++i) { + Trajectory traj = trajectories[i]; + double average_distance = std::numeric_limits::infinity(); + double closest_obstacle_distance = std::numeric_limits::infinity(); + + Trajectory global_plan_in_traj_frame; + try { + global_plan_in_traj_frame = global_plan.transform_to_frame(traj.get_frame_id()); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR("LocalPlanner", + "Failed to transform global plan to trajectory frame: %s", ex.what()); + return true; + } + + for (int j = 0; j < traj.waypoint_count(); j++) { + Waypoint wp = traj.get_waypoint(j); + + nav_msgs::msg::Odometry odom = wp.odometry(now, traj.get_frame_id()); + geometry_msgs::msg::PoseStamped pose; + pose.header = odom.header; + pose.pose = odom.pose.pose; + closest_obstacle_distance = std::min(closest_obstacle_distance, values[i][j]); + + tf2::Vector3 closest_point; + int wp_index; + double path_distance; + bool valid = global_plan_in_traj_frame.get_closest_point( + wp.position(), &closest_point, &wp_index, &path_distance); + double forward_progress_penalty = -forward_progress_penalty_weight * path_distance; + + if (valid) { + if (!std::isfinite(average_distance)) { + average_distance = 0; + } + average_distance += + closest_point.distance(wp.position()) + forward_progress_penalty; + } + } + + average_distance /= traj.waypoint_count(); + bool collision = closest_obstacle_distance <= robot_radius; + if (!collision) { + all_in_collision = false; + } + + visualization_msgs::msg::MarkerArray traj_markers; + if (collision) { + // red for collision + traj_markers = traj.get_markers(1, 0, 0, .5); + } else { + traj_markers = traj.get_markers(0, 1, 0, .5); + } + + if (look_past_distance > 0) { + if (traj.waypoint_count() >= 2) { + Waypoint curr_wp = traj.get_waypoint(traj.waypoint_count() - 1); + Waypoint prev_wp = traj.get_waypoint(traj.waypoint_count() - 2); + + tf2::Vector3 segment = curr_wp.position() - prev_wp.position(); + segment.normalize(); + + tf2::Vector3 position = curr_wp.position() + look_past_distance * segment; + + tf2::Vector3 closest_point; + int wp_index; + bool valid = global_plan_in_traj_frame.get_closest_point( + position, &closest_point, &wp_index); + + if (!valid) { + collision = true; + } else { + average_distance = position.distance(closest_point); + } + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = traj.get_frame_id(); + marker.header.stamp = now; + marker.ns = "look_past"; + marker.id = i; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + marker.pose.orientation.w = 1; + marker.scale.x = 0.3; + marker.scale.y = 0.3; + marker.scale.z = 0.3; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + look_past_markers.markers.push_back(marker); + } + } + traj_lib_markers.markers.insert(traj_lib_markers.markers.end(), + traj_markers.markers.begin(), + traj_markers.markers.end()); + double cost = + average_distance - obstacle_penalty_weight * + std::min(closest_obstacle_distance, obstacle_check_radius); + if (!collision && cost < min_cost) { + min_cost = cost; + best_traj_index = i; + *best_traj_ret = traj; + } + } + if (best_traj_index < look_past_markers.markers.size()) { + look_past_markers.markers[best_traj_index].color.r = 0; + look_past_markers.markers[best_traj_index].color.g = 1; + look_past_markers.markers[best_traj_index].color.b = 0; + look_past_markers.markers[best_traj_index].color.a = 1; + } + + vis_pub->publish(traj_lib_markers); + pc_map->publish_debug(); + look_past_vis_pub->publish(look_past_markers); + + return all_in_collision; + } + + // subscriber callbacks + void global_plan_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr global_plan) { + RCLCPP_INFO_STREAM(this->get_logger(), "GOT GLOBAL PLAN, goal_mode: " << goal_mode); + if (goal_mode != TRAJECTORY) return; + + this->global_plan = global_plan; + got_global_plan = true; + global_plan_trajectory_distance = 0; + } + + void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr wp) { + if (goal_mode != AUTO_WAYPOINT) { + waypoint_buffer.clear(); + waypoint_buffer.push_back(wp); + return; + } + + // remove old waypoints if necessary + waypoint_buffer.push_back(wp); + if ((wp.header.stamp - waypoint_buffer.front().header.stamp).toSec() > + waypoint_buffer_duration) { + waypoint_buffer.pop_front(); + } + + // stitch together the history of waypoints + airstack_msgs::msg::TrajectoryXYZVYaw global_plan; + global_plan.header.frame_id = wp.header.frame_id; + global_plan.header.stamp = wp.header.stamp; + + std::vector backwards_global_plan; + + std::vector prev_wps; + for (auto it = waypoint_buffer.rbegin(); it != waypoint_buffer.rend(); it++) { + geometry_msgs::msg::PointStamped curr_wp = *it; + airstack_msgs::msg::WaypointXYZVYaw waypoint; + waypoint.position.x = curr_wp.point.x; + waypoint.position.y = curr_wp.point.y; + waypoint.position.z = curr_wp.point.z; + + if (prev_wps.size() > 1) { + geometry_msgs::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; + geometry_msgs::PointStamped prev2_wp = prev_wps[prev_wps.size() - 2]; + float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + + pow(curr_wp.point.y - prev_wp.point.y, 2)); + + float a = pow(prev_wp.point.x - curr_wp.point.x, 2) + + pow(prev_wp.point.y - curr_wp.point.y, 2); + float b = pow(prev_wp.point.x - prev2_wp.point.x, 2) + + pow(prev_wp.point.y - prev2_wp.point.y, 2); + float c = pow(prev2_wp.point.x - curr_wp.point.x, 2) + + pow(prev2_wp.point.y - curr_wp.point.y, 2); + float angle = acos((a + b - c) / sqrt(4 * a * b)); + + float angle_diff = fabs(atan2(sin(angle - M_PI), cos(angle - M_PI))); + + // ROS_INFO_STREAM("\tdistance: " << distance << " angle: " << angle*180./M_PI << " + // angle_diff: " << angle_diff*180./M_PI); + + if (distance >= waypoint_spacing_threshold && + angle_diff < waypoint_angle_threshold) { + // ROS_INFO_STREAM("\tADDING wp: " << curr_wp.point.x << " " << curr_wp.point.y + // << " " << curr_wp.point.z); + backwards_global_plan.push_back(waypoint); + prev_wps.push_back(curr_wp); + } + } else { + if (prev_wps.size() == 0) { + prev_wps.push_back(curr_wp); + backwards_global_plan.push_back(waypoint); + } else if (prev_wps.size() == 1) { + geometry_msgs::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; + float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + + pow(curr_wp.point.y - prev_wp.point.y, 2)); + if (distance >= waypoint_spacing_threshold) { + prev_wps.push_back(curr_wp); + backwards_global_plan.push_back(waypoint); + } + } + } + } + for (int i = 0; i < backwards_global_plan.size(); i++) { + global_plan.waypoints.push_back( + backwards_global_plan[backwards_global_plan.size() - 1 - i]); + } + if (got_tracking_point) { + try { + tf2::Vector3 tp_pos = tf2::Vector3(tracking_point_odom.pose.pose.position.x, + tracking_point_odom.pose.pose.position.y, + tracking_point_odom.pose.pose.position.z); + + // translate wp to tracking point frame + geometry_msgs::msg::PointStamped wp_msg_in_tp_frame = + tf_buffer.transform(wp, tracking_point_odom.header.frame_id); + tf2::Vector3 wp_pos_in_tp_frame = + tf2::Vector3(wp_msg_in_tp_frame.point.x, wp_msg_in_tp_frame.point.y, + wp_msg_in_tp_frame.point.z); + + tf2::Vector3 direction = (wp_pos_in_tp_frame - tp_pos).normalized() * 3; + tf2::Vector3 wp2_pos = wp_msg_in_tp_frame + direction; + + airstack_msgs::msg::WaypointXYZVYaw wp1, wp2; + wp1.position.x = wp_pos_in_tp_frame.x(); + wp1.position.y = wp_pos_in_tp_frame.y(); + wp1.position.z = wp_pos_in_tp_frame.z(); + wp2.position.x = wp2_pos.x(); + wp2.position.y = wp2_pos.y(); + wp2.position.z = wp2_pos.z(); + global_plan.waypoints.push_back(wp1); + global_plan.waypoints.push_back(wp2); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR("LocalPlanner", "Failed to get transform: %s", ex.what()); + } + } + + global_plan_trajectory_distance = 0; + this->global_plan = global_plan; + this->got_global_plan = true; + } + + void custom_waypoint_callback(const geometry_msgs::msg::PoseStamped::SharedPtr wp) { + if (!got_look_ahead) return; + + try { + auto wp_msg_in_la_frame = + tf_buffer.transform(wp.pose.position, look_ahead_odom.header.frame_id); + + tf2::Vector3 la_position = tf2::Vector3(look_ahead_odom.pose.pose.position.x, + look_ahead_odom.pose.pose.position.y, + look_ahead_odom.pose.pose.position.z); + tf2::Vector3 wp_position = + tf2::Vector3(wp_msg_in_la_frame.x, wp_msg_in_la_frame.y, wp_msg_in_la_frame.z); + + airstack_msgs::msg::TrajectoryXYZVYaw global_plan; + global_plan.header.frame_id = look_ahead_odom.header.frame_id; + global_plan.header.stamp = this->now(); + + airstack_msgs::msg::WaypointXYZVYaw wp1, wp2; + wp1.position.x = la_position.x(); + wp1.position.y = la_position.y(); + wp1.position.z = la_position.z(); + wp2.position.x = wp_position.x(); + wp2.position.y = wp_position.y(); + wp2.position.z = wp_position.z(); + global_plan.waypoints.push_back(wp1); + global_plan.waypoints.push_back(wp2); + + global_plan_trajectory_distance = 0; + this->global_plan = global_plan; + this->got_global_plan = true; + + goal_mode = CUSTOM_WAYPOINT; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR("LocalPlanner", "Failed to get transform: %s", ex.what()); + } + } + void update_waypoint_mode() { + if (goal_mode == CUSTOM_WAYPOINT) { + if (global_plan.waypoints.size() < 2) goal_mode = AUTO_WAYPOINT; + + // check if the time limit for reaching the waypoint has elapsed + double elapsed_time = (this->now() - global_plan.header.stamp).toSec(); + double distance = 0; + for (int i = 1; i < global_plan.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw prev_wp, curr_wp; + prev_wp = global_plan.waypoints[i - 1]; + curr_wp = global_plan.waypoints[i]; + + distance += sqrt(pow(prev_wp.position.x - curr_wp.position.x, 2) + + pow(prev_wp.position.y - curr_wp.position.y, 2)); + } + + // ROS_INFO_STREAM("elapsed: " << elapsed_time << " / " << + // distance/custom_waypoint_timeout_factor << " distance: " << distance); + if (elapsed_time >= distance / custom_waypoint_timeout_factor) { + // ROS_INFO_STREAM("CUSTOM WAYPOINT TIMEOUT REACHED"); + goal_mode = AUTO_WAYPOINT; + } + + // check if we are close enough to the waypoint + if (got_tracking_point) { + try { + tf2::Vector3 tp_pos = tf2::Vector3(tracking_point_odom.pose.pose.position.x, + tracking_point_odom.pose.pose.position.y, + tracking_point_odom.pose.pose.position.z); + + // translate wp to tracking point frame + geometry_msgs::msg::PointStamped wp_msg_in_tp_frame = tf_buffer.transform( + global_plan.waypoints.back().position, tracking_point_odom.header.frame_id); + tf2::Vector3 wp_pos_in_tp_frame = + tf2::Vector3(wp_msg_in_tp_frame.point.x, wp_msg_in_tp_frame.point.y, + wp_msg_in_tp_frame.point.z); + + if (tp_position.distance(wp_pos_in_tp_frame) < + custom_waypoint_distance_threshold) { + goal_mode = AUTO_WAYPOINT; + } + } catch (tf2::TransformException& ex) { + ROS_ERROR_STREAM("TransformException in update_waypoint_mode: " << ex.what()); + } + } + } + } + void look_ahead_callback(const nav_msgs::msg::Odometry::SharedPtr odom) { + got_look_ahead = true; + look_ahead_odom = odom; + } + void tracking_point_callback(const nav_msgs::msg::Odometry::SharedPtr odom) { + got_tracking_point = true; + tracking_point_odom = odom; + } + void range_up_callback(const sensor_msgs::msg::Range::SharedPtr range) { + got_range_up = true; + range_up = range; + } + void range_down_callback(const sensor_msgs::msg::Range::SharedPtr range) { + got_range_down = true; + range_down = range; + } +}; diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/package.xml b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/package.xml new file mode 100644 index 000000000..213ded957 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/package.xml @@ -0,0 +1,29 @@ + + + + droan_local_planner + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake + + airstack_msgs + trajectory_library + rclcpp + std_msgs + nav_msgs + trajectory_controller + core_map_representation_interface + tf2 + tf2_ros + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/src/droan_local_planner.cpp b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/src/droan_local_planner.cpp new file mode 100644 index 000000000..6b6daa757 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/droan_local_planner/src/droan_local_planner.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world droan_local_planner package\n"); + return 0; +} diff --git a/ros_ws/src/robot/autonomy/planning/local/tflib/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/tflib/CMakeLists.txt new file mode 100644 index 000000000..c7534a9c1 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/tflib/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5) +project(tflib) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +include_directories( + ${rclcpp_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +add_library(tflib src/tflib.cpp) + +target_link_libraries(tflib + ${rclcpp_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ${nav_msgs_LIBRARIES} + ${std_msgs_LIBRARIES} + ${tf2_LIBRARIES} + ${tf2_ros_LIBRARIES} +) + +ament_export_targets(tflibTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(ament_cmake rclcpp sensor_msgs std_msgs nav_msgs +tf2 +tf2_ros +geometry_msgs +) +# install(TARGETS disparity_graph + # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + #) + install( + TARGETS tflib + EXPORT tflibTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) +#install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(FILES package.xml + DESTINATION share/${PROJECT_NAME} +) +#install(DIRECTORY include +## DESTINATION include/${PROJECT_NAME} +#S) +install(DIRECTORY include/ + DESTINATION include/ + FILES_MATCHING PATTERN "*.h" +) + +ament_package() \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/tflib/include/tflib/tflib.h b/ros_ws/src/robot/autonomy/planning/local/tflib/include/tflib/tflib.h new file mode 100644 index 000000000..1930ccd45 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/tflib/include/tflib/tflib.h @@ -0,0 +1,57 @@ +#ifndef _TFLIB_H_ +#define _TFLIB_H_ + +/* +#include +#include +#include +#include +#include +*/ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace tflib { + +// ========================================================================== +// -------------------------------- Conversion ------------------------------ +// ========================================================================== + +tf2::Quaternion to_tf(geometry_msgs::msg::Quaternion q); +tf2::Vector3 to_tf(geometry_msgs::msg::Point p); +tf2::Vector3 to_tf(geometry_msgs::msg::Vector3 v); +geometry_msgs::msg::TransformStamped to_tf(nav_msgs::msg::Odometry odom, std::string frame); + +// ========================================================================== +// --------------------------------- Utils ---------------------------------- +// ========================================================================== + +tf2::Quaternion get_stabilized(tf2::Quaternion q); +tf2::Transform get_stabilized(tf2::Transform transform); +geometry_msgs::msg::TransformStamped get_stabilized(geometry_msgs::msg::TransformStamped transform); +bool transform_odometry(tf2_ros::Buffer* tf_buffer, nav_msgs::msg::Odometry odom, + std::string new_frame_id, std::string new_child_frame_id, + nav_msgs::msg::Odometry* out_odom); +// This can throw any exception that lookupTransform throws +nav_msgs::msg::Odometry transform_odometry(tf2_ros::Buffer* tf_buffer, nav_msgs::msg::Odometry odom, + std::string new_frame_id, std::string new_child_frame_id, + rclcpp::Duration polling_sleep_duration); + +// ========================================================================== +// ------------------------------ Transforms -------------------------------- +// ========================================================================== + +bool to_frame(tf2_ros::Buffer* tf_buffer, tf2::Vector3 vec, std::string source, std::string target, + rclcpp::Time stamp, tf2::Vector3* out, + rclcpp::Duration duration = rclcpp::Duration(0, 100000000)); // 0.1 second; +} // namespace tflib + +#endif diff --git a/ros_ws/src/robot/autonomy/planning/local/tflib/package.xml b/ros_ws/src/robot/autonomy/planning/local/tflib/package.xml new file mode 100644 index 000000000..4cf9afe9d --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/tflib/package.xml @@ -0,0 +1,30 @@ + + + + tflib + 0.0.0 + The tflib package + + john + TODO + ament_cmake + + rclcpp + + rclpy + + geometry_msgs + nav_msgs + std_msgs + tf2 + tf2_ros + + builtin_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/planning/local/tflib/src/tflib.cpp b/ros_ws/src/robot/autonomy/planning/local/tflib/src/tflib.cpp new file mode 100644 index 000000000..7a54a2100 --- /dev/null +++ b/ros_ws/src/robot/autonomy/planning/local/tflib/src/tflib.cpp @@ -0,0 +1,193 @@ +#include + +namespace tflib { + +// ========================================================================== +// -------------------------------- Conversion ------------------------------ +// ========================================================================== + +tf2::Quaternion to_tf(geometry_msgs::msg::Quaternion q) { + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + return quat; +} + +tf2::Vector3 to_tf(geometry_msgs::msg::Point p) { + tf2::Vector3 v(p.x, p.y, p.z); + return v; +} + +tf2::Vector3 to_tf(geometry_msgs::msg::Vector3 v) { + tf2::Vector3 vec(v.x, v.y, v.z); + return vec; +} +/* +geometry_msgs::msg::TransformStamped to_tf(nav_msgs::msg::Odometry odom, std::string frame) { + tf::Transform transform; + transform.setOrigin(to_tf(odom.pose.pose.position)); + transform.setRotation(to_tf(odom.pose.pose.orientation)); + + tf::StampedTransform stamped_transform(transform, odom.header.stamp, odom.header.frame_id, + frame); + return stamped_transform; +} +*/ +geometry_msgs::msg::TransformStamped to_tf(nav_msgs::msg::Odometry odom, std::string frame) { + geometry_msgs::msg::TransformStamped stamped_transform; + stamped_transform.header.stamp = odom.header.stamp; + stamped_transform.header.frame_id = odom.header.frame_id; + stamped_transform.child_frame_id = frame; + stamped_transform.transform.translation.x = odom.pose.pose.position.x; + stamped_transform.transform.translation.y = odom.pose.pose.position.y; + stamped_transform.transform.translation.z = odom.pose.pose.position.z; + stamped_transform.transform.rotation = odom.pose.pose.orientation; + + return stamped_transform; +} +// ========================================================================== +// --------------------------------- Utils ---------------------------------- +// ========================================================================== + +tf2::Quaternion get_stabilized(tf2::Quaternion q) { + tf2::Quaternion stabilized_q; + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + stabilized_q.setRPY(0, 0, yaw); + return stabilized_q; +} + +tf2::Transform get_stabilized(tf2::Transform transform) { + tf2::Transform stabilized_transform; + stabilized_transform.setOrigin(transform.getOrigin()); + stabilized_transform.setRotation(get_stabilized(transform.getRotation())); + return stabilized_transform; +} +/* +tf2::StampedTransform get_stabilized(tf2::StampedTransform transform) { + tf2::StampedTransform stabilized_transform(transform); + stabilized_transform.setRotation(get_stabilized(transform.getRotation())); + return stabilized_transform; +} +*/ +geometry_msgs::msg::TransformStamped get_stabilized( + geometry_msgs::msg::TransformStamped transform) { + geometry_msgs::msg::TransformStamped stabilized_transform = transform; + tf2::Quaternion q, stabilized_q; + tf2::fromMsg(transform.transform.rotation, q); + stabilized_q = get_stabilized(q); + stabilized_transform.transform.rotation = + tf2::toMsg(stabilized_q); + return stabilized_transform; +} +bool transform_odometry(tf2_ros::Buffer* tf_buffer, nav_msgs::msg::Odometry odom, + std::string new_frame_id, std::string new_child_frame_id, + nav_msgs::msg::Odometry* out_odom) { + try { + *out_odom = transform_odometry(tf_buffer, odom, new_frame_id, new_child_frame_id, + rclcpp::Duration(0, 100000000)); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(rclcpp::get_logger("tflib"), "Transform exception in transform_odometry: %s", + ex.what()); + + return false; + } + + return true; +} + +// This can throw any exception that lookupTransform throws +nav_msgs::msg::Odometry transform_odometry(tf2_ros::Buffer* tf_buffer, nav_msgs::msg::Odometry odom, + std::string new_frame_id, std::string new_child_frame_id, + rclcpp::Duration polling_sleep_duration) { + nav_msgs::msg::Odometry out_odom; + geometry_msgs::msg::TransformStamped transform_frame_id, transform_child_frame_id; + /* + listener->waitForTransform(new_frame_id, odom.header.frame_id, odom.header.stamp, + polling_sleep_duration); + listener->lookupTransform(new_frame_id, odom.header.frame_id, odom.header.stamp, + transform_frame_id); + listener->waitForTransform(new_child_frame_id, odom.child_frame_id, odom.header.stamp, + polling_sleep_duration); + listener->lookupTransform(new_child_frame_id, odom.child_frame_id, odom.header.stamp, + transform_child_frame_id); + */ + transform_frame_id = tf_buffer->lookupTransform(new_frame_id, odom.header.frame_id, + odom.header.stamp, polling_sleep_duration); + transform_child_frame_id = tf_buffer->lookupTransform( + new_child_frame_id, odom.child_frame_id, odom.header.stamp, polling_sleep_duration); + + // transform_child_frame_id.setOrigin(tf::Vector3( + // 0, 0, 0)); // don't want translation when transforming linear and angular velocities + transform_child_frame_id.transform.translation.x = 0; + transform_child_frame_id.transform.translation.y = 0; + transform_child_frame_id.transform.translation.z = 0; + + out_odom.header.stamp = odom.header.stamp; + out_odom.header.frame_id = new_frame_id; + out_odom.child_frame_id = new_child_frame_id; + /* + tf2::Vector3 position = transform_frame_id * tflib::to_tf(odom.pose.pose.position); + out_odom.pose.pose.position.x = position.x(); + out_odom.pose.pose.position.y = position.y(); + out_odom.pose.pose.position.z = position.z(); + + tf2::Quaternion quaternion = transform_frame_id * tflib::to_tf(odom.pose.pose.orientation); + out_odom.pose.pose.orientation.x = quaternion.x(); + out_odom.pose.pose.orientation.y = quaternion.y(); + out_odom.pose.pose.orientation.z = quaternion.z(); + out_odom.pose.pose.orientation.w = quaternion.w(); + + tf2::Vector3 velocity = transform_child_frame_id * tflib::to_tf(odom.twist.twist.linear); + out_odom.twist.twist.linear.x = velocity.x(); + out_odom.twist.twist.linear.y = velocity.y(); + out_odom.twist.twist.linear.z = velocity.z(); + + tf2::Vector3 angular = transform_child_frame_id * tflib::to_tf(odom.twist.twist.angular); + out_odom.twist.twist.angular.x = angular.x(); + out_odom.twist.twist.angular.y = angular.y(); + out_odom.twist.twist.angular.z = angular.z(); + */ + // Transform position + tf2::doTransform(odom.pose.pose.position, out_odom.pose.pose.position, transform_frame_id); + + // Transform orientation + tf2::doTransform(odom.pose.pose.orientation, out_odom.pose.pose.orientation, + transform_frame_id); + + // Transform linear velocity + tf2::doTransform(odom.twist.twist.linear, out_odom.twist.twist.linear, + transform_child_frame_id); + + // Transform angular velocity + tf2::doTransform(odom.twist.twist.angular, out_odom.twist.twist.angular, + transform_child_frame_id); + return out_odom; +} + +// ========================================================================== +// ------------------------------ Transforms -------------------------------- +// ========================================================================== + +bool to_frame(tf2_ros::Buffer* tf_buffer, tf2::Vector3 vec, std::string source, std::string target, + rclcpp::Time stamp, tf2::Vector3* out, rclcpp::Duration duration) { + if (out == NULL) return false; + + try { + // tf::StampedTransform transform; + // listener->waitForTransform(target, source, stamp, duration); + // listener->lookupTransform(target, source, stamp, transform); + geometry_msgs::msg::TransformStamped transform = + tf_buffer->lookupTransform(target, source, stamp, duration); + + tf2::Transform tf2_transform; + tf2::fromMsg(transform.transform, tf2_transform); + + *out = tf2_transform * vec; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(rclcpp::get_logger("tflib"), "TransformException in to_frame: %s", ex.what()); + return false; + } + + return true; +} + +} // namespace tflib diff --git a/ros_ws/src/robot/robot_bringup/CMakeLists.txt b/ros_ws/src/robot/robot_bringup/CMakeLists.txt index c7795f507..7f506e7f5 100644 --- a/ros_ws/src/robot/robot_bringup/CMakeLists.txt +++ b/ros_ws/src/robot/robot_bringup/CMakeLists.txt @@ -29,3 +29,4 @@ install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) ament_package() + \ No newline at end of file diff --git a/ros_ws/src/robot/robot_bringup/launch/launch_robot.yaml b/ros_ws/src/robot/robot_bringup/launch/launch_robot.yaml index 1acad2580..433f0aa8d 100644 --- a/ros_ws/src/robot/robot_bringup/launch/launch_robot.yaml +++ b/ros_ws/src/robot/robot_bringup/launch/launch_robot.yaml @@ -3,15 +3,21 @@ launch: - arg: name: "robot_name" default: "robot1" - # include another launch file in the chatter_ns namespace - group: - push_ros_namespace: namespace: "$(var robot_name)" # AUTONOMY - # controls + - node: + pkg: "depth_to_disparity_package" + exec: "depth_to_disparity" + name: "depth_to_disparity" + # local planner - include: - file: "$(find-pkg-share controls_bringup)/launch/launch_controls.yaml" + file: "$(find-pkg-share disparity_expansion)/launch/disparity_expansion.xml" + # controls + # - include: + # file: "$(find-pkg-share controls_bringup)/launch/launch_controls.yaml" # world model - include: file: "$(find-pkg-share vdb_mapping_ros2)/launch/vdb_mapping_ros2.py" @@ -20,4 +26,4 @@ launch: exec: "rviz2" name: "rviz2_robot" namespace: "$(var robot_name)" - args: "-d $(find-pkg-share robot_bringup)/rviz/robot.rviz" + args: "-d $(find-pkg-share robot_bringup)/rviz/robot.rviz --ros-args --log-level INFO" diff --git a/ros_ws/src/robot/robot_bringup/rviz/robot.rviz b/ros_ws/src/robot/robot_bringup/rviz/robot.rviz index c80fe9e9e..2241045f2 100644 --- a/ros_ws/src/robot/robot_bringup/rviz/robot.rviz +++ b/ros_ws/src/robot/robot_bringup/rviz/robot.rviz @@ -6,6 +6,7 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /DisparityExpansion1 Splitter Ratio: 0.5 Tree Height: 385 - Class: rviz_common/Selection @@ -95,6 +96,74 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 3.0580480098724365 + Min Value: 0.06459617614746094 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 3 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot1/perception/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 140 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03999999910593033 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot1/perception/laser_scan + Use Fixed Frame: true + Use rainbow: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -131,21 +200,21 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz_default_plugins/LaserScan + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 50 + Max Intensity: 220 Min Color: 0; 0; 0 - Min Intensity: 1 - Name: LaserScan + Min Intensity: 120 + Name: DisparityExpansion Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.009999999776482582 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -153,10 +222,38 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /robot1/perception/laser_scan + Value: /expansion/cloud_fg Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: expand_disparity_fg + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /narrow_stereo/left/expanded_disparity_fg + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: expanded_disparity_bg + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /narrow_stereo/left/expanded_disparity_bg + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -203,25 +300,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 33.41558074951172 + Distance: 18.20482635498047 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.2950841188430786 - Y: 1.5061166286468506 - Z: -1.1563808917999268 + X: 0.965320885181427 + Y: 1.274168610572815 + Z: 2.3382456302642822 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4103981554508209 + Pitch: 0.40979740023612976 Target Frame: Value: Orbit (rviz) - Yaw: 5.63358736038208 + Yaw: 0.9285785555839539 Saved: ~ Window Geometry: Depth: @@ -231,7 +328,7 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000020a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a00440065007000740068010000024b0000009a0000002800fffffffb0000000600520047004201000002eb000000c90000002800ffffff000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000018a0000050afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000018c000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006801000001cd000000910000002800fffffffb0000000600520047004201000002640000014b0000002800fffffffb000000260065007800700061006e0064005f006400690073007000610072006900740079005f0066006701000003b5000000b20000002800fffffffb0000002a0065007800700061006e006400650064005f006400690073007000610072006900740079005f00620067010000046d000000d80000002800ffffff000000010000015f00000506fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000506000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000082a0000050a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RGB: collapsed: false Selection: @@ -242,6 +339,10 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 360 + Width: 2490 + X: 1150 + Y: 27 + expand_disparity_fg: + collapsed: false + expanded_disparity_bg: + collapsed: false diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/.gitignore b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/.gitignore new file mode 100644 index 000000000..e69de29bb diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/.gitignore b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/.gitignore new file mode 100644 index 000000000..fecc32a81 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/.gitignore @@ -0,0 +1,3 @@ +AscentQLinux/ +logs/*.BIN +logs/*.TXT \ No newline at end of file diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/Makefile b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/Makefile new file mode 100644 index 000000000..bcb26a80b --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/Makefile @@ -0,0 +1,2 @@ +all: + g++ inject.cpp -fPIC -shared -o inject.so diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/README b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/README new file mode 100644 index 000000000..d40407d23 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/README @@ -0,0 +1,5 @@ +install mavproxy +Unzip package +In terminal window, run sudo chmod +x spirit_sitl +run ./spirit_sitl -S --model coaxial -I0 & mavproxy.py --master tcp:127.0.0.1:5760 --out udp:127.0.0.1:14552 +Open AscentQLinux folder and launch the AscentQ file diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/ascent_sitl.yaml b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/ascent_sitl.yaml new file mode 100644 index 000000000..149520f04 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/ascent_sitl.yaml @@ -0,0 +1,17 @@ +session_name: Ascent Spirit +windows: + - window_name: SITL + layout: tiled + panes: + - shell_command: + - echo hi#python3 server_test.py + - shell_command: + - gdb ./spirit_sitl --command=test.gdb -ex "r -S --model coaxial -I0 --base-port $BASE_PORT" + - shell_command: + - echo $ASCENT_SITL_PORT + - echo $ISAAC_SIM_PORT + - echo $AUTONOMY_STACK_PORT + - mavproxy.py --streamrate=100 --master tcp:127.0.0.1:$BASE_PORT --out udp:127.0.0.1:$ASCENT_SITL_PORT --out udp:127.0.0.1:$ISAAC_SIM_PORT --out udp:127.0.0.1:$AUTONOMY_STACK_PORT + - shell_command: + - sleep 5 + - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/eeprom.bin b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/eeprom.bin new file mode 100644 index 000000000..16bd8f46e Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/eeprom.bin differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.cpp b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.cpp new file mode 100644 index 000000000..532885df4 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.cpp @@ -0,0 +1,108 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int sockfd = -1; +struct sockaddr_in servaddr; + +static __attribute__((constructor)) void init(void){ + /* + // Create socket + std::cout << "1" << std::endl; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Socket creation failed" << std::endl; + exit(1); + } + + std::cout << "2" << std::endl; + memset(&servaddr, 0, sizeof(servaddr)); + + // Server information + std::cout << "3" << std::endl; + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(65432); + if (inet_pton(AF_INET, "127.0.0.1", &servaddr.sin_addr) <= 0) { + std::cerr << "Invalid address/ Address not supported" << std::endl; + exit(1); + } + + // Connect to server + std::cout << "4" << std::endl; + if (connect(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) { + std::cerr << "Connection Failed" << std::endl; + exit(1); + } + //*/ +} + +void injected_function(unsigned long i){ + static auto start = std::chrono::high_resolution_clock::now(); + auto elapsed = std::chrono::high_resolution_clock::now() - start; + long long microseconds = std::chrono::duration_cast(elapsed).count(); + static long long prev_microseconds = microseconds; + static long long prev_i = i; + std::cout << "injected function " << microseconds << " " << (microseconds - prev_microseconds) << " " << i << " " << (i - prev_i) << " " << (i - microseconds) << std::endl; + std::cout << "size: " << sizeof(unsigned long) << std::endl; + prev_i = i; + prev_microseconds = microseconds; + + + if (sockfd == -1){ + // Create socket + std::cout << "1" << std::endl; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Socket creation failed" << std::endl; + exit(1); + } + + std::cout << "2" << std::endl; + memset(&servaddr, 0, sizeof(servaddr)); + + // Server information + std::cout << "3" << std::endl; + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(65432); + if (inet_pton(AF_INET, "127.0.0.1", &servaddr.sin_addr) <= 0) { + std::cerr << "Invalid address/ Address not supported" << std::endl; + exit(1); + } + + // Connect to server + std::cout << "4" << std::endl; + if (connect(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) { + std::cerr << "Connection Failed" << std::endl; + exit(1); + } + } + + // Send message + //* + char message_type = 't'; + uint8_t message[16];//sizeof(message_type) + sizeof(i)]; + std::memcpy(&message[0], &message_type, sizeof(message_type)); + std::memcpy(&message[sizeof(i)], &i, sizeof(i)); + send(sockfd, message, sizeof(message), 0); + //send(sockfd, &i, sizeof(i), 0); + + // Receive response + char buffer[1024] = {0}; + int time_to_sleep; + //int valread = read(sockfd, buffer, 1024); + int valread = read(sockfd, &time_to_sleep, sizeof(time_to_sleep)); + //std::cout << "Server: " << buffer << std::endl; + std::cout << "Server: " << time_to_sleep << std::endl; + if(time_to_sleep > 0) + usleep(time_to_sleep); + //*/ +} + + +static __attribute__((destructor)) void end(void){ + close(sockfd); +} diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.so b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.so new file mode 100755 index 000000000..89008f3a7 Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.so differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.parm b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.parm new file mode 100644 index 000000000..c5418a818 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.parm @@ -0,0 +1,1392 @@ +AA_ANG_KP_HI 5.250000 +AA_ANG_KP_IN 7.000000 +AA_ANG_KP_LO 4.000000 +AA_ANG_KP_SL -0.230000 +AA_AUTO_CONF 1.000000 +AA_BATT_CAP 6000.000000 +AA_BATT_NUM 1.000000 +AA_BATT_P 2.000000 +AA_BATT_R_HI 0.021000 +AA_BATT_S 12.000000 +AA_BLD_HUNG 3.000000 +AA_CAM_TYPE 0.000000 +AA_FTC_EN 1.000000 +AA_FTC_ON_RBT 1.000000 +AA_GCS_TYPE 4.000000 +AA_KS_MS 200.000000 +AA_NTRL_ANGL 20.000000 +AA_PAYLD_WT 0.000000 +AA_RAT_KD_HI 0.018000 +AA_RAT_KD_IN 0.004217 +AA_RAT_KD_LO 0.005000 +AA_RAT_KD_SL 0.000273 +AA_RAT_KI_HI 0.290000 +AA_RAT_KI_IN -0.038900 +AA_RAT_KI_LO 0.120000 +AA_RAT_KI_SL 0.022500 +AA_RAT_KP_HI 0.350000 +AA_RAT_KP_IN 0.021100 +AA_RAT_KP_LO 0.180000 +AA_RAT_KP_SL 0.022500 +AA_RPMH_SL 156.000000 +AA_RPMH_SL_HI 4000.000000 +AA_RPMH_SL_IN 1553.000000 +AA_RPMH_SL_LO 2700.000000 +AA_RPM_HOVER 2837.238770 +AA_STW_LND 1.000000 +AA_STW_LND_H 200.000000 +AA_TOP_MNT 0.000000 +AA_TOTAL_WT 8.232301 +AA_TPLE_EN 1.000000 +AA_VEHICLE 1.000000 +AA_VIS_SRC 0.000000 +ACRO_BAL_PITCH 1.000000 +ACRO_BAL_ROLL 1.000000 +ACRO_OPTIONS 0.000000 +ACRO_RP_EXPO 0.300000 +ACRO_RP_RATE 360.000000 +ACRO_RP_RATE_TC 0.000000 +ACRO_THR_MID 0.000000 +ACRO_TRAINER 2.000000 +ACRO_Y_EXPO 0.000000 +ACRO_Y_RATE 202.500000 +ACRO_Y_RATE_TC 0.000000 +ADSB_TYPE 0.000000 +AHRS_COMP_BETA 0.100000 +AHRS_EKF_TYPE 3.000000 +AHRS_GPS_GAIN 1.000000 +AHRS_GPS_MINSATS 6.000000 +AHRS_GPS_USE 1.000000 +AHRS_ORIENTATION 0.000000 +AHRS_RP_P 0.250000 +AHRS_TRIM_X 0.000107 +AHRS_TRIM_Y -0.000116 +AHRS_TRIM_Z 0.000000 +AHRS_WIND_MAX 0.000000 +AHRS_YAW_P 0.200000 +ANGLE_MAX 3000.000000 +ARMING_ACCTHRESH 0.750000 +ARMING_CHECK 1.000000 +ARMING_MIS_ITEMS 0.000000 +ARMING_OPTIONS 0.000000 +ARMING_RUDDER 2.000000 +ARSPD_TYPE 0.000000 +ATC_ACCEL_P_MAX 105909.296875 +ATC_ACCEL_R_MAX 105909.296875 +ATC_ACCEL_Y_MAX 36000.000000 +ATC_ANGLE_BOOST 1.000000 +ATC_ANG_LIM_TC 5.000000 +ATC_ANG_PIT_P 5.106571 +ATC_ANG_RLL_P 5.106571 +ATC_ANG_YAW_P 3.000000 +ATC_INPUT_TC 0.400000 +ATC_PIRO_COMP 1.000000 +ATC_RATE_FF_ENAB 1.000000 +ATC_RATE_P_MAX 120.000000 +ATC_RATE_R_MAX 120.000000 +ATC_RATE_Y_MAX 90.000000 +ATC_RAT_PIT_D 0.006466 +ATC_RAT_PIT_FF 0.000000 +ATC_RAT_PIT_FLTD 15.000000 +ATC_RAT_PIT_FLTE 15.000000 +ATC_RAT_PIT_FLTT 15.000000 +ATC_RAT_PIT_I 0.146327 +ATC_RAT_PIT_IMAX 0.600000 +ATC_RAT_PIT_P 0.206327 +ATC_RAT_PIT_SMAX 0.000000 +ATC_RAT_RLL_D 0.006466 +ATC_RAT_RLL_FF 0.000000 +ATC_RAT_RLL_FLTD 15.000000 +ATC_RAT_RLL_FLTE 15.000000 +ATC_RAT_RLL_FLTT 15.000000 +ATC_RAT_RLL_I 0.146327 +ATC_RAT_RLL_IMAX 0.600000 +ATC_RAT_RLL_P 0.206327 +ATC_RAT_RLL_SMAX 0.000000 +ATC_RAT_YAW_D 0.000000 +ATC_RAT_YAW_FF 0.000000 +ATC_RAT_YAW_FLTD 20.000000 +ATC_RAT_YAW_FLTE 2.500000 +ATC_RAT_YAW_FLTT 5.000000 +ATC_RAT_YAW_I 0.018000 +ATC_RAT_YAW_IMAX 0.300000 +ATC_RAT_YAW_P 0.180000 +ATC_RAT_YAW_SMAX 0.000000 +ATC_SLEW_YAW 6000.000000 +ATC_THR_MIX_MAN 0.500000 +ATC_THR_MIX_MAX 0.500000 +ATC_THR_MIX_MIN 0.100000 +AUTOTUNE_AGGR 0.075000 +AUTOTUNE_AXES 2.000000 +AUTOTUNE_MIN_D 0.002000 +AUTO_OPTIONS 0.000000 +AVD_ENABLE 0.000000 +AVOID_ACCEL_MAX 3.000000 +AVOID_ALT_MIN 0.000000 +AVOID_ANGLE_MAX 1500.000000 +AVOID_BACKUP_DZ 0.100000 +AVOID_BACKUP_SPD 0.750000 +AVOID_BEHAVE 0.000000 +AVOID_DIST_MAX 5.000000 +AVOID_ENABLE 1.000000 +AVOID_MARGIN 2.000000 +BARO1_DEVID 65540.000000 +BARO1_GND_PRESS 94503.125000 +BARO1_WCF_ENABLE 0.000000 +BARO2_DEVID 65796.000000 +BARO2_GND_PRESS 94502.125000 +BARO2_WCF_ENABLE 0.000000 +BARO3_DEVID 0.000000 +BARO3_GND_PRESS 0.000000 +BARO3_WCF_ENABLE 0.000000 +BARO_ALTERR_MAX 2000.000000 +BARO_ALT_OFFSET 0.000000 +BARO_EXT_BUS -1.000000 +BARO_FIELD_ELV 0.000000 +BARO_FLTR_RNG 30.000000 +BARO_GND_TEMP 0.000000 +BARO_OPTIONS 0.000000 +BARO_PRIMARY 0.000000 +BARO_PROBE_EXT 0.000000 +BATT2_MONITOR 0.000000 +BATT3_MONITOR 0.000000 +BATT4_MONITOR 0.000000 +BATT5_MONITOR 0.000000 +BATT6_MONITOR 0.000000 +BATT7_MONITOR 0.000000 +BATT8_MONITOR 0.000000 +BATT9_MONITOR 0.000000 +BATT_AMP_OFFSET 0.000000 +BATT_AMP_PERVLT 17.000000 +BATT_ARM_MAH 0.000000 +BATT_ARM_VOLT 0.000000 +BATT_CAPACITY 6000.000000 +BATT_CRT_MAH 0.000000 +BATT_CRT_VOLT 36.000000 +BATT_CURR_PIN 12.000000 +BATT_FS_CRT_ACT 0.000000 +BATT_FS_LOW_ACT 0.000000 +BATT_FS_VOLTSRC 0.000000 +BATT_LOW_MAH 0.000000 +BATT_LOW_TIMER 10.000000 +BATT_LOW_VOLT 38.000000 +BATT_MONITOR 4.000000 +BATT_OPTIONS 0.000000 +BATT_SERIAL_NUM -1.000000 +BATT_VLT_OFFSET 0.000000 +BATT_VOLT_MULT 10.100000 +BATT_VOLT_PIN 13.000000 +BCN_TYPE 0.000000 +BRD_BOOT_DELAY 0.000000 +BRD_OPTIONS 1.000000 +BRD_RTC_TYPES 1.000000 +BRD_RTC_TZ_MIN 0.000000 +BRD_SAFETYENABLE 0.000000 +BRD_SAFETYOPTION 0.000000 +BRD_SAFETY_MASK 0.000000 +BRD_SERIAL_NUM 0.000000 +BRD_VBUS_MIN 4.300000 +BRD_VSERVO_MIN 4.000000 +BTN_ENABLE 0.000000 +CAM_AUTO_ONLY 0.000000 +CAM_DURATION 10.000000 +CAM_FEEDBACK_PIN -1.000000 +CAM_FEEDBACK_POL 1.000000 +CAM_MAX_ROLL 0.000000 +CAM_MIN_INTERVAL 0.000000 +CAM_RC_TYPE 0.000000 +CAM_RELAY_ON 1.000000 +CAM_SERVO_OFF 1100.000000 +CAM_SERVO_ON 1300.000000 +CAM_TRIGG_DIST 0.000000 +CAM_TRIGG_TYPE 0.000000 +CAM_TYPE 0.000000 +CAN_D1_PROTOCOL 0.000000 +CAN_D2_PROTOCOL 0.000000 +CAN_LOGLEVEL 0.000000 +CAN_P1_DRIVER 0.000000 +CAN_P2_DRIVER 0.000000 +CAN_SLCAN_CPORT 0.000000 +CAN_SLCAN_SDELAY 1.000000 +CAN_SLCAN_SERNUM -1.000000 +CAN_SLCAN_TIMOUT 0.000000 +CC_TYPE 0.000000 +CHUTE_ENABLED 0.000000 +CIRCLE_OPTIONS 5.000000 +CIRCLE_RADIUS 6000.000000 +CIRCLE_RATE 8.000000 +COMPASS_AUTODEC 1.000000 +COMPASS_AUTO_ROT 2.000000 +COMPASS_CAL_FIT 32.000000 +COMPASS_DEC 0.220826 +COMPASS_DEV_ID 97539.000000 +COMPASS_DEV_ID2 131874.000000 +COMPASS_DEV_ID3 263178.000000 +COMPASS_DEV_ID4 97283.000000 +COMPASS_DEV_ID5 97795.000000 +COMPASS_DEV_ID6 98051.000000 +COMPASS_DEV_ID7 0.000000 +COMPASS_DEV_ID8 0.000000 +COMPASS_DIA2_X 1.000000 +COMPASS_DIA2_Y 1.000000 +COMPASS_DIA2_Z 1.000000 +COMPASS_DIA3_X 1.000000 +COMPASS_DIA3_Y 1.000000 +COMPASS_DIA3_Z 1.000000 +COMPASS_DIA_X 1.000000 +COMPASS_DIA_Y 1.000000 +COMPASS_DIA_Z 1.000000 +COMPASS_ENABLE 1.000000 +COMPASS_EXTERN2 0.000000 +COMPASS_EXTERN3 0.000000 +COMPASS_EXTERNAL 1.000000 +COMPASS_FLTR_RNG 30.000000 +COMPASS_LEARN 0.000000 +COMPASS_MOT2_X 0.000000 +COMPASS_MOT2_Y 0.000000 +COMPASS_MOT2_Z 0.000000 +COMPASS_MOT3_X 0.000000 +COMPASS_MOT3_Y 0.000000 +COMPASS_MOT3_Z 0.000000 +COMPASS_MOTCT 0.000000 +COMPASS_MOT_X 0.000000 +COMPASS_MOT_Y 0.000000 +COMPASS_MOT_Z 0.000000 +COMPASS_ODI2_X 0.000000 +COMPASS_ODI2_Y 0.000000 +COMPASS_ODI2_Z 0.000000 +COMPASS_ODI3_X 0.000000 +COMPASS_ODI3_Y 0.000000 +COMPASS_ODI3_Z 0.000000 +COMPASS_ODI_X 0.000000 +COMPASS_ODI_Y 0.000000 +COMPASS_ODI_Z 0.000000 +COMPASS_OFFS_MAX 1800.000000 +COMPASS_OFS2_X 5.000000 +COMPASS_OFS2_Y 13.000000 +COMPASS_OFS2_Z -18.000000 +COMPASS_OFS3_X 5.000000 +COMPASS_OFS3_Y 13.000000 +COMPASS_OFS3_Z -18.000000 +COMPASS_OFS_X 5.000000 +COMPASS_OFS_Y 13.000000 +COMPASS_OFS_Z -18.000000 +COMPASS_OPTIONS 0.000000 +COMPASS_ORIENT 0.000000 +COMPASS_ORIENT2 0.000000 +COMPASS_ORIENT3 0.000000 +COMPASS_PMOT_EN 0.000000 +COMPASS_PRIO1_ID 97539.000000 +COMPASS_PRIO2_ID 131874.000000 +COMPASS_PRIO3_ID 263178.000000 +COMPASS_SCALE 1.000000 +COMPASS_SCALE2 1.000000 +COMPASS_SCALE3 1.000000 +COMPASS_TYPEMASK 0.000000 +COMPASS_USE 1.000000 +COMPASS_USE2 1.000000 +COMPASS_USE3 0.000000 +CUST_ROT_ENABLE 0.000000 +DEV_OPTIONS 0.000000 +DID_ENABLE 0.000000 +DISARM_DELAY 8.000000 +EAHRS_TYPE 0.000000 +EFI_TYPE 0.000000 +EK2_ENABLE 0.000000 +EK3_ABIAS_P_NSE 0.003000 +EK3_ACC_BIAS_LIM 1.000000 +EK3_ACC_P_NSE 0.350000 +EK3_AFFINITY 0.000000 +EK3_ALT_M_NSE 2.000000 +EK3_BCN_DELAY 50.000000 +EK3_BCN_I_GTE 500.000000 +EK3_BCN_M_NSE 1.000000 +EK3_BETA_MASK 0.000000 +EK3_CHECK_SCALE 100.000000 +EK3_DRAG_BCOEF_X 0.000000 +EK3_DRAG_BCOEF_Y 0.000000 +EK3_DRAG_MCOEF 0.000000 +EK3_DRAG_M_NSE 0.500000 +EK3_EAS_I_GATE 400.000000 +EK3_EAS_M_NSE 1.400000 +EK3_ENABLE 1.000000 +EK3_ERR_THRESH 0.200000 +EK3_FLOW_DELAY 10.000000 +EK3_FLOW_I_GATE 300.000000 +EK3_FLOW_M_NSE 0.250000 +EK3_FLOW_USE 1.000000 +EK3_GBIAS_P_NSE 0.001000 +EK3_GLITCH_RAD 25.000000 +EK3_GND_EFF_DZ 4.000000 +EK3_GPS_CHECK 31.000000 +EK3_GPS_VACC_MAX 0.000000 +EK3_GSF_RST_MAX 2.000000 +EK3_GSF_RUN_MASK 7.000000 +EK3_GSF_USE_MASK 7.000000 +EK3_GYRO_P_NSE 0.015000 +EK3_HGT_DELAY 60.000000 +EK3_HGT_I_GATE 500.000000 +EK3_HRT_FILT 2.000000 +EK3_IMU_MASK 7.000000 +EK3_LOG_LEVEL 0.000000 +EK3_MAGB_P_NSE 0.000100 +EK3_MAGE_P_NSE 0.001000 +EK3_MAG_CAL 3.000000 +EK3_MAG_EF_LIM 50.000000 +EK3_MAG_I_GATE 300.000000 +EK3_MAG_MASK 0.000000 +EK3_MAG_M_NSE 0.050000 +EK3_MAX_FLOW 2.500000 +EK3_NOAID_M_NSE 10.000000 +EK3_OGNM_TEST_SF 2.000000 +EK3_OGN_HGT_MASK 0.000000 +EK3_POSNE_M_NSE 0.500000 +EK3_POS_I_GATE 500.000000 +EK3_PRIMARY 1.000000 +EK3_RNG_I_GATE 500.000000 +EK3_RNG_M_NSE 0.500000 +EK3_RNG_USE_HGT -1.000000 +EK3_RNG_USE_SPD 2.000000 +EK3_SRC1_POSXY 3.000000 +EK3_SRC1_POSZ 1.000000 +EK3_SRC1_VELXY 3.000000 +EK3_SRC1_VELZ 3.000000 +EK3_SRC1_YAW 1.000000 +EK3_SRC2_POSXY 0.000000 +EK3_SRC2_POSZ 1.000000 +EK3_SRC2_VELXY 0.000000 +EK3_SRC2_VELZ 0.000000 +EK3_SRC2_YAW 0.000000 +EK3_SRC3_POSXY 0.000000 +EK3_SRC3_POSZ 1.000000 +EK3_SRC3_VELXY 0.000000 +EK3_SRC3_VELZ 0.000000 +EK3_SRC3_YAW 0.000000 +EK3_SRC_OPTIONS 1.000000 +EK3_TAU_OUTPUT 25.000000 +EK3_TERR_GRAD 0.100000 +EK3_VELD_M_NSE 0.500000 +EK3_VELNE_M_NSE 0.300000 +EK3_VEL_I_GATE 500.000000 +EK3_VIS_VERR_MAX 0.900000 +EK3_VIS_VERR_MIN 0.100000 +EK3_WENC_VERR 0.100000 +EK3_WIND_PSCALE 1.000000 +EK3_WIND_P_NSE 0.200000 +EK3_YAW_I_GATE 300.000000 +EK3_YAW_M_NSE 0.500000 +ESC_CALIBRATION 0.000000 +ESC_TLM_MAV_OFS 0.000000 +FENCE_ACTION 0.000000 +FENCE_ALT_MAX 100.000000 +FENCE_ALT_MIN -30.000000 +FENCE_ENABLE 0.000000 +FENCE_MARGIN 2.000000 +FENCE_RADIUS 152.399994 +FENCE_TOTAL 0.000000 +FENCE_TYPE 0.000000 +FFT_ENABLE 0.000000 +FHLD_BRAKE_RATE 8.000000 +FHLD_FILT_HZ 5.000000 +FHLD_FLOW_MAX 0.600000 +FHLD_QUAL_MIN 10.000000 +FHLD_XY_FILT_HZ 5.000000 +FHLD_XY_I 0.300000 +FHLD_XY_IMAX 3000.000000 +FHLD_XY_P 0.200000 +FLIGHT_OPTIONS 0.000000 +FLOW_TYPE 0.000000 +FLTMODE1 2.000000 +FLTMODE2 2.000000 +FLTMODE3 5.000000 +FLTMODE4 6.000000 +FLTMODE5 6.000000 +FLTMODE6 6.000000 +FLTMODE_CH 0.000000 +FOLL_ENABLE 0.000000 +FORMAT_VERSION 120.000000 +FRAME_CLASS 9.000000 +FRAME_TYPE 1.000000 +FRSKY_DNLINK1_ID 20.000000 +FRSKY_DNLINK2_ID 7.000000 +FRSKY_DNLINK_ID 27.000000 +FRSKY_OPTIONS 0.000000 +FRSKY_UPLINK_ID 13.000000 +FS_CRASH_CHECK 1.000000 +FS_DR_ENABLE 0.000000 +FS_DR_TIMEOUT 30.000000 +FS_EKF_ACTION 2.000000 +FS_EKF_THRESH 0.800000 +FS_GCS_ENABLE 1.000000 +FS_GCS_TIMEOUT 5.000000 +FS_OPTIONS 8.000000 +FS_THR_ENABLE 1.000000 +FS_THR_VALUE 950.000000 +FS_VIBE_ENABLE 1.000000 +GCS_PID_MASK 0.000000 +GEN_TYPE 0.000000 +GND_EFFECT_COMP 1.000000 +GPS1_CAN_OVRIDE 0.000000 +GPS2_CAN_OVRIDE 0.000000 +GPS_AUTO_CONFIG 1.000000 +GPS_AUTO_SWITCH 1.000000 +GPS_BLEND_MASK 5.000000 +GPS_BLEND_TC 10.000000 +GPS_CAN_NODEID1 0.000000 +GPS_CAN_NODEID2 0.000000 +GPS_COM_PORT 1.000000 +GPS_COM_PORT2 1.000000 +GPS_DELAY_MS 0.000000 +GPS_DELAY_MS2 0.000000 +GPS_DRV_OPTIONS 0.000000 +GPS_GNSS_MODE 0.000000 +GPS_GNSS_MODE2 0.000000 +GPS_HDOP_GOOD 140.000000 +GPS_INJECT_TO 127.000000 +GPS_MB1_TYPE 0.000000 +GPS_MB2_TYPE 0.000000 +GPS_MIN_DGPS 100.000000 +GPS_MIN_ELEV -100.000000 +GPS_NAVFILTER 8.000000 +GPS_POS1_X 0.000000 +GPS_POS1_Y 0.000000 +GPS_POS1_Z -0.200000 +GPS_POS2_X 0.000000 +GPS_POS2_Y 0.000000 +GPS_POS2_Z 0.000000 +GPS_PRIMARY 0.000000 +GPS_RATE_MS 200.000000 +GPS_RATE_MS2 200.000000 +GPS_RAW_DATA 0.000000 +GPS_SAVE_CFG 2.000000 +GPS_SBAS_MODE 2.000000 +GPS_SBP_LOGMASK -256.000000 +GPS_TYPE 1.000000 +GPS_TYPE2 0.000000 +GRIP_ENABLE 0.000000 +GUID_OPTIONS 0.000000 +GUID_TIMEOUT 3.000000 +INITIAL_MODE 2.000000 +INS_ACC1_CALTEMP 25.018280 +INS_ACC2OFFS_X 0.001000 +INS_ACC2OFFS_Y 0.001000 +INS_ACC2OFFS_Z 0.001000 +INS_ACC2SCAL_X 1.000000 +INS_ACC2SCAL_Y 1.000000 +INS_ACC2SCAL_Z 1.000000 +INS_ACC2_CALTEMP 25.018280 +INS_ACC2_ID 2753036.000000 +INS_ACC3OFFS_X 0.001000 +INS_ACC3OFFS_Y 0.001000 +INS_ACC3OFFS_Z 0.001000 +INS_ACC3SCAL_X 1.000000 +INS_ACC3SCAL_Y 1.000000 +INS_ACC3SCAL_Z 1.000000 +INS_ACC3_CALTEMP -300.000000 +INS_ACC3_ID 2753044.000000 +INS_ACCEL_FILTER 20.000000 +INS_ACCOFFS_X 0.001000 +INS_ACCOFFS_Y 0.001000 +INS_ACCOFFS_Z 0.001000 +INS_ACCSCAL_X 1.000000 +INS_ACCSCAL_Y 1.000000 +INS_ACCSCAL_Z 1.000000 +INS_ACC_BODYFIX 1.000000 +INS_ACC_ID 2753028.000000 +INS_ENABLE_MASK 127.000000 +INS_FAST_SAMPLE 7.000000 +INS_GYR1_CALTEMP 26.901920 +INS_GYR2OFFS_X 0.000474 +INS_GYR2OFFS_Y 0.000467 +INS_GYR2OFFS_Z 0.000444 +INS_GYR2_CALTEMP 26.901920 +INS_GYR2_ID 2752780.000000 +INS_GYR3OFFS_X 0.000471 +INS_GYR3OFFS_Y 0.000466 +INS_GYR3OFFS_Z 0.000460 +INS_GYR3_CALTEMP 26.901920 +INS_GYR3_ID 2752788.000000 +INS_GYROFFS_X 0.000471 +INS_GYROFFS_Y 0.000449 +INS_GYROFFS_Z 0.000465 +INS_GYRO_FILTER 20.000000 +INS_GYRO_RATE 2.000000 +INS_GYR_CAL 0.000000 +INS_GYR_ID 2752772.000000 +INS_HNTC2_ATT 40.000000 +INS_HNTC2_BW 40.000000 +INS_HNTC2_ENABLE 1.000000 +INS_HNTC2_FM_RAT 1.000000 +INS_HNTC2_FREQ 80.000000 +INS_HNTC2_HMNCS 3.000000 +INS_HNTC2_MODE 1.000000 +INS_HNTC2_OPTS 0.000000 +INS_HNTC2_REF 0.000000 +INS_HNTCH_ATT 40.000000 +INS_HNTCH_BW 40.000000 +INS_HNTCH_ENABLE 1.000000 +INS_HNTCH_FM_RAT 1.000000 +INS_HNTCH_FREQ 80.000000 +INS_HNTCH_HMNCS 3.000000 +INS_HNTCH_MODE 1.000000 +INS_HNTCH_OPTS 0.000000 +INS_HNTCH_REF 0.000000 +INS_LOG_BAT_CNT 1024.000000 +INS_LOG_BAT_LGCT 32.000000 +INS_LOG_BAT_LGIN 20.000000 +INS_LOG_BAT_MASK 0.000000 +INS_LOG_BAT_OPT 0.000000 +INS_POS1_X 0.000000 +INS_POS1_Y 0.000000 +INS_POS1_Z 0.150000 +INS_POS2_X 0.000000 +INS_POS2_Y 0.000000 +INS_POS2_Z 0.150000 +INS_POS3_X 0.000000 +INS_POS3_Y 0.000000 +INS_POS3_Z 0.150000 +INS_STILL_THRESH 2.500000 +INS_TCAL1_ENABLE 0.000000 +INS_TCAL2_ENABLE 0.000000 +INS_TCAL3_ENABLE 0.000000 +INS_TCAL_OPTIONS 0.000000 +INS_TRIM_OPTION 1.000000 +INS_USE 1.000000 +INS_USE2 1.000000 +INS_USE3 1.000000 +LAND_ALT_LOW 1200.000000 +LAND_REPOSITION 1.000000 +LAND_SPEED 75.000000 +LAND_SPEED_HIGH 200.000000 +LGR_ENABLE 0.000000 +LOG_BACKEND_TYPE 1.000000 +LOG_BITMASK 161790.000000 +LOG_BLK_RATEMAX 0.000000 +LOG_DISARMED 0.000000 +LOG_FILE_BUFSIZE 32.000000 +LOG_FILE_DSRMROT 0.000000 +LOG_FILE_MB_FREE 500.000000 +LOG_FILE_RATEMAX 0.000000 +LOG_FILE_TIMEOUT 5.000000 +LOG_FORCE_NOLOG 0.000000 +LOG_MAV_BUFSIZE 8.000000 +LOG_MAV_RATEMAX 0.000000 +LOG_REPLAY 0.000000 +LOIT_ACC_MAX 300.000000 +LOIT_ANG_MAX 0.000000 +LOIT_BRK_ACCEL 200.000000 +LOIT_BRK_DELAY 0.500000 +LOIT_BRK_JERK 750.000000 +LOIT_SPEED 1500.000000 +MIS_OPTIONS 0.000000 +MIS_RESTART 0.000000 +MIS_TOTAL 0.000000 +MNT1_TYPE 0.000000 +MNT2_TYPE 0.000000 +MOT_BAT_CURR_MAX 0.000000 +MOT_BAT_CURR_TC 1.000000 +MOT_BAT_IDX 0.000000 +MOT_BAT_VOLT_MAX 0.000000 +MOT_BAT_VOLT_MIN 0.000000 +MOT_BOOST_SCALE 0.000000 +MOT_EN_RPM_COMP 1.000000 +MOT_HOVER_LEARN 1.000000 +MOT_PWM_MAX 1900.000000 +MOT_PWM_MIN 1090.000000 +MOT_PWM_TYPE 0.000000 +MOT_RPM_DEADBAND 400.000000 +MOT_RPM_SCALE 0.400000 +MOT_RP_LAG 0.000000 +MOT_SAFE_DISARM 0.000000 +MOT_SAFE_TIME 1.000000 +MOT_SLEW_DN_TIME 0.250000 +MOT_SLEW_UP_TIME 0.500000 +MOT_SPIN_ARM 0.200000 +MOT_SPIN_MAX 0.990000 +MOT_SPIN_MIN 0.200000 +MOT_SPOOL_TIME 1.000000 +MOT_THST_EXPO 0.500000 +MOT_THST_HOVER 0.400000 +MOT_YAW_HEADROOM 50.000000 +MSP_OPTIONS 0.000000 +MSP_OSD_NCELLS 0.000000 +NTF_BUZZ_ON_LVL 1.000000 +NTF_BUZZ_PIN -1.000000 +NTF_BUZZ_TYPES 0.000000 +NTF_BUZZ_VOLUME 100.000000 +NTF_DISPLAY_TYPE 0.000000 +NTF_LED_BRIGHT 3.000000 +NTF_LED_LEN 1.000000 +NTF_LED_OVERRIDE 0.000000 +NTF_LED_TYPES 8.000000 +OA_TYPE 0.000000 +OSD_TYPE 0.000000 +PHLD_BRAKE_ANGLE 3000.000000 +PHLD_BRAKE_RATE 12.000000 +PILOT_ACCEL_Z 250.000000 +PILOT_SPEED_DN 250.000000 +PILOT_SPEED_UP 400.000000 +PILOT_THR_BHV 3.000000 +PILOT_THR_FILT 3.000000 +PILOT_TKOFF_ALT 500.000000 +PILOT_Y_EXPO 0.000000 +PILOT_Y_RATE 202.500000 +PILOT_Y_RATE_TC 0.000000 +PLND_ENABLED 0.000000 +PRX1_TYPE 0.000000 +PRX2_TYPE 0.000000 +PRX3_TYPE 0.000000 +PRX_FILT 0.250000 +PRX_IGN_GND 0.000000 +PRX_LOG_RAW 0.000000 +PSC_ACCZ_D 0.000000 +PSC_ACCZ_FF 0.000000 +PSC_ACCZ_FLTD 15.000000 +PSC_ACCZ_FLTE 15.000000 +PSC_ACCZ_FLTT 10.000000 +PSC_ACCZ_I 1.000000 +PSC_ACCZ_IMAX 500.000000 +PSC_ACCZ_P 0.500000 +PSC_ACCZ_SMAX 0.000000 +PSC_ANGLE_MAX 0.000000 +PSC_JERK_XY 5.000000 +PSC_JERK_Z 5.000000 +PSC_POSXY_P 0.500000 +PSC_POSZ_P 1.000000 +PSC_VELXY_D 0.300000 +PSC_VELXY_FF 0.000000 +PSC_VELXY_FLTD 10.000000 +PSC_VELXY_FLTE 10.000000 +PSC_VELXY_I 0.400000 +PSC_VELXY_IMAX 500.000000 +PSC_VELXY_P 1.000000 +PSC_VELZ_D 0.000000 +PSC_VELZ_FF 0.000000 +PSC_VELZ_FLTD 5.000000 +PSC_VELZ_FLTE 5.000000 +PSC_VELZ_I 0.000000 +PSC_VELZ_IMAX 1000.000000 +PSC_VELZ_P 2.500000 +RALLY_INCL_HOME 1.000000 +RALLY_LIMIT_KM 0.300000 +RALLY_TOTAL 0.000000 +RC10_DZ 0.000000 +RC10_MAX 1995.000000 +RC10_MIN 1105.000000 +RC10_OPTION 0.000000 +RC10_REVERSED 0.000000 +RC10_TRIM 1105.000000 +RC11_DZ 0.000000 +RC11_MAX 1995.000000 +RC11_MIN 1105.000000 +RC11_OPTION 0.000000 +RC11_REVERSED 0.000000 +RC11_TRIM 1105.000000 +RC12_DZ 0.000000 +RC12_MAX 1900.000000 +RC12_MIN 1100.000000 +RC12_OPTION 0.000000 +RC12_REVERSED 0.000000 +RC12_TRIM 1500.000000 +RC13_DZ 0.000000 +RC13_MAX 1900.000000 +RC13_MIN 1100.000000 +RC13_OPTION 0.000000 +RC13_REVERSED 0.000000 +RC13_TRIM 1500.000000 +RC14_DZ 0.000000 +RC14_MAX 1900.000000 +RC14_MIN 1100.000000 +RC14_OPTION 0.000000 +RC14_REVERSED 0.000000 +RC14_TRIM 1500.000000 +RC15_DZ 0.000000 +RC15_MAX 1900.000000 +RC15_MIN 1100.000000 +RC15_OPTION 0.000000 +RC15_REVERSED 0.000000 +RC15_TRIM 1500.000000 +RC16_DZ 0.000000 +RC16_MAX 1900.000000 +RC16_MIN 1100.000000 +RC16_OPTION 0.000000 +RC16_REVERSED 0.000000 +RC16_TRIM 1500.000000 +RC1_DZ 30.000000 +RC1_MAX 1927.000000 +RC1_MIN 1102.000000 +RC1_OPTION 0.000000 +RC1_REVERSED 0.000000 +RC1_TRIM 1515.000000 +RC2_DZ 30.000000 +RC2_MAX 1927.000000 +RC2_MIN 1102.000000 +RC2_OPTION 0.000000 +RC2_REVERSED 0.000000 +RC2_TRIM 1515.000000 +RC3_DZ 30.000000 +RC3_MAX 1927.000000 +RC3_MIN 1102.000000 +RC3_OPTION 0.000000 +RC3_REVERSED 0.000000 +RC3_TRIM 1515.000000 +RC4_DZ 30.000000 +RC4_MAX 1927.000000 +RC4_MIN 1102.000000 +RC4_OPTION 0.000000 +RC4_REVERSED 0.000000 +RC4_TRIM 1515.000000 +RC5_DZ 0.000000 +RC5_MAX 1900.000000 +RC5_MIN 1100.000000 +RC5_OPTION 0.000000 +RC5_REVERSED 0.000000 +RC5_TRIM 1500.000000 +RC6_DZ 30.000000 +RC6_MAX 1927.000000 +RC6_MIN 1102.000000 +RC6_OPTION 0.000000 +RC6_REVERSED 0.000000 +RC6_TRIM 1515.000000 +RC7_DZ 0.000000 +RC7_MAX 1995.000000 +RC7_MIN 1105.000000 +RC7_OPTION 0.000000 +RC7_REVERSED 0.000000 +RC7_TRIM 1105.000000 +RC8_DZ 0.000000 +RC8_MAX 1995.000000 +RC8_MIN 1105.000000 +RC8_OPTION 0.000000 +RC8_REVERSED 0.000000 +RC8_TRIM 1105.000000 +RC9_DZ 10.000000 +RC9_MAX 1995.000000 +RC9_MIN 1105.000000 +RC9_OPTION 0.000000 +RC9_REVERSED 0.000000 +RC9_TRIM 1105.000000 +RCMAP_PITCH 2.000000 +RCMAP_ROLL 1.000000 +RCMAP_THROTTLE 3.000000 +RCMAP_YAW 4.000000 +RC_OPTIONS 0.000000 +RC_OVERRIDE_TIME 3.000000 +RC_PROTOCOLS 1.000000 +RC_SPEED 490.000000 +RELAY_DEFAULT 0.000000 +RELAY_PIN -1.000000 +RELAY_PIN2 -1.000000 +RELAY_PIN3 -1.000000 +RELAY_PIN4 -1.000000 +RELAY_PIN5 -1.000000 +RELAY_PIN6 -1.000000 +RNGFND1_TYPE 0.000000 +RNGFND2_TYPE 0.000000 +RNGFND3_TYPE 0.000000 +RNGFND4_TYPE 0.000000 +RNGFND5_TYPE 0.000000 +RNGFND6_TYPE 0.000000 +RNGFND7_TYPE 0.000000 +RNGFND8_TYPE 0.000000 +RNGFND9_TYPE 0.000000 +RNGFNDA_TYPE 0.000000 +RNGFND_FILT 0.500000 +RPM1_ESC_MASK 0.000000 +RPM1_MAX 100000.000000 +RPM1_MIN 10.000000 +RPM1_MIN_QUAL 0.500000 +RPM1_PIN -1.000000 +RPM1_SCALING 1.000000 +RPM1_TYPE 10.000000 +RPM2_ESC_MASK 0.000000 +RPM2_MAX 100000.000000 +RPM2_MIN 10.000000 +RPM2_MIN_QUAL 0.500000 +RPM2_PIN -1.000000 +RPM2_SCALING 1.000000 +RPM2_TYPE 10.000000 +RSSI_TYPE 0.000000 +RTL_ALT 1500.000000 +RTL_ALT_FINAL 0.000000 +RTL_ALT_TYPE 0.000000 +RTL_CLIMB_MIN 0.000000 +RTL_CONE_SLOPE 3.000000 +RTL_LOIT_TIME 2000.000000 +RTL_OPTIONS 0.000000 +RTL_SPEED 1500.000000 +SCHED_DEBUG 0.000000 +SCHED_LOOP_RATE 400.000000 +SCHED_OPTIONS 0.000000 +SCR_ENABLE 0.000000 +SERIAL0_BAUD 115.000000 +SERIAL0_PROTOCOL 2.000000 +SERIAL1_BAUD 57.000000 +SERIAL1_OPTIONS 0.000000 +SERIAL1_PROTOCOL 2.000000 +SERIAL2_BAUD 0.000000 +SERIAL2_OPTIONS 0.000000 +SERIAL2_PROTOCOL 0.000000 +SERIAL3_BAUD 38.000000 +SERIAL3_OPTIONS 0.000000 +SERIAL3_PROTOCOL 5.000000 +SERIAL4_BAUD 38.000000 +SERIAL4_OPTIONS 0.000000 +SERIAL4_PROTOCOL 5.000000 +SERIAL5_BAUD 57.000000 +SERIAL5_OPTIONS 0.000000 +SERIAL5_PROTOCOL -1.000000 +SERIAL6_BAUD 57.000000 +SERIAL6_OPTIONS 0.000000 +SERIAL6_PROTOCOL -1.000000 +SERIAL7_BAUD 57.000000 +SERIAL7_OPTIONS 0.000000 +SERIAL7_PROTOCOL -1.000000 +SERIAL_PASS1 0.000000 +SERIAL_PASS2 -1.000000 +SERIAL_PASSTIMO 15.000000 +SERVO10_FUNCTION 0.000000 +SERVO10_MAX 1900.000000 +SERVO10_MIN 1100.000000 +SERVO10_REVERSED 0.000000 +SERVO10_TRIM 1500.000000 +SERVO11_FUNCTION 0.000000 +SERVO11_MAX 1900.000000 +SERVO11_MIN 1100.000000 +SERVO11_REVERSED 0.000000 +SERVO11_TRIM 1100.000000 +SERVO12_FUNCTION 0.000000 +SERVO12_MAX 1900.000000 +SERVO12_MIN 1100.000000 +SERVO12_REVERSED 0.000000 +SERVO12_TRIM 1500.000000 +SERVO13_FUNCTION 0.000000 +SERVO13_MAX 1900.000000 +SERVO13_MIN 1100.000000 +SERVO13_REVERSED 0.000000 +SERVO13_TRIM 0.000000 +SERVO14_FUNCTION 0.000000 +SERVO14_MAX 1900.000000 +SERVO14_MIN 1100.000000 +SERVO14_REVERSED 0.000000 +SERVO14_TRIM 1500.000000 +SERVO15_FUNCTION 0.000000 +SERVO15_MAX 1900.000000 +SERVO15_MIN 1100.000000 +SERVO15_REVERSED 0.000000 +SERVO15_TRIM 1500.000000 +SERVO16_FUNCTION 0.000000 +SERVO16_MAX 1900.000000 +SERVO16_MIN 1100.000000 +SERVO16_REVERSED 0.000000 +SERVO16_TRIM 1500.000000 +SERVO1_FUNCTION 34.000000 +SERVO1_MAX 2100.000000 +SERVO1_MIN 900.000000 +SERVO1_REVERSED 1.000000 +SERVO1_TRIM 1500.000000 +SERVO2_FUNCTION 33.000000 +SERVO2_MAX 2100.000000 +SERVO2_MIN 900.000000 +SERVO2_REVERSED 0.000000 +SERVO2_TRIM 1500.000000 +SERVO3_FUNCTION 35.000000 +SERVO3_MAX 2000.000000 +SERVO3_MIN 1000.000000 +SERVO3_REVERSED 0.000000 +SERVO3_TRIM 1000.000000 +SERVO4_FUNCTION 36.000000 +SERVO4_MAX 2000.000000 +SERVO4_MIN 1000.000000 +SERVO4_REVERSED 0.000000 +SERVO4_TRIM 1000.000000 +SERVO5_FUNCTION 0.000000 +SERVO5_MAX 1801.000000 +SERVO5_MIN 1200.000000 +SERVO5_REVERSED 0.000000 +SERVO5_TRIM 1501.000000 +SERVO6_FUNCTION 0.000000 +SERVO6_MAX 2001.000000 +SERVO6_MIN 1000.000000 +SERVO6_REVERSED 0.000000 +SERVO6_TRIM 1500.000000 +SERVO7_FUNCTION 0.000000 +SERVO7_MAX 1994.000000 +SERVO7_MIN 1003.000000 +SERVO7_REVERSED 0.000000 +SERVO7_TRIM 1504.000000 +SERVO8_FUNCTION 0.000000 +SERVO8_MAX 2001.000000 +SERVO8_MIN 1005.000000 +SERVO8_REVERSED 1.000000 +SERVO8_TRIM 1500.000000 +SERVO9_FUNCTION 0.000000 +SERVO9_MAX 1900.000000 +SERVO9_MIN 1100.000000 +SERVO9_REVERSED 0.000000 +SERVO9_TRIM 1500.000000 +SERVO_32_ENABLE 0.000000 +SERVO_DSHOT_ESC 0.000000 +SERVO_DSHOT_RATE 0.000000 +SERVO_FTW_MASK 0.000000 +SERVO_FTW_POLES 14.000000 +SERVO_FTW_RVMASK 0.000000 +SERVO_GPIO_MASK 32512.000000 +SERVO_RATE 200.000000 +SERVO_ROB_POSMAX 4095.000000 +SERVO_ROB_POSMIN 0.000000 +SERVO_SBUS_RATE 200.000000 +SERVO_VOLZ_MASK 0.000000 +SID_AXIS 0.000000 +SIMPLE 0.000000 +SIM_ACC1_BIAS_X 0.000000 +SIM_ACC1_BIAS_Y 0.000000 +SIM_ACC1_BIAS_Z 0.000000 +SIM_ACC1_RND 0.000000 +SIM_ACC1_SCAL_X 0.000000 +SIM_ACC1_SCAL_Y 0.000000 +SIM_ACC1_SCAL_Z 0.000000 +SIM_ACC2_BIAS_X 0.000000 +SIM_ACC2_BIAS_Y 0.000000 +SIM_ACC2_BIAS_Z 0.000000 +SIM_ACC2_RND 0.000000 +SIM_ACC2_SCAL_X 0.000000 +SIM_ACC2_SCAL_Y 0.000000 +SIM_ACC2_SCAL_Z 0.000000 +SIM_ACC3_BIAS_X 0.000000 +SIM_ACC3_BIAS_Y 0.000000 +SIM_ACC3_BIAS_Z 0.000000 +SIM_ACC3_RND 0.000000 +SIM_ACC3_SCAL_X 0.000000 +SIM_ACC3_SCAL_Y 0.000000 +SIM_ACC3_SCAL_Z 0.000000 +SIM_ACCEL1_FAIL 0.000000 +SIM_ACCEL2_FAIL 0.000000 +SIM_ACCEL3_FAIL 0.000000 +SIM_ACC_FAIL_MSK 0.000000 +SIM_ACC_TRIM_X 0.000000 +SIM_ACC_TRIM_Y 0.000000 +SIM_ACC_TRIM_Z 0.000000 +SIM_ADSB_ALT 1000.000000 +SIM_ADSB_COUNT -1.000000 +SIM_ADSB_RADIUS 10000.000000 +SIM_ADSB_TX 0.000000 +SIM_ARSPD2_FAIL 0.000000 +SIM_ARSPD2_FAILP 0.000000 +SIM_ARSPD2_OFS 2013.000000 +SIM_ARSPD2_PITOT 0.000000 +SIM_ARSPD2_RATIO 1.990000 +SIM_ARSPD2_RND 2.000000 +SIM_ARSPD2_SIGN 0.000000 +SIM_ARSPD_FAIL 0.000000 +SIM_ARSPD_FAILP 0.000000 +SIM_ARSPD_OFS 2013.000000 +SIM_ARSPD_PITOT 0.000000 +SIM_ARSPD_RATIO 1.990000 +SIM_ARSPD_RND 2.000000 +SIM_ARSPD_SIGN 0.000000 +SIM_BAR2_DELAY 0.000000 +SIM_BAR2_DISABLE 0.000000 +SIM_BAR2_DRIFT 0.000000 +SIM_BAR2_FREEZE 0.000000 +SIM_BAR2_GLITCH 0.000000 +SIM_BAR2_RND 0.200000 +SIM_BAR2_WCF_BAK 0.000000 +SIM_BAR2_WCF_FWD 0.000000 +SIM_BAR2_WCF_LFT 0.000000 +SIM_BAR2_WCF_RGT 0.000000 +SIM_BAR3_DELAY 0.000000 +SIM_BAR3_DISABLE 0.000000 +SIM_BAR3_DRIFT 0.000000 +SIM_BAR3_FREEZE 0.000000 +SIM_BAR3_GLITCH 0.000000 +SIM_BAR3_RND 0.200000 +SIM_BAR3_WCF_BAK 0.000000 +SIM_BAR3_WCF_FWD 0.000000 +SIM_BAR3_WCF_LFT 0.000000 +SIM_BAR3_WCF_RGT 0.000000 +SIM_BARO_COUNT 2.000000 +SIM_BARO_DELAY 0.000000 +SIM_BARO_DISABLE 0.000000 +SIM_BARO_DRIFT 0.000000 +SIM_BARO_FREEZE 0.000000 +SIM_BARO_GLITCH 0.000000 +SIM_BARO_RND 0.200000 +SIM_BARO_WCF_BAK 0.000000 +SIM_BARO_WCF_FWD 0.000000 +SIM_BARO_WCF_LFT 0.000000 +SIM_BARO_WCF_RGT 0.000000 +SIM_BATT_CAP_AH 17.000000 +SIM_BATT_NUM 1.000000 +SIM_BATT_RES_OHM 0.070000 +SIM_BATT_VOLTAGE 500.000000 +SIM_BAUDLIMIT_EN 0.000000 +SIM_CORE_KG 2.330000 +SIM_DRIFT_SPEED 0.050000 +SIM_DRIFT_TIME 5.000000 +SIM_EFI_TYPE 0.000000 +SIM_ENGINE_FAIL 0.000000 +SIM_ENGINE_MUL 1.000000 +SIM_ESC_ARM_RPM 0.000000 +SIM_ESC_TELEM 1.000000 +SIM_FLOAT_EXCEPT 1.000000 +SIM_FLOW_DELAY 0.000000 +SIM_FLOW_ENABLE 0.000000 +SIM_FLOW_POS_X 0.000000 +SIM_FLOW_POS_Y 0.000000 +SIM_FLOW_POS_Z 0.000000 +SIM_FLOW_RATE 10.000000 +SIM_FLOW_RND 0.050000 +SIM_FTOWESC_ENA 0.000000 +SIM_FTOWESC_POW 4095.000000 +SIM_GND_BEHAV -1.000000 +SIM_GPS2_ACC 0.300000 +SIM_GPS2_ALT_OFS 0.000000 +SIM_GPS2_BYTELOS 0.000000 +SIM_GPS2_DISABLE 0.000000 +SIM_GPS2_DRFTALT 0.000000 +SIM_GPS2_GLTCH_X 0.000000 +SIM_GPS2_GLTCH_Y 0.000000 +SIM_GPS2_GLTCH_Z 0.000000 +SIM_GPS2_HDG 0.000000 +SIM_GPS2_HZ 5.000000 +SIM_GPS2_LAG_MS 100.000000 +SIM_GPS2_LCKTIME 0.000000 +SIM_GPS2_NOISE 0.000000 +SIM_GPS2_NUMSATS 10.000000 +SIM_GPS2_POS_X 0.000000 +SIM_GPS2_POS_Y 0.000000 +SIM_GPS2_POS_Z 0.000000 +SIM_GPS2_TYPE 1.000000 +SIM_GPS2_VERR_X 0.000000 +SIM_GPS2_VERR_Y 0.000000 +SIM_GPS2_VERR_Z 0.000000 +SIM_GPS_ACC 0.300000 +SIM_GPS_ALT_OFS 0.000000 +SIM_GPS_BYTELOSS 0.000000 +SIM_GPS_DISABLE 0.000000 +SIM_GPS_DRIFTALT 0.000000 +SIM_GPS_GLITCH_X 0.000000 +SIM_GPS_GLITCH_Y 0.000000 +SIM_GPS_GLITCH_Z 0.000000 +SIM_GPS_HDG 0.000000 +SIM_GPS_HZ 5.000000 +SIM_GPS_LAG_MS 100.000000 +SIM_GPS_LOCKTIME 0.000000 +SIM_GPS_NOISE 0.000000 +SIM_GPS_NUMSATS 10.000000 +SIM_GPS_POS_X 0.000000 +SIM_GPS_POS_Y 0.000000 +SIM_GPS_POS_Z 0.000000 +SIM_GPS_TYPE 1.000000 +SIM_GPS_VERR_X 0.000000 +SIM_GPS_VERR_Y 0.000000 +SIM_GPS_VERR_Z 0.000000 +SIM_GRPE_ENABLE 0.000000 +SIM_GRPE_PIN -1.000000 +SIM_GRPS_ENABLE 0.000000 +SIM_GRPS_GRAB 2000.000000 +SIM_GRPS_PIN -1.000000 +SIM_GRPS_RELEASE 1000.000000 +SIM_GRPS_REVERSE 0.000000 +SIM_GYR1_RND 0.000000 +SIM_GYR1_SCALE_X 0.000000 +SIM_GYR1_SCALE_Y 0.000000 +SIM_GYR1_SCALE_Z 0.000000 +SIM_GYR2_RND 0.000000 +SIM_GYR2_SCALE_X 0.000000 +SIM_GYR2_SCALE_Y 0.000000 +SIM_GYR2_SCALE_Z 0.000000 +SIM_GYR3_RND 0.000000 +SIM_GYR3_SCALE_X 0.000000 +SIM_GYR3_SCALE_Y 0.000000 +SIM_GYR3_SCALE_Z 0.000000 +SIM_GYR_FAIL_MSK 0.000000 +SIM_IE24_ENABLE 0.000000 +SIM_IE24_ERROR 0.000000 +SIM_IE24_STATE -1.000000 +SIM_IMUT1_ENABLE 0.000000 +SIM_IMUT2_ENABLE 0.000000 +SIM_IMUT3_ENABLE 0.000000 +SIM_IMUT_END 45.000000 +SIM_IMUT_FIXED 0.000000 +SIM_IMUT_START 25.000000 +SIM_IMUT_TCONST 300.000000 +SIM_IMU_COUNT 3.000000 +SIM_IMU_POS_X 0.000000 +SIM_IMU_POS_Y 0.000000 +SIM_IMU_POS_Z 0.000000 +SIM_INIT_ALT_OFS 0.000000 +SIM_INIT_LAT_OFS 0.000000 +SIM_INIT_LON_OFS 0.000000 +SIM_INS_THR_MIN 0.100000 +SIM_JSON_MASTER 0.000000 +SIM_LED_LAYOUT 0.000000 +SIM_LOOP_DELAY 0.000000 +SIM_MAG1_DEVID 97539.000000 +SIM_MAG1_FAIL 0.000000 +SIM_MAG1_SCALING 1.000000 +SIM_MAG2_DEVID 131874.000000 +SIM_MAG2_DIA_X 0.000000 +SIM_MAG2_DIA_Y 0.000000 +SIM_MAG2_DIA_Z 0.000000 +SIM_MAG2_FAIL 0.000000 +SIM_MAG2_ODI_X 0.000000 +SIM_MAG2_ODI_Y 0.000000 +SIM_MAG2_ODI_Z 0.000000 +SIM_MAG2_OFS_X 5.000000 +SIM_MAG2_OFS_Y 13.000000 +SIM_MAG2_OFS_Z -18.000000 +SIM_MAG2_ORIENT 0.000000 +SIM_MAG2_SCALING 1.000000 +SIM_MAG3_DEVID 263178.000000 +SIM_MAG3_DIA_X 0.000000 +SIM_MAG3_DIA_Y 0.000000 +SIM_MAG3_DIA_Z 0.000000 +SIM_MAG3_FAIL 0.000000 +SIM_MAG3_ODI_X 0.000000 +SIM_MAG3_ODI_Y 0.000000 +SIM_MAG3_ODI_Z 0.000000 +SIM_MAG3_OFS_X 5.000000 +SIM_MAG3_OFS_Y 13.000000 +SIM_MAG3_OFS_Z -18.000000 +SIM_MAG3_ORIENT 0.000000 +SIM_MAG3_SCALING 1.000000 +SIM_MAG4_DEVID 97283.000000 +SIM_MAG5_DEVID 97795.000000 +SIM_MAG6_DEVID 98051.000000 +SIM_MAG7_DEVID 0.000000 +SIM_MAG8_DEVID 0.000000 +SIM_MAG_ALY_HGT 1.000000 +SIM_MAG_ALY_X 0.000000 +SIM_MAG_ALY_Y 0.000000 +SIM_MAG_ALY_Z 0.000000 +SIM_MAG_DELAY 0.000000 +SIM_MAG_DIA_X 0.000000 +SIM_MAG_DIA_Y 0.000000 +SIM_MAG_DIA_Z 0.000000 +SIM_MAG_MOT_X 0.000000 +SIM_MAG_MOT_Y 0.000000 +SIM_MAG_MOT_Z 0.000000 +SIM_MAG_ODI_X 0.000000 +SIM_MAG_ODI_Y 0.000000 +SIM_MAG_ODI_Z 0.000000 +SIM_MAG_OFS_X 5.000000 +SIM_MAG_OFS_Y 13.000000 +SIM_MAG_OFS_Z -18.000000 +SIM_MAG_ORIENT 0.000000 +SIM_MAG_RND 0.000000 +SIM_ODOM_ENABLE 0.000000 +SIM_OH_MASK 0.000000 +SIM_OPOS_ALT 584.000000 +SIM_OPOS_HDG 353.000000 +SIM_OPOS_LAT -35.363258 +SIM_OPOS_LNG 149.165207 +SIM_PARA_ENABLE 0.000000 +SIM_PARA_PIN -1.000000 +SIM_PAYLOAD_KG 0.000000 +SIM_PIN_MASK 0.000000 +SIM_PLD_ALT_LMT 15.000000 +SIM_PLD_DIST_LMT 10.000000 +SIM_PLD_ENABLE 0.000000 +SIM_PLD_HEIGHT 0.000000 +SIM_PLD_LAT 0.000000 +SIM_PLD_LON 0.000000 +SIM_PLD_OPTIONS 0.000000 +SIM_PLD_ORIENT 24.000000 +SIM_PLD_RATE 100.000000 +SIM_PLD_TYPE 0.000000 +SIM_PLD_YAW 0.000000 +SIM_RATE_HZ 1200.000000 +SIM_RC_CHANCOUNT 16.000000 +SIM_RC_FAIL 0.000000 +SIM_RICH_CTRL -1.000000 +SIM_RICH_ENABLE 0.000000 +SIM_SAFETY_STATE 2.000000 +SIM_SAIL_TYPE 0.000000 +SIM_SERVO_SPEED 0.140000 +SIM_SHIP_DSIZE 10.000000 +SIM_SHIP_ENABLE 0.000000 +SIM_SHIP_OFS_X 0.000000 +SIM_SHIP_OFS_Y 0.000000 +SIM_SHIP_OFS_Z 0.000000 +SIM_SHIP_PSIZE 1000.000000 +SIM_SHIP_SPEED 3.000000 +SIM_SHIP_SYSID 17.000000 +SIM_SHOVE_TIME 0.000000 +SIM_SHOVE_X 0.000000 +SIM_SHOVE_Y 0.000000 +SIM_SHOVE_Z 0.000000 +SIM_SONAR_GLITCH 0.000000 +SIM_SONAR_POS_X 0.000000 +SIM_SONAR_POS_Y 0.000000 +SIM_SONAR_POS_Z 0.000000 +SIM_SONAR_RND 0.000000 +SIM_SONAR_ROT 25.000000 +SIM_SONAR_SCALE 12.121200 +SIM_SPEEDUP -1.000000 +SIM_SPR_ENABLE 0.000000 +SIM_SPR_PUMP -1.000000 +SIM_SPR_SPIN -1.000000 +SIM_TA_ENABLE 1.000000 +SIM_TEMP_BFACTOR 0.000000 +SIM_TEMP_BRD_OFF 20.000000 +SIM_TEMP_START 25.000000 +SIM_TEMP_TCONST 30.000000 +SIM_TERRAIN 1.000000 +SIM_THML_SCENARI 0.000000 +SIM_TIDE_DIR 0.000000 +SIM_TIDE_SPEED 0.000000 +SIM_TWIST_TIME 0.000000 +SIM_TWIST_X 0.000000 +SIM_TWIST_Y 0.000000 +SIM_TWIST_Z 0.000000 +SIM_VIB_FREQ_X 0.000000 +SIM_VIB_FREQ_Y 0.000000 +SIM_VIB_FREQ_Z 0.000000 +SIM_VIB_MOT_HMNC 1.000000 +SIM_VIB_MOT_MASK 0.000000 +SIM_VIB_MOT_MAX 0.000000 +SIM_VIB_MOT_MULT 1.000000 +SIM_VICON_FAIL 0.000000 +SIM_VICON_GLIT_X 0.000000 +SIM_VICON_GLIT_Y 0.000000 +SIM_VICON_GLIT_Z 0.000000 +SIM_VICON_POS_X 0.000000 +SIM_VICON_POS_Y 0.000000 +SIM_VICON_POS_Z 0.000000 +SIM_VICON_TMASK 3.000000 +SIM_VICON_VGLI_X 0.000000 +SIM_VICON_VGLI_Y 0.000000 +SIM_VICON_VGLI_Z 0.000000 +SIM_VICON_YAW 0.000000 +SIM_VICON_YAWERR 0.000000 +SIM_WAVE_AMP 0.500000 +SIM_WAVE_DIR 0.000000 +SIM_WAVE_ENABLE 0.000000 +SIM_WAVE_LENGTH 10.000000 +SIM_WAVE_SPEED 0.500000 +SIM_WIND_DIR 180.000000 +SIM_WIND_DIR_Z 0.000000 +SIM_WIND_SPD 0.000000 +SIM_WIND_T 0.000000 +SIM_WIND_TURB 0.000000 +SIM_WIND_T_ALT 60.000000 +SIM_WIND_T_COEF 0.010000 +SIM_WOW_PIN -1.000000 +SPRAY_ENABLE 0.000000 +SR0_ADSB 120.000000 +SR0_EXTRA1 120.000000 +SR0_EXTRA2 120.000000 +SR0_EXTRA3 120.000000 +SR0_EXT_STAT 120.000000 +SR0_PARAMS 0.000000 +SR0_POSITION 120.000000 +SR0_RAW_CTRL 120.000000 +SR0_RAW_SENS 120.000000 +SR0_RC_CHAN 120.000000 +SR1_ADSB 0.000000 +SR1_EXTRA1 0.000000 +SR1_EXTRA2 0.000000 +SR1_EXTRA3 0.000000 +SR1_EXT_STAT 0.000000 +SR1_PARAMS 0.000000 +SR1_POSITION 0.000000 +SR1_RAW_CTRL 0.000000 +SR1_RAW_SENS 0.000000 +SR1_RC_CHAN 0.000000 +SR2_ADSB 0.000000 +SR2_EXTRA1 0.000000 +SR2_EXTRA2 0.000000 +SR2_EXTRA3 0.000000 +SR2_EXT_STAT 0.000000 +SR2_PARAMS 0.000000 +SR2_POSITION 0.000000 +SR2_RAW_CTRL 0.000000 +SR2_RAW_SENS 0.000000 +SR2_RC_CHAN 0.000000 +SR3_ADSB 0.000000 +SR3_EXTRA1 0.000000 +SR3_EXTRA2 0.000000 +SR3_EXTRA3 0.000000 +SR3_EXT_STAT 0.000000 +SR3_PARAMS 0.000000 +SR3_POSITION 0.000000 +SR3_RAW_CTRL 0.000000 +SR3_RAW_SENS 0.000000 +SR3_RC_CHAN 0.000000 +SR4_ADSB 0.000000 +SR4_EXTRA1 0.000000 +SR4_EXTRA2 0.000000 +SR4_EXTRA3 0.000000 +SR4_EXT_STAT 0.000000 +SR4_PARAMS 0.000000 +SR4_POSITION 0.000000 +SR4_RAW_CTRL 0.000000 +SR4_RAW_SENS 0.000000 +SR4_RC_CHAN 0.000000 +SR5_ADSB 0.000000 +SR5_EXTRA1 0.000000 +SR5_EXTRA2 0.000000 +SR5_EXTRA3 0.000000 +SR5_EXT_STAT 0.000000 +SR5_PARAMS 0.000000 +SR5_POSITION 0.000000 +SR5_RAW_CTRL 0.000000 +SR5_RAW_SENS 0.000000 +SR5_RC_CHAN 0.000000 +SR6_ADSB 0.000000 +SR6_EXTRA1 0.000000 +SR6_EXTRA2 0.000000 +SR6_EXTRA3 0.000000 +SR6_EXT_STAT 0.000000 +SR6_PARAMS 0.000000 +SR6_POSITION 0.000000 +SR6_RAW_CTRL 0.000000 +SR6_RAW_SENS 0.000000 +SR6_RC_CHAN 0.000000 +SRTL_ACCURACY 2.000000 +SRTL_OPTIONS 0.000000 +SRTL_POINTS 0.000000 +STAT_BOOTCNT 194.000000 +STAT_FLTTIME 10581.000000 +STAT_RESET 255623488.000000 +STAT_RMFLTTIME1 80881.000000 +STAT_RMFLTTIME2 890881.000000 +STAT_RUNTIME 431602.000000 +SUPER_SIMPLE 0.000000 +SURFTRAK_MODE 1.000000 +SYSID_ENFORCE 0.000000 +SYSID_MYGCS 255.000000 +SYSID_THISMAV 1.000000 +TCAL_ENABLED 0.000000 +TELEM_DELAY 0.000000 +TERRAIN_ENABLE 1.000000 +TERRAIN_MARGIN 0.050000 +TERRAIN_OFS_MAX 15.000000 +TERRAIN_OPTIONS 0.000000 +TERRAIN_SPACING 100.000000 +THROW_MOT_START 0.000000 +THROW_NEXTMODE 18.000000 +THROW_TYPE 0.000000 +THR_DZ 10.000000 +TKOFF_RPM_MIN 0.000000 +TKOFF_SLEW_TIME 1.000000 +TUNE 0.000000 +TUNE_MAX 0.000000 +TUNE_MIN 0.000000 +VISO_TYPE 0.000000 +VTX_ENABLE 0.000000 +WINCH_TYPE 0.000000 +WPNAV_ACCEL 250.000000 +WPNAV_ACCEL_Z 200.000000 +WPNAV_JERK 1.000000 +WPNAV_RADIUS 300.000000 +WPNAV_RFND_USE 0.000000 +WPNAV_SPEED 1500.000000 +WPNAV_SPEED_DN 300.000000 +WPNAV_SPEED_UP 500.000000 +WPNAV_TER_MARGIN 10.000000 +WP_NAVALT_MIN 0.000000 +WP_YAW_BEHAVIOR 2.000000 +ZIGZ_AUTO_ENABLE 0.000000 diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.tlog b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.tlog new file mode 100644 index 000000000..7d7512d96 Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.tlog differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.tlog.raw b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.tlog.raw new file mode 100644 index 000000000..201a7d0dc Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav.tlog.raw differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav_0_1.parm b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav_0_1.parm new file mode 100644 index 000000000..0b482820b --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/mav_0_1.parm @@ -0,0 +1,1392 @@ +AA_ANG_KP_HI 5.250000 +AA_ANG_KP_IN 7.000000 +AA_ANG_KP_LO 4.000000 +AA_ANG_KP_SL -0.230000 +AA_AUTO_CONF 1 +AA_BATT_CAP 6000 +AA_BATT_NUM 1 +AA_BATT_P 2 +AA_BATT_R_HI 0.021000 +AA_BATT_S 12 +AA_BLD_HUNG 3.000000 +AA_CAM_TYPE 0 +AA_FTC_EN 1 +AA_FTC_ON_RBT 1 +AA_GCS_TYPE 4 +AA_KS_MS 200 +AA_NTRL_ANGL 20.000000 +AA_PAYLD_WT 0.000000 +AA_RAT_KD_HI 0.018000 +AA_RAT_KD_IN 0.004217 +AA_RAT_KD_LO 0.005000 +AA_RAT_KD_SL 0.000273 +AA_RAT_KI_HI 0.290000 +AA_RAT_KI_IN -0.038900 +AA_RAT_KI_LO 0.120000 +AA_RAT_KI_SL 0.022500 +AA_RAT_KP_HI 0.350000 +AA_RAT_KP_IN 0.021100 +AA_RAT_KP_LO 0.180000 +AA_RAT_KP_SL 0.022500 +AA_RPMH_SL 156.000000 +AA_RPMH_SL_HI 4000.000000 +AA_RPMH_SL_IN 1553.000000 +AA_RPMH_SL_LO 2700.000000 +AA_RPM_HOVER 2837.238770 +AA_STW_LND 1 +AA_STW_LND_H 200 +AA_TOP_MNT 0 +AA_TOTAL_WT 8.232301 +AA_TPLE_EN 1 +AA_VEHICLE 1 +AA_VIS_SRC 0 +ACRO_BAL_PITCH 1.000000 +ACRO_BAL_ROLL 1.000000 +ACRO_OPTIONS 0 +ACRO_RP_EXPO 0.300000 +ACRO_RP_RATE 360.000000 +ACRO_RP_RATE_TC 0.000000 +ACRO_THR_MID 0.000000 +ACRO_TRAINER 2 +ACRO_Y_EXPO 0.000000 +ACRO_Y_RATE 202.500000 +ACRO_Y_RATE_TC 0.000000 +ADSB_TYPE 0 +AHRS_COMP_BETA 0.100000 +AHRS_EKF_TYPE 3 +AHRS_GPS_GAIN 1.000000 +AHRS_GPS_MINSATS 6 +AHRS_GPS_USE 1 +AHRS_ORIENTATION 0 +AHRS_RP_P 0.250000 +AHRS_TRIM_X 0.000107 +AHRS_TRIM_Y -0.000116 +AHRS_TRIM_Z 0.000000 +AHRS_WIND_MAX 0 +AHRS_YAW_P 0.200000 +ANGLE_MAX 3000 +ARMING_ACCTHRESH 0.750000 +ARMING_CHECK 1 +ARMING_MIS_ITEMS 0 +ARMING_OPTIONS 0 +ARMING_RUDDER 2 +ARSPD_TYPE 0 +ATC_ACCEL_P_MAX 105909.296875 +ATC_ACCEL_R_MAX 105909.296875 +ATC_ACCEL_Y_MAX 36000.000000 +ATC_ANGLE_BOOST 1 +ATC_ANG_LIM_TC 5.000000 +ATC_ANG_PIT_P 5.106571 +ATC_ANG_RLL_P 5.106571 +ATC_ANG_YAW_P 3.000000 +ATC_INPUT_TC 0.400000 +ATC_PIRO_COMP 1 +ATC_RATE_FF_ENAB 1 +ATC_RATE_P_MAX 120.000000 +ATC_RATE_R_MAX 120.000000 +ATC_RATE_Y_MAX 90.000000 +ATC_RAT_PIT_D 0.006466 +ATC_RAT_PIT_FF 0.000000 +ATC_RAT_PIT_FLTD 15.000000 +ATC_RAT_PIT_FLTE 15.000000 +ATC_RAT_PIT_FLTT 15.000000 +ATC_RAT_PIT_I 0.146327 +ATC_RAT_PIT_IMAX 0.600000 +ATC_RAT_PIT_P 0.206327 +ATC_RAT_PIT_SMAX 0.000000 +ATC_RAT_RLL_D 0.006466 +ATC_RAT_RLL_FF 0.000000 +ATC_RAT_RLL_FLTD 15.000000 +ATC_RAT_RLL_FLTE 15.000000 +ATC_RAT_RLL_FLTT 15.000000 +ATC_RAT_RLL_I 0.146327 +ATC_RAT_RLL_IMAX 0.600000 +ATC_RAT_RLL_P 0.206327 +ATC_RAT_RLL_SMAX 0.000000 +ATC_RAT_YAW_D 0.000000 +ATC_RAT_YAW_FF 0.000000 +ATC_RAT_YAW_FLTD 20.000000 +ATC_RAT_YAW_FLTE 2.500000 +ATC_RAT_YAW_FLTT 5.000000 +ATC_RAT_YAW_I 0.018000 +ATC_RAT_YAW_IMAX 0.300000 +ATC_RAT_YAW_P 0.180000 +ATC_RAT_YAW_SMAX 0.000000 +ATC_SLEW_YAW 6000.000000 +ATC_THR_MIX_MAN 0.500000 +ATC_THR_MIX_MAX 0.500000 +ATC_THR_MIX_MIN 0.100000 +AUTOTUNE_AGGR 0.075000 +AUTOTUNE_AXES 2 +AUTOTUNE_MIN_D 0.002000 +AUTO_OPTIONS 0 +AVD_ENABLE 0 +AVOID_ACCEL_MAX 3.000000 +AVOID_ALT_MIN 0.000000 +AVOID_ANGLE_MAX 1500 +AVOID_BACKUP_DZ 0.100000 +AVOID_BACKUP_SPD 0.750000 +AVOID_BEHAVE 0 +AVOID_DIST_MAX 5.000000 +AVOID_ENABLE 1 +AVOID_MARGIN 2.000000 +BARO1_DEVID 65540 +BARO1_GND_PRESS 94503.125000 +BARO1_WCF_ENABLE 0 +BARO2_DEVID 65796 +BARO2_GND_PRESS 94502.125000 +BARO2_WCF_ENABLE 0 +BARO3_DEVID 0 +BARO3_GND_PRESS 0.000000 +BARO3_WCF_ENABLE 0 +BARO_ALTERR_MAX 2000.000000 +BARO_ALT_OFFSET 0.000000 +BARO_EXT_BUS -1 +BARO_FIELD_ELV 0.000000 +BARO_FLTR_RNG 30 +BARO_GND_TEMP 0.000000 +BARO_OPTIONS 0 +BARO_PRIMARY 0 +BARO_PROBE_EXT 0 +BATT2_MONITOR 0 +BATT3_MONITOR 0 +BATT4_MONITOR 0 +BATT5_MONITOR 0 +BATT6_MONITOR 0 +BATT7_MONITOR 0 +BATT8_MONITOR 0 +BATT9_MONITOR 0 +BATT_AMP_OFFSET 0.000000 +BATT_AMP_PERVLT 17.000000 +BATT_ARM_MAH 0 +BATT_ARM_VOLT 0.000000 +BATT_CAPACITY 6000 +BATT_CRT_MAH 0.000000 +BATT_CRT_VOLT 36.000000 +BATT_CURR_PIN 12 +BATT_FS_CRT_ACT 0 +BATT_FS_LOW_ACT 0 +BATT_FS_VOLTSRC 0 +BATT_LOW_MAH 0.000000 +BATT_LOW_TIMER 10 +BATT_LOW_VOLT 38.000000 +BATT_MONITOR 4 +BATT_OPTIONS 0 +BATT_SERIAL_NUM -1 +BATT_VLT_OFFSET 0.000000 +BATT_VOLT_MULT 10.100000 +BATT_VOLT_PIN 13 +BCN_TYPE 0 +BRD_BOOT_DELAY 0 +BRD_OPTIONS 1 +BRD_RTC_TYPES 1 +BRD_RTC_TZ_MIN 0 +BRD_SAFETYENABLE 0 +BRD_SAFETYOPTION 0 +BRD_SAFETY_MASK 0 +BRD_SERIAL_NUM 0 +BRD_VBUS_MIN 4.300000 +BRD_VSERVO_MIN 4.000000 +BTN_ENABLE 0 +CAM_AUTO_ONLY 0 +CAM_DURATION 10 +CAM_FEEDBACK_PIN -1 +CAM_FEEDBACK_POL 1 +CAM_MAX_ROLL 0 +CAM_MIN_INTERVAL 0 +CAM_RC_TYPE 0 +CAM_RELAY_ON 1 +CAM_SERVO_OFF 1100 +CAM_SERVO_ON 1300 +CAM_TRIGG_DIST 0.000000 +CAM_TRIGG_TYPE 0 +CAM_TYPE 0 +CAN_D1_PROTOCOL 0 +CAN_D2_PROTOCOL 0 +CAN_LOGLEVEL 0 +CAN_P1_DRIVER 0 +CAN_P2_DRIVER 0 +CAN_SLCAN_CPORT 0 +CAN_SLCAN_SDELAY 1 +CAN_SLCAN_SERNUM -1 +CAN_SLCAN_TIMOUT 0 +CC_TYPE 0 +CHUTE_ENABLED 0 +CIRCLE_OPTIONS 5 +CIRCLE_RADIUS 6000.000000 +CIRCLE_RATE 8.000000 +COMPASS_AUTODEC 1 +COMPASS_AUTO_ROT 2 +COMPASS_CAL_FIT 32.000000 +COMPASS_DEC 0.220826 +COMPASS_DEV_ID 97539 +COMPASS_DEV_ID2 131874 +COMPASS_DEV_ID3 263178 +COMPASS_DEV_ID4 97283 +COMPASS_DEV_ID5 97795 +COMPASS_DEV_ID6 98051 +COMPASS_DEV_ID7 0 +COMPASS_DEV_ID8 0 +COMPASS_DIA2_X 1.000000 +COMPASS_DIA2_Y 1.000000 +COMPASS_DIA2_Z 1.000000 +COMPASS_DIA3_X 1.000000 +COMPASS_DIA3_Y 1.000000 +COMPASS_DIA3_Z 1.000000 +COMPASS_DIA_X 1.000000 +COMPASS_DIA_Y 1.000000 +COMPASS_DIA_Z 1.000000 +COMPASS_ENABLE 1 +COMPASS_EXTERN2 0 +COMPASS_EXTERN3 0 +COMPASS_EXTERNAL 1 +COMPASS_FLTR_RNG 30 +COMPASS_LEARN 0 +COMPASS_MOT2_X 0.000000 +COMPASS_MOT2_Y 0.000000 +COMPASS_MOT2_Z 0.000000 +COMPASS_MOT3_X 0.000000 +COMPASS_MOT3_Y 0.000000 +COMPASS_MOT3_Z 0.000000 +COMPASS_MOTCT 0 +COMPASS_MOT_X 0.000000 +COMPASS_MOT_Y 0.000000 +COMPASS_MOT_Z 0.000000 +COMPASS_ODI2_X 0.000000 +COMPASS_ODI2_Y 0.000000 +COMPASS_ODI2_Z 0.000000 +COMPASS_ODI3_X 0.000000 +COMPASS_ODI3_Y 0.000000 +COMPASS_ODI3_Z 0.000000 +COMPASS_ODI_X 0.000000 +COMPASS_ODI_Y 0.000000 +COMPASS_ODI_Z 0.000000 +COMPASS_OFFS_MAX 1800 +COMPASS_OFS2_X 5.000000 +COMPASS_OFS2_Y 13.000000 +COMPASS_OFS2_Z -18.000000 +COMPASS_OFS3_X 5.000000 +COMPASS_OFS3_Y 13.000000 +COMPASS_OFS3_Z -18.000000 +COMPASS_OFS_X 5.000000 +COMPASS_OFS_Y 13.000000 +COMPASS_OFS_Z -18.000000 +COMPASS_OPTIONS 0 +COMPASS_ORIENT 0 +COMPASS_ORIENT2 0 +COMPASS_ORIENT3 0 +COMPASS_PMOT_EN 0 +COMPASS_PRIO1_ID 97539 +COMPASS_PRIO2_ID 131874 +COMPASS_PRIO3_ID 263178 +COMPASS_SCALE 1.000000 +COMPASS_SCALE2 1.000000 +COMPASS_SCALE3 1.000000 +COMPASS_TYPEMASK 0 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 0 +CUST_ROT_ENABLE 0 +DEV_OPTIONS 0 +DID_ENABLE 0 +DISARM_DELAY 8 +EAHRS_TYPE 0 +EFI_TYPE 0 +EK2_ENABLE 0 +EK3_ABIAS_P_NSE 0.003000 +EK3_ACC_BIAS_LIM 1.000000 +EK3_ACC_P_NSE 0.350000 +EK3_AFFINITY 0 +EK3_ALT_M_NSE 2.000000 +EK3_BCN_DELAY 50 +EK3_BCN_I_GTE 500 +EK3_BCN_M_NSE 1.000000 +EK3_BETA_MASK 0 +EK3_CHECK_SCALE 100 +EK3_DRAG_BCOEF_X 0.000000 +EK3_DRAG_BCOEF_Y 0.000000 +EK3_DRAG_MCOEF 0.000000 +EK3_DRAG_M_NSE 0.500000 +EK3_EAS_I_GATE 400 +EK3_EAS_M_NSE 1.400000 +EK3_ENABLE 1 +EK3_ERR_THRESH 0.200000 +EK3_FLOW_DELAY 10 +EK3_FLOW_I_GATE 300 +EK3_FLOW_M_NSE 0.250000 +EK3_FLOW_USE 1 +EK3_GBIAS_P_NSE 0.001000 +EK3_GLITCH_RAD 25 +EK3_GND_EFF_DZ 4.000000 +EK3_GPS_CHECK 31 +EK3_GPS_VACC_MAX 0.000000 +EK3_GSF_RST_MAX 2 +EK3_GSF_RUN_MASK 7 +EK3_GSF_USE_MASK 7 +EK3_GYRO_P_NSE 0.015000 +EK3_HGT_DELAY 60 +EK3_HGT_I_GATE 500 +EK3_HRT_FILT 2.000000 +EK3_IMU_MASK 7 +EK3_LOG_LEVEL 0 +EK3_MAGB_P_NSE 0.000100 +EK3_MAGE_P_NSE 0.001000 +EK3_MAG_CAL 3 +EK3_MAG_EF_LIM 50 +EK3_MAG_I_GATE 300 +EK3_MAG_MASK 0 +EK3_MAG_M_NSE 0.050000 +EK3_MAX_FLOW 2.500000 +EK3_NOAID_M_NSE 10.000000 +EK3_OGNM_TEST_SF 2.000000 +EK3_OGN_HGT_MASK 0 +EK3_POSNE_M_NSE 0.500000 +EK3_POS_I_GATE 500 +EK3_PRIMARY 1 +EK3_RNG_I_GATE 500 +EK3_RNG_M_NSE 0.500000 +EK3_RNG_USE_HGT -1 +EK3_RNG_USE_SPD 2.000000 +EK3_SRC1_POSXY 3 +EK3_SRC1_POSZ 1 +EK3_SRC1_VELXY 3 +EK3_SRC1_VELZ 3 +EK3_SRC1_YAW 1 +EK3_SRC2_POSXY 0 +EK3_SRC2_POSZ 1 +EK3_SRC2_VELXY 0 +EK3_SRC2_VELZ 0 +EK3_SRC2_YAW 0 +EK3_SRC3_POSXY 0 +EK3_SRC3_POSZ 1 +EK3_SRC3_VELXY 0 +EK3_SRC3_VELZ 0 +EK3_SRC3_YAW 0 +EK3_SRC_OPTIONS 1 +EK3_TAU_OUTPUT 25 +EK3_TERR_GRAD 0.100000 +EK3_VELD_M_NSE 0.500000 +EK3_VELNE_M_NSE 0.300000 +EK3_VEL_I_GATE 500 +EK3_VIS_VERR_MAX 0.900000 +EK3_VIS_VERR_MIN 0.100000 +EK3_WENC_VERR 0.100000 +EK3_WIND_PSCALE 1.000000 +EK3_WIND_P_NSE 0.200000 +EK3_YAW_I_GATE 300 +EK3_YAW_M_NSE 0.500000 +ESC_CALIBRATION 0 +ESC_TLM_MAV_OFS 0 +FENCE_ACTION 0 +FENCE_ALT_MAX 100.000000 +FENCE_ALT_MIN -30.000000 +FENCE_ENABLE 0 +FENCE_MARGIN 2.000000 +FENCE_RADIUS 152.399994 +FENCE_TOTAL 0 +FENCE_TYPE 0 +FFT_ENABLE 0 +FHLD_BRAKE_RATE 8 +FHLD_FILT_HZ 5.000000 +FHLD_FLOW_MAX 0.600000 +FHLD_QUAL_MIN 10 +FHLD_XY_FILT_HZ 5.000000 +FHLD_XY_I 0.300000 +FHLD_XY_IMAX 3000.000000 +FHLD_XY_P 0.200000 +FLIGHT_OPTIONS 0 +FLOW_TYPE 0 +FLTMODE1 2 +FLTMODE2 2 +FLTMODE3 5 +FLTMODE4 6 +FLTMODE5 6 +FLTMODE6 6 +FLTMODE_CH 0 +FOLL_ENABLE 0 +FORMAT_VERSION 120 +FRAME_CLASS 9 +FRAME_TYPE 1 +FRSKY_DNLINK1_ID 20 +FRSKY_DNLINK2_ID 7 +FRSKY_DNLINK_ID 27 +FRSKY_OPTIONS 0 +FRSKY_UPLINK_ID 13 +FS_CRASH_CHECK 1 +FS_DR_ENABLE 0 +FS_DR_TIMEOUT 30 +FS_EKF_ACTION 2 +FS_EKF_THRESH 0.800000 +FS_GCS_ENABLE 1 +FS_GCS_TIMEOUT 5.000000 +FS_OPTIONS 8 +FS_THR_ENABLE 1 +FS_THR_VALUE 950 +FS_VIBE_ENABLE 1 +GCS_PID_MASK 0 +GEN_TYPE 0 +GND_EFFECT_COMP 1 +GPS1_CAN_OVRIDE 0 +GPS2_CAN_OVRIDE 0 +GPS_AUTO_CONFIG 1 +GPS_AUTO_SWITCH 1 +GPS_BLEND_MASK 5 +GPS_BLEND_TC 10.000000 +GPS_CAN_NODEID1 0 +GPS_CAN_NODEID2 0 +GPS_COM_PORT 1 +GPS_COM_PORT2 1 +GPS_DELAY_MS 0 +GPS_DELAY_MS2 0 +GPS_DRV_OPTIONS 0 +GPS_GNSS_MODE 0 +GPS_GNSS_MODE2 0 +GPS_HDOP_GOOD 140 +GPS_INJECT_TO 127 +GPS_MB1_TYPE 0 +GPS_MB2_TYPE 0 +GPS_MIN_DGPS 100 +GPS_MIN_ELEV -100 +GPS_NAVFILTER 8 +GPS_POS1_X 0.000000 +GPS_POS1_Y 0.000000 +GPS_POS1_Z -0.200000 +GPS_POS2_X 0.000000 +GPS_POS2_Y 0.000000 +GPS_POS2_Z 0.000000 +GPS_PRIMARY 0 +GPS_RATE_MS 200 +GPS_RATE_MS2 200 +GPS_RAW_DATA 0 +GPS_SAVE_CFG 2 +GPS_SBAS_MODE 2 +GPS_SBP_LOGMASK -256 +GPS_TYPE 1 +GPS_TYPE2 0 +GRIP_ENABLE 0 +GUID_OPTIONS 0 +GUID_TIMEOUT 3.000000 +INITIAL_MODE 2 +INS_ACC1_CALTEMP 25.018280 +INS_ACC2OFFS_X 0.001000 +INS_ACC2OFFS_Y 0.001000 +INS_ACC2OFFS_Z 0.001000 +INS_ACC2SCAL_X 1.000000 +INS_ACC2SCAL_Y 1.000000 +INS_ACC2SCAL_Z 1.000000 +INS_ACC2_CALTEMP 25.018280 +INS_ACC2_ID 2753036 +INS_ACC3OFFS_X 0.001000 +INS_ACC3OFFS_Y 0.001000 +INS_ACC3OFFS_Z 0.001000 +INS_ACC3SCAL_X 1.000000 +INS_ACC3SCAL_Y 1.000000 +INS_ACC3SCAL_Z 1.000000 +INS_ACC3_CALTEMP -300.000000 +INS_ACC3_ID 2753044 +INS_ACCEL_FILTER 20 +INS_ACCOFFS_X 0.001000 +INS_ACCOFFS_Y 0.001000 +INS_ACCOFFS_Z 0.001000 +INS_ACCSCAL_X 1.000000 +INS_ACCSCAL_Y 1.000000 +INS_ACCSCAL_Z 1.000000 +INS_ACC_BODYFIX 1 +INS_ACC_ID 2753028 +INS_ENABLE_MASK 127 +INS_FAST_SAMPLE 7 +INS_GYR1_CALTEMP 26.901920 +INS_GYR2OFFS_X 0.000474 +INS_GYR2OFFS_Y 0.000467 +INS_GYR2OFFS_Z 0.000444 +INS_GYR2_CALTEMP 26.901920 +INS_GYR2_ID 2752780 +INS_GYR3OFFS_X 0.000471 +INS_GYR3OFFS_Y 0.000466 +INS_GYR3OFFS_Z 0.000460 +INS_GYR3_CALTEMP 26.901920 +INS_GYR3_ID 2752788 +INS_GYROFFS_X 0.000471 +INS_GYROFFS_Y 0.000449 +INS_GYROFFS_Z 0.000465 +INS_GYRO_FILTER 20 +INS_GYRO_RATE 2 +INS_GYR_CAL 0 +INS_GYR_ID 2752772 +INS_HNTC2_ATT 40.000000 +INS_HNTC2_BW 40.000000 +INS_HNTC2_ENABLE 1 +INS_HNTC2_FM_RAT 1.000000 +INS_HNTC2_FREQ 80.000000 +INS_HNTC2_HMNCS 3 +INS_HNTC2_MODE 1 +INS_HNTC2_OPTS 0 +INS_HNTC2_REF 0.000000 +INS_HNTCH_ATT 40.000000 +INS_HNTCH_BW 40.000000 +INS_HNTCH_ENABLE 1 +INS_HNTCH_FM_RAT 1.000000 +INS_HNTCH_FREQ 80.000000 +INS_HNTCH_HMNCS 3 +INS_HNTCH_MODE 1 +INS_HNTCH_OPTS 0 +INS_HNTCH_REF 0.000000 +INS_LOG_BAT_CNT 1024 +INS_LOG_BAT_LGCT 32 +INS_LOG_BAT_LGIN 20 +INS_LOG_BAT_MASK 0 +INS_LOG_BAT_OPT 0 +INS_POS1_X 0.000000 +INS_POS1_Y 0.000000 +INS_POS1_Z 0.150000 +INS_POS2_X 0.000000 +INS_POS2_Y 0.000000 +INS_POS2_Z 0.150000 +INS_POS3_X 0.000000 +INS_POS3_Y 0.000000 +INS_POS3_Z 0.150000 +INS_STILL_THRESH 2.500000 +INS_TCAL1_ENABLE 0 +INS_TCAL2_ENABLE 0 +INS_TCAL3_ENABLE 0 +INS_TCAL_OPTIONS 0 +INS_TRIM_OPTION 1 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +LAND_ALT_LOW 1200 +LAND_REPOSITION 1 +LAND_SPEED 75 +LAND_SPEED_HIGH 200 +LGR_ENABLE 0 +LOG_BACKEND_TYPE 1 +LOG_BITMASK 161790 +LOG_BLK_RATEMAX 0.000000 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 32 +LOG_FILE_DSRMROT 0 +LOG_FILE_MB_FREE 500 +LOG_FILE_RATEMAX 0.000000 +LOG_FILE_TIMEOUT 5 +LOG_FORCE_NOLOG 0 +LOG_MAV_BUFSIZE 8 +LOG_MAV_RATEMAX 0.000000 +LOG_REPLAY 0 +LOIT_ACC_MAX 300.000000 +LOIT_ANG_MAX 0.000000 +LOIT_BRK_ACCEL 200.000000 +LOIT_BRK_DELAY 0.500000 +LOIT_BRK_JERK 750.000000 +LOIT_SPEED 1500.000000 +MIS_OPTIONS 0 +MIS_RESTART 0 +MIS_TOTAL 0 +MNT1_TYPE 0 +MNT2_TYPE 0 +MOT_BAT_CURR_MAX 0.000000 +MOT_BAT_CURR_TC 1.000000 +MOT_BAT_IDX 0 +MOT_BAT_VOLT_MAX 0.000000 +MOT_BAT_VOLT_MIN 0.000000 +MOT_BOOST_SCALE 0.000000 +MOT_EN_RPM_COMP 1 +MOT_HOVER_LEARN 1 +MOT_PWM_MAX 1900 +MOT_PWM_MIN 1090 +MOT_PWM_TYPE 0 +MOT_RPM_DEADBAND 400.000000 +MOT_RPM_SCALE 0.400000 +MOT_RP_LAG 0.000000 +MOT_SAFE_DISARM 0 +MOT_SAFE_TIME 1.000000 +MOT_SLEW_DN_TIME 0.250000 +MOT_SLEW_UP_TIME 0.500000 +MOT_SPIN_ARM 0.200000 +MOT_SPIN_MAX 0.990000 +MOT_SPIN_MIN 0.200000 +MOT_SPOOL_TIME 1.000000 +MOT_THST_EXPO 0.500000 +MOT_THST_HOVER 0.400000 +MOT_YAW_HEADROOM 50 +MSP_OPTIONS 0 +MSP_OSD_NCELLS 0 +NTF_BUZZ_ON_LVL 1 +NTF_BUZZ_PIN -1 +NTF_BUZZ_TYPES 0 +NTF_BUZZ_VOLUME 100 +NTF_DISPLAY_TYPE 0 +NTF_LED_BRIGHT 3 +NTF_LED_LEN 1 +NTF_LED_OVERRIDE 0 +NTF_LED_TYPES 8 +OA_TYPE 0 +OSD_TYPE 0 +PHLD_BRAKE_ANGLE 3000 +PHLD_BRAKE_RATE 12 +PILOT_ACCEL_Z 250 +PILOT_SPEED_DN 250 +PILOT_SPEED_UP 400 +PILOT_THR_BHV 3 +PILOT_THR_FILT 3.000000 +PILOT_TKOFF_ALT 500.000000 +PILOT_Y_EXPO 0.000000 +PILOT_Y_RATE 202.500000 +PILOT_Y_RATE_TC 0.000000 +PLND_ENABLED 0 +PRX1_TYPE 0 +PRX2_TYPE 0 +PRX3_TYPE 0 +PRX_FILT 0.250000 +PRX_IGN_GND 0 +PRX_LOG_RAW 0 +PSC_ACCZ_D 0.000000 +PSC_ACCZ_FF 0.000000 +PSC_ACCZ_FLTD 15.000000 +PSC_ACCZ_FLTE 15.000000 +PSC_ACCZ_FLTT 10.000000 +PSC_ACCZ_I 1.000000 +PSC_ACCZ_IMAX 500.000000 +PSC_ACCZ_P 0.500000 +PSC_ACCZ_SMAX 0.000000 +PSC_ANGLE_MAX 0.000000 +PSC_JERK_XY 5.000000 +PSC_JERK_Z 5.000000 +PSC_POSXY_P 0.500000 +PSC_POSZ_P 1.000000 +PSC_VELXY_D 0.300000 +PSC_VELXY_FF 0.000000 +PSC_VELXY_FLTD 10.000000 +PSC_VELXY_FLTE 10.000000 +PSC_VELXY_I 0.400000 +PSC_VELXY_IMAX 500.000000 +PSC_VELXY_P 1.000000 +PSC_VELZ_D 0.000000 +PSC_VELZ_FF 0.000000 +PSC_VELZ_FLTD 5.000000 +PSC_VELZ_FLTE 5.000000 +PSC_VELZ_I 0.000000 +PSC_VELZ_IMAX 1000.000000 +PSC_VELZ_P 2.500000 +RALLY_INCL_HOME 1 +RALLY_LIMIT_KM 0.300000 +RALLY_TOTAL 0 +RC10_DZ 0 +RC10_MAX 1995 +RC10_MIN 1105 +RC10_OPTION 0 +RC10_REVERSED 0 +RC10_TRIM 1105 +RC11_DZ 0 +RC11_MAX 1995 +RC11_MIN 1105 +RC11_OPTION 0 +RC11_REVERSED 0 +RC11_TRIM 1105 +RC12_DZ 0 +RC12_MAX 1900 +RC12_MIN 1100 +RC12_OPTION 0 +RC12_REVERSED 0 +RC12_TRIM 1500 +RC13_DZ 0 +RC13_MAX 1900 +RC13_MIN 1100 +RC13_OPTION 0 +RC13_REVERSED 0 +RC13_TRIM 1500 +RC14_DZ 0 +RC14_MAX 1900 +RC14_MIN 1100 +RC14_OPTION 0 +RC14_REVERSED 0 +RC14_TRIM 1500 +RC15_DZ 0 +RC15_MAX 1900 +RC15_MIN 1100 +RC15_OPTION 0 +RC15_REVERSED 0 +RC15_TRIM 1500 +RC16_DZ 0 +RC16_MAX 1900 +RC16_MIN 1100 +RC16_OPTION 0 +RC16_REVERSED 0 +RC16_TRIM 1500 +RC1_DZ 30 +RC1_MAX 1927 +RC1_MIN 1102 +RC1_OPTION 0 +RC1_REVERSED 0 +RC1_TRIM 1515 +RC2_DZ 30 +RC2_MAX 1927 +RC2_MIN 1102 +RC2_OPTION 0 +RC2_REVERSED 0 +RC2_TRIM 1515 +RC3_DZ 30 +RC3_MAX 1927 +RC3_MIN 1102 +RC3_OPTION 0 +RC3_REVERSED 0 +RC3_TRIM 1515 +RC4_DZ 30 +RC4_MAX 1927 +RC4_MIN 1102 +RC4_OPTION 0 +RC4_REVERSED 0 +RC4_TRIM 1515 +RC5_DZ 0 +RC5_MAX 1900 +RC5_MIN 1100 +RC5_OPTION 0 +RC5_REVERSED 0 +RC5_TRIM 1500 +RC6_DZ 30 +RC6_MAX 1927 +RC6_MIN 1102 +RC6_OPTION 0 +RC6_REVERSED 0 +RC6_TRIM 1515 +RC7_DZ 0 +RC7_MAX 1995 +RC7_MIN 1105 +RC7_OPTION 0 +RC7_REVERSED 0 +RC7_TRIM 1105 +RC8_DZ 0 +RC8_MAX 1995 +RC8_MIN 1105 +RC8_OPTION 0 +RC8_REVERSED 0 +RC8_TRIM 1105 +RC9_DZ 10 +RC9_MAX 1995 +RC9_MIN 1105 +RC9_OPTION 0 +RC9_REVERSED 0 +RC9_TRIM 1105 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RC_OPTIONS 0 +RC_OVERRIDE_TIME 3.000000 +RC_PROTOCOLS 1 +RC_SPEED 490 +RELAY_DEFAULT 0 +RELAY_PIN -1 +RELAY_PIN2 -1 +RELAY_PIN3 -1 +RELAY_PIN4 -1 +RELAY_PIN5 -1 +RELAY_PIN6 -1 +RNGFND1_TYPE 0 +RNGFND2_TYPE 0 +RNGFND3_TYPE 0 +RNGFND4_TYPE 0 +RNGFND5_TYPE 0 +RNGFND6_TYPE 0 +RNGFND7_TYPE 0 +RNGFND8_TYPE 0 +RNGFND9_TYPE 0 +RNGFNDA_TYPE 0 +RNGFND_FILT 0.500000 +RPM1_ESC_MASK 0 +RPM1_MAX 100000.000000 +RPM1_MIN 10.000000 +RPM1_MIN_QUAL 0.500000 +RPM1_PIN -1 +RPM1_SCALING 1.000000 +RPM1_TYPE 10 +RPM2_ESC_MASK 0 +RPM2_MAX 100000.000000 +RPM2_MIN 10.000000 +RPM2_MIN_QUAL 0.500000 +RPM2_PIN -1 +RPM2_SCALING 1.000000 +RPM2_TYPE 10 +RSSI_TYPE 0 +RTL_ALT 1500 +RTL_ALT_FINAL 0 +RTL_ALT_TYPE 0 +RTL_CLIMB_MIN 0 +RTL_CONE_SLOPE 3.000000 +RTL_LOIT_TIME 2000 +RTL_OPTIONS 0 +RTL_SPEED 1500 +SCHED_DEBUG 0 +SCHED_LOOP_RATE 400 +SCHED_OPTIONS 0 +SCR_ENABLE 0 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 57 +SERIAL1_OPTIONS 0 +SERIAL1_PROTOCOL 2 +SERIAL2_BAUD 0 +SERIAL2_OPTIONS 0 +SERIAL2_PROTOCOL 0 +SERIAL3_BAUD 38 +SERIAL3_OPTIONS 0 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 38 +SERIAL4_OPTIONS 0 +SERIAL4_PROTOCOL 5 +SERIAL5_BAUD 57 +SERIAL5_OPTIONS 0 +SERIAL5_PROTOCOL -1 +SERIAL6_BAUD 57 +SERIAL6_OPTIONS 0 +SERIAL6_PROTOCOL -1 +SERIAL7_BAUD 57 +SERIAL7_OPTIONS 0 +SERIAL7_PROTOCOL -1 +SERIAL_PASS1 0 +SERIAL_PASS2 -1 +SERIAL_PASSTIMO 15 +SERVO10_FUNCTION 0 +SERVO10_MAX 1900 +SERVO10_MIN 1100 +SERVO10_REVERSED 0 +SERVO10_TRIM 1500 +SERVO11_FUNCTION 0 +SERVO11_MAX 1900 +SERVO11_MIN 1100 +SERVO11_REVERSED 0 +SERVO11_TRIM 1100 +SERVO12_FUNCTION 0 +SERVO12_MAX 1900 +SERVO12_MIN 1100 +SERVO12_REVERSED 0 +SERVO12_TRIM 1500 +SERVO13_FUNCTION 0 +SERVO13_MAX 1900 +SERVO13_MIN 1100 +SERVO13_REVERSED 0 +SERVO13_TRIM 0 +SERVO14_FUNCTION 0 +SERVO14_MAX 1900 +SERVO14_MIN 1100 +SERVO14_REVERSED 0 +SERVO14_TRIM 1500 +SERVO15_FUNCTION 0 +SERVO15_MAX 1900 +SERVO15_MIN 1100 +SERVO15_REVERSED 0 +SERVO15_TRIM 1500 +SERVO16_FUNCTION 0 +SERVO16_MAX 1900 +SERVO16_MIN 1100 +SERVO16_REVERSED 0 +SERVO16_TRIM 1500 +SERVO1_FUNCTION 34 +SERVO1_MAX 2100 +SERVO1_MIN 900 +SERVO1_REVERSED 1 +SERVO1_TRIM 1500 +SERVO2_FUNCTION 33 +SERVO2_MAX 2100 +SERVO2_MIN 900 +SERVO2_REVERSED 0 +SERVO2_TRIM 1500 +SERVO3_FUNCTION 35 +SERVO3_MAX 2000 +SERVO3_MIN 1000 +SERVO3_REVERSED 0 +SERVO3_TRIM 1000 +SERVO4_FUNCTION 36 +SERVO4_MAX 2000 +SERVO4_MIN 1000 +SERVO4_REVERSED 0 +SERVO4_TRIM 1000 +SERVO5_FUNCTION 0 +SERVO5_MAX 1801 +SERVO5_MIN 1200 +SERVO5_REVERSED 0 +SERVO5_TRIM 1501 +SERVO6_FUNCTION 0 +SERVO6_MAX 2001 +SERVO6_MIN 1000 +SERVO6_REVERSED 0 +SERVO6_TRIM 1500 +SERVO7_FUNCTION 0 +SERVO7_MAX 1994 +SERVO7_MIN 1003 +SERVO7_REVERSED 0 +SERVO7_TRIM 1504 +SERVO8_FUNCTION 0 +SERVO8_MAX 2001 +SERVO8_MIN 1005 +SERVO8_REVERSED 1 +SERVO8_TRIM 1500 +SERVO9_FUNCTION 0 +SERVO9_MAX 1900 +SERVO9_MIN 1100 +SERVO9_REVERSED 0 +SERVO9_TRIM 1500 +SERVO_32_ENABLE 0 +SERVO_DSHOT_ESC 0 +SERVO_DSHOT_RATE 0 +SERVO_FTW_MASK 0 +SERVO_FTW_POLES 14 +SERVO_FTW_RVMASK 0 +SERVO_GPIO_MASK 32512 +SERVO_RATE 200 +SERVO_ROB_POSMAX 4095 +SERVO_ROB_POSMIN 0 +SERVO_SBUS_RATE 200 +SERVO_VOLZ_MASK 0 +SID_AXIS 0 +SIMPLE 0 +SIM_ACC1_BIAS_X 0.000000 +SIM_ACC1_BIAS_Y 0.000000 +SIM_ACC1_BIAS_Z 0.000000 +SIM_ACC1_RND 0.000000 +SIM_ACC1_SCAL_X 0.000000 +SIM_ACC1_SCAL_Y 0.000000 +SIM_ACC1_SCAL_Z 0.000000 +SIM_ACC2_BIAS_X 0.000000 +SIM_ACC2_BIAS_Y 0.000000 +SIM_ACC2_BIAS_Z 0.000000 +SIM_ACC2_RND 0.000000 +SIM_ACC2_SCAL_X 0.000000 +SIM_ACC2_SCAL_Y 0.000000 +SIM_ACC2_SCAL_Z 0.000000 +SIM_ACC3_BIAS_X 0.000000 +SIM_ACC3_BIAS_Y 0.000000 +SIM_ACC3_BIAS_Z 0.000000 +SIM_ACC3_RND 0.000000 +SIM_ACC3_SCAL_X 0.000000 +SIM_ACC3_SCAL_Y 0.000000 +SIM_ACC3_SCAL_Z 0.000000 +SIM_ACCEL1_FAIL 0.000000 +SIM_ACCEL2_FAIL 0.000000 +SIM_ACCEL3_FAIL 0.000000 +SIM_ACC_FAIL_MSK 0 +SIM_ACC_TRIM_X 0.000000 +SIM_ACC_TRIM_Y 0.000000 +SIM_ACC_TRIM_Z 0.000000 +SIM_ADSB_ALT 1000.000000 +SIM_ADSB_COUNT -1 +SIM_ADSB_RADIUS 10000.000000 +SIM_ADSB_TX 0 +SIM_ARSPD2_FAIL 0.000000 +SIM_ARSPD2_FAILP 0.000000 +SIM_ARSPD2_OFS 2013.000000 +SIM_ARSPD2_PITOT 0.000000 +SIM_ARSPD2_RATIO 1.990000 +SIM_ARSPD2_RND 2.000000 +SIM_ARSPD2_SIGN 0 +SIM_ARSPD_FAIL 0.000000 +SIM_ARSPD_FAILP 0.000000 +SIM_ARSPD_OFS 2013.000000 +SIM_ARSPD_PITOT 0.000000 +SIM_ARSPD_RATIO 1.990000 +SIM_ARSPD_RND 2.000000 +SIM_ARSPD_SIGN 0 +SIM_BAR2_DELAY 0 +SIM_BAR2_DISABLE 0 +SIM_BAR2_DRIFT 0.000000 +SIM_BAR2_FREEZE 0 +SIM_BAR2_GLITCH 0.000000 +SIM_BAR2_RND 0.200000 +SIM_BAR2_WCF_BAK 0.000000 +SIM_BAR2_WCF_FWD 0.000000 +SIM_BAR2_WCF_LFT 0.000000 +SIM_BAR2_WCF_RGT 0.000000 +SIM_BAR3_DELAY 0 +SIM_BAR3_DISABLE 0 +SIM_BAR3_DRIFT 0.000000 +SIM_BAR3_FREEZE 0 +SIM_BAR3_GLITCH 0.000000 +SIM_BAR3_RND 0.200000 +SIM_BAR3_WCF_BAK 0.000000 +SIM_BAR3_WCF_FWD 0.000000 +SIM_BAR3_WCF_LFT 0.000000 +SIM_BAR3_WCF_RGT 0.000000 +SIM_BARO_COUNT 2 +SIM_BARO_DELAY 0 +SIM_BARO_DISABLE 0 +SIM_BARO_DRIFT 0.000000 +SIM_BARO_FREEZE 0 +SIM_BARO_GLITCH 0.000000 +SIM_BARO_RND 0.200000 +SIM_BARO_WCF_BAK 0.000000 +SIM_BARO_WCF_FWD 0.000000 +SIM_BARO_WCF_LFT 0.000000 +SIM_BARO_WCF_RGT 0.000000 +SIM_BATT_CAP_AH 17.000000 +SIM_BATT_NUM 1.000000 +SIM_BATT_RES_OHM 0.070000 +SIM_BATT_VOLTAGE 500.000000 +SIM_BAUDLIMIT_EN 0 +SIM_CORE_KG 2.330000 +SIM_DRIFT_SPEED 0.050000 +SIM_DRIFT_TIME 5.000000 +SIM_EFI_TYPE 0 +SIM_ENGINE_FAIL 0 +SIM_ENGINE_MUL 1.000000 +SIM_ESC_ARM_RPM 0.000000 +SIM_ESC_TELEM 1 +SIM_FLOAT_EXCEPT 1 +SIM_FLOW_DELAY 0 +SIM_FLOW_ENABLE 0 +SIM_FLOW_POS_X 0.000000 +SIM_FLOW_POS_Y 0.000000 +SIM_FLOW_POS_Z 0.000000 +SIM_FLOW_RATE 10 +SIM_FLOW_RND 0.050000 +SIM_FTOWESC_ENA 0 +SIM_FTOWESC_POW 4095 +SIM_GND_BEHAV -1 +SIM_GPS2_ACC 0.300000 +SIM_GPS2_ALT_OFS 0 +SIM_GPS2_BYTELOS 0.000000 +SIM_GPS2_DISABLE 0 +SIM_GPS2_DRFTALT 0.000000 +SIM_GPS2_GLTCH_X 0.000000 +SIM_GPS2_GLTCH_Y 0.000000 +SIM_GPS2_GLTCH_Z 0.000000 +SIM_GPS2_HDG 0 +SIM_GPS2_HZ 5 +SIM_GPS2_LAG_MS 100 +SIM_GPS2_LCKTIME 0 +SIM_GPS2_NOISE 0.000000 +SIM_GPS2_NUMSATS 10 +SIM_GPS2_POS_X 0.000000 +SIM_GPS2_POS_Y 0.000000 +SIM_GPS2_POS_Z 0.000000 +SIM_GPS2_TYPE 1 +SIM_GPS2_VERR_X 0.000000 +SIM_GPS2_VERR_Y 0.000000 +SIM_GPS2_VERR_Z 0.000000 +SIM_GPS_ACC 0.300000 +SIM_GPS_ALT_OFS 0 +SIM_GPS_BYTELOSS 0.000000 +SIM_GPS_DISABLE 0 +SIM_GPS_DRIFTALT 0.000000 +SIM_GPS_GLITCH_X 0.000000 +SIM_GPS_GLITCH_Y 0.000000 +SIM_GPS_GLITCH_Z 0.000000 +SIM_GPS_HDG 0 +SIM_GPS_HZ 5 +SIM_GPS_LAG_MS 100 +SIM_GPS_LOCKTIME 0 +SIM_GPS_NOISE 0.000000 +SIM_GPS_NUMSATS 10 +SIM_GPS_POS_X 0.000000 +SIM_GPS_POS_Y 0.000000 +SIM_GPS_POS_Z 0.000000 +SIM_GPS_TYPE 1 +SIM_GPS_VERR_X 0.000000 +SIM_GPS_VERR_Y 0.000000 +SIM_GPS_VERR_Z 0.000000 +SIM_GRPE_ENABLE 0 +SIM_GRPE_PIN -1 +SIM_GRPS_ENABLE 0 +SIM_GRPS_GRAB 2000 +SIM_GRPS_PIN -1 +SIM_GRPS_RELEASE 1000 +SIM_GRPS_REVERSE 0 +SIM_GYR1_RND 0.000000 +SIM_GYR1_SCALE_X 0.000000 +SIM_GYR1_SCALE_Y 0.000000 +SIM_GYR1_SCALE_Z 0.000000 +SIM_GYR2_RND 0.000000 +SIM_GYR2_SCALE_X 0.000000 +SIM_GYR2_SCALE_Y 0.000000 +SIM_GYR2_SCALE_Z 0.000000 +SIM_GYR3_RND 0.000000 +SIM_GYR3_SCALE_X 0.000000 +SIM_GYR3_SCALE_Y 0.000000 +SIM_GYR3_SCALE_Z 0.000000 +SIM_GYR_FAIL_MSK 0 +SIM_IE24_ENABLE 0 +SIM_IE24_ERROR 0 +SIM_IE24_STATE -1 +SIM_IMUT1_ENABLE 0 +SIM_IMUT2_ENABLE 0 +SIM_IMUT3_ENABLE 0 +SIM_IMUT_END 45.000000 +SIM_IMUT_FIXED 0.000000 +SIM_IMUT_START 25.000000 +SIM_IMUT_TCONST 300.000000 +SIM_IMU_COUNT 3 +SIM_IMU_POS_X 0.000000 +SIM_IMU_POS_Y 0.000000 +SIM_IMU_POS_Z 0.000000 +SIM_INIT_ALT_OFS 0.000000 +SIM_INIT_LAT_OFS 0.000000 +SIM_INIT_LON_OFS 0.000000 +SIM_INS_THR_MIN 0.100000 +SIM_JSON_MASTER 0 +SIM_LED_LAYOUT 0 +SIM_LOOP_DELAY 0 +SIM_MAG1_DEVID 97539 +SIM_MAG1_FAIL 0 +SIM_MAG1_SCALING 1.000000 +SIM_MAG2_DEVID 131874 +SIM_MAG2_DIA_X 0.000000 +SIM_MAG2_DIA_Y 0.000000 +SIM_MAG2_DIA_Z 0.000000 +SIM_MAG2_FAIL 0 +SIM_MAG2_ODI_X 0.000000 +SIM_MAG2_ODI_Y 0.000000 +SIM_MAG2_ODI_Z 0.000000 +SIM_MAG2_OFS_X 5.000000 +SIM_MAG2_OFS_Y 13.000000 +SIM_MAG2_OFS_Z -18.000000 +SIM_MAG2_ORIENT 0 +SIM_MAG2_SCALING 1.000000 +SIM_MAG3_DEVID 263178 +SIM_MAG3_DIA_X 0.000000 +SIM_MAG3_DIA_Y 0.000000 +SIM_MAG3_DIA_Z 0.000000 +SIM_MAG3_FAIL 0 +SIM_MAG3_ODI_X 0.000000 +SIM_MAG3_ODI_Y 0.000000 +SIM_MAG3_ODI_Z 0.000000 +SIM_MAG3_OFS_X 5.000000 +SIM_MAG3_OFS_Y 13.000000 +SIM_MAG3_OFS_Z -18.000000 +SIM_MAG3_ORIENT 0 +SIM_MAG3_SCALING 1.000000 +SIM_MAG4_DEVID 97283 +SIM_MAG5_DEVID 97795 +SIM_MAG6_DEVID 98051 +SIM_MAG7_DEVID 0 +SIM_MAG8_DEVID 0 +SIM_MAG_ALY_HGT 1.000000 +SIM_MAG_ALY_X 0.000000 +SIM_MAG_ALY_Y 0.000000 +SIM_MAG_ALY_Z 0.000000 +SIM_MAG_DELAY 0 +SIM_MAG_DIA_X 0.000000 +SIM_MAG_DIA_Y 0.000000 +SIM_MAG_DIA_Z 0.000000 +SIM_MAG_MOT_X 0.000000 +SIM_MAG_MOT_Y 0.000000 +SIM_MAG_MOT_Z 0.000000 +SIM_MAG_ODI_X 0.000000 +SIM_MAG_ODI_Y 0.000000 +SIM_MAG_ODI_Z 0.000000 +SIM_MAG_OFS_X 5.000000 +SIM_MAG_OFS_Y 13.000000 +SIM_MAG_OFS_Z -18.000000 +SIM_MAG_ORIENT 0 +SIM_MAG_RND 0.000000 +SIM_ODOM_ENABLE 0 +SIM_OH_MASK 0 +SIM_OPOS_ALT 584.000000 +SIM_OPOS_HDG 353.000000 +SIM_OPOS_LAT -35.363258 +SIM_OPOS_LNG 149.165207 +SIM_PARA_ENABLE 0 +SIM_PARA_PIN -1 +SIM_PAYLOAD_KG 0.000000 +SIM_PIN_MASK 0 +SIM_PLD_ALT_LMT 15.000000 +SIM_PLD_DIST_LMT 10.000000 +SIM_PLD_ENABLE 0 +SIM_PLD_HEIGHT 0.000000 +SIM_PLD_LAT 0.000000 +SIM_PLD_LON 0.000000 +SIM_PLD_OPTIONS 0 +SIM_PLD_ORIENT 24 +SIM_PLD_RATE 100 +SIM_PLD_TYPE 0 +SIM_PLD_YAW 0 +SIM_RATE_HZ 1200 +SIM_RC_CHANCOUNT 16 +SIM_RC_FAIL 0 +SIM_RICH_CTRL -1 +SIM_RICH_ENABLE 0 +SIM_SAFETY_STATE 0 +SIM_SAIL_TYPE 0 +SIM_SERVO_SPEED 0.140000 +SIM_SHIP_DSIZE 10.000000 +SIM_SHIP_ENABLE 0 +SIM_SHIP_OFS_X 0.000000 +SIM_SHIP_OFS_Y 0.000000 +SIM_SHIP_OFS_Z 0.000000 +SIM_SHIP_PSIZE 1000.000000 +SIM_SHIP_SPEED 3.000000 +SIM_SHIP_SYSID 17 +SIM_SHOVE_TIME 0 +SIM_SHOVE_X 0.000000 +SIM_SHOVE_Y 0.000000 +SIM_SHOVE_Z 0.000000 +SIM_SONAR_GLITCH 0.000000 +SIM_SONAR_POS_X 0.000000 +SIM_SONAR_POS_Y 0.000000 +SIM_SONAR_POS_Z 0.000000 +SIM_SONAR_RND 0.000000 +SIM_SONAR_ROT 25 +SIM_SONAR_SCALE 12.121200 +SIM_SPEEDUP -1.000000 +SIM_SPR_ENABLE 0 +SIM_SPR_PUMP -1 +SIM_SPR_SPIN -1 +SIM_TA_ENABLE 1 +SIM_TEMP_BFACTOR 0.000000 +SIM_TEMP_BRD_OFF 20.000000 +SIM_TEMP_START 25.000000 +SIM_TEMP_TCONST 30.000000 +SIM_TERRAIN 1 +SIM_THML_SCENARI 0 +SIM_TIDE_DIR 0.000000 +SIM_TIDE_SPEED 0.000000 +SIM_TWIST_TIME 0 +SIM_TWIST_X 0.000000 +SIM_TWIST_Y 0.000000 +SIM_TWIST_Z 0.000000 +SIM_VIB_FREQ_X 0.000000 +SIM_VIB_FREQ_Y 0.000000 +SIM_VIB_FREQ_Z 0.000000 +SIM_VIB_MOT_HMNC 1 +SIM_VIB_MOT_MASK 0 +SIM_VIB_MOT_MAX 0.000000 +SIM_VIB_MOT_MULT 1.000000 +SIM_VICON_FAIL 0 +SIM_VICON_GLIT_X 0.000000 +SIM_VICON_GLIT_Y 0.000000 +SIM_VICON_GLIT_Z 0.000000 +SIM_VICON_POS_X 0.000000 +SIM_VICON_POS_Y 0.000000 +SIM_VICON_POS_Z 0.000000 +SIM_VICON_TMASK 3 +SIM_VICON_VGLI_X 0.000000 +SIM_VICON_VGLI_Y 0.000000 +SIM_VICON_VGLI_Z 0.000000 +SIM_VICON_YAW 0 +SIM_VICON_YAWERR 0 +SIM_WAVE_AMP 0.500000 +SIM_WAVE_DIR 0.000000 +SIM_WAVE_ENABLE 0 +SIM_WAVE_LENGTH 10.000000 +SIM_WAVE_SPEED 0.500000 +SIM_WIND_DIR 180.000000 +SIM_WIND_DIR_Z 0.000000 +SIM_WIND_SPD 0.000000 +SIM_WIND_T 0 +SIM_WIND_TURB 0.000000 +SIM_WIND_T_ALT 60.000000 +SIM_WIND_T_COEF 0.010000 +SIM_WOW_PIN -1 +SPRAY_ENABLE 0 +SR0_ADSB 120 +SR0_EXTRA1 120 +SR0_EXTRA2 120 +SR0_EXTRA3 120 +SR0_EXT_STAT 120 +SR0_PARAMS 0 +SR0_POSITION 120 +SR0_RAW_CTRL 120 +SR0_RAW_SENS 120 +SR0_RC_CHAN 120 +SR1_ADSB 0 +SR1_EXTRA1 0 +SR1_EXTRA2 0 +SR1_EXTRA3 0 +SR1_EXT_STAT 0 +SR1_PARAMS 0 +SR1_POSITION 0 +SR1_RAW_CTRL 0 +SR1_RAW_SENS 0 +SR1_RC_CHAN 0 +SR2_ADSB 0 +SR2_EXTRA1 0 +SR2_EXTRA2 0 +SR2_EXTRA3 0 +SR2_EXT_STAT 0 +SR2_PARAMS 0 +SR2_POSITION 0 +SR2_RAW_CTRL 0 +SR2_RAW_SENS 0 +SR2_RC_CHAN 0 +SR3_ADSB 0 +SR3_EXTRA1 0 +SR3_EXTRA2 0 +SR3_EXTRA3 0 +SR3_EXT_STAT 0 +SR3_PARAMS 0 +SR3_POSITION 0 +SR3_RAW_CTRL 0 +SR3_RAW_SENS 0 +SR3_RC_CHAN 0 +SR4_ADSB 0 +SR4_EXTRA1 0 +SR4_EXTRA2 0 +SR4_EXTRA3 0 +SR4_EXT_STAT 0 +SR4_PARAMS 0 +SR4_POSITION 0 +SR4_RAW_CTRL 0 +SR4_RAW_SENS 0 +SR4_RC_CHAN 0 +SR5_ADSB 0 +SR5_EXTRA1 0 +SR5_EXTRA2 0 +SR5_EXTRA3 0 +SR5_EXT_STAT 0 +SR5_PARAMS 0 +SR5_POSITION 0 +SR5_RAW_CTRL 0 +SR5_RAW_SENS 0 +SR5_RC_CHAN 0 +SR6_ADSB 0 +SR6_EXTRA1 0 +SR6_EXTRA2 0 +SR6_EXTRA3 0 +SR6_EXT_STAT 0 +SR6_PARAMS 0 +SR6_POSITION 0 +SR6_RAW_CTRL 0 +SR6_RAW_SENS 0 +SR6_RC_CHAN 0 +SRTL_ACCURACY 2.000000 +SRTL_OPTIONS 0 +SRTL_POINTS 0 +STAT_BOOTCNT 188 +STAT_FLTTIME 10548 +STAT_RESET 255623488 +STAT_RMFLTTIME1 80914 +STAT_RMFLTTIME2 890914 +STAT_RUNTIME 427173 +SUPER_SIMPLE 0 +SURFTRAK_MODE 1 +SYSID_ENFORCE 0 +SYSID_MYGCS 255 +SYSID_THISMAV 1 +TCAL_ENABLED 0 +TELEM_DELAY 0 +TERRAIN_ENABLE 1 +TERRAIN_MARGIN 0.050000 +TERRAIN_OFS_MAX 15.000000 +TERRAIN_OPTIONS 0 +TERRAIN_SPACING 100 +THROW_MOT_START 0 +THROW_NEXTMODE 18 +THROW_TYPE 0 +THR_DZ 10 +TKOFF_RPM_MIN 0 +TKOFF_SLEW_TIME 1.000000 +TUNE 0 +TUNE_MAX 0.000000 +TUNE_MIN 0.000000 +VISO_TYPE 0 +VTX_ENABLE 0 +WINCH_TYPE 0 +WPNAV_ACCEL 250.000000 +WPNAV_ACCEL_Z 200.000000 +WPNAV_JERK 1.000000 +WPNAV_RADIUS 300.000000 +WPNAV_RFND_USE 0 +WPNAV_SPEED 1500.000000 +WPNAV_SPEED_DN 300.000000 +WPNAV_SPEED_UP 500.000000 +WPNAV_TER_MARGIN 10.000000 +WP_NAVALT_MIN 0.000000 +WP_YAW_BEHAVIOR 2 +ZIGZ_AUTO_ENABLE 0 diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/persistent.dat b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/persistent.dat new file mode 100644 index 000000000..53d61c5cc Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/persistent.dat differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/server_test.py b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/server_test.py new file mode 100644 index 000000000..6460795d9 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/server_test.py @@ -0,0 +1,55 @@ +import socket +import threading +import time +import struct + +sim_start_time = time.time() + + +def get_sim_time(): + return (time.time() - sim_start_time)/2. + +def handle_client(conn, addr): + print(f'Connected by {addr}') + + initial_sitl_time = -1. + initial_sim_time = -1. + + with conn: + while True: + data = conn.recv(8) + print('data', data) + if not data: + break + + t = struct.unpack('Q', data)[0] + s = get_sim_time() + + if initial_sitl_time < 0: + print('initial') + initial_sitl_time = t + initial_sim_time = s + + sitl_time = t - initial_sitl_time + sim_time = (s - initial_sim_time)*1000000 + time_to_sleep = int(sitl_time - sim_time) + print('time to sleep', time_to_sleep, sitl_time, sim_time) + + #print(f'Received {data.decode()} from {addr}') + #time.sleep(0.01) + #conn.sendall(b'Hello from server') + conn.sendall(struct.pack('i', time_to_sleep)) + +def start_server(host='127.0.0.1', port=65432): + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.bind((host, port)) + s.listen() + print(f'Server listening on {host}:{port}') + while True: + conn, addr = s.accept() + client_thread = threading.Thread(target=handle_client, args=(conn, addr)) + client_thread.start() + print(f'Active connections: {threading.active_count() - 1}') + +if __name__ == "__main__": + start_server() diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/spirit_sitl b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/spirit_sitl new file mode 100755 index 000000000..510705375 Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/spirit_sitl differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/N00E000.DAT b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/N00E000.DAT new file mode 100644 index 000000000..634a3edc3 Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/N00E000.DAT differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/N42W072.DAT b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/N42W072.DAT new file mode 100644 index 000000000..25fbbf3df Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/N42W072.DAT differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/S36E149.DAT b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/S36E149.DAT new file mode 100644 index 000000000..48eef0cdf Binary files /dev/null and b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/terrain/S36E149.DAT differ diff --git a/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/test.gdb b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/test.gdb new file mode 100644 index 000000000..03291d094 --- /dev/null +++ b/simulation/extension_docker/extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/test.gdb @@ -0,0 +1,12 @@ +set environment LD_PRELOAD /extras/kit-app-template/source/extensions/omni.example.spawn_prims/omni/example/spawn_prims/AscentAeroSystems/AscentAeroSystemsSITLPackage/inject.so + +b _ZN7HALSITL9Scheduler10stop_clockEm +catch signal SIGSEGV + +commands 1 + #info registers rsi + call (void)injected_function($rsi) + continue + +commands 2 + bt \ No newline at end of file diff --git a/simulation/gaz_sim.py b/simulation/gaz_sim.py new file mode 100644 index 000000000..d5ae8a09b --- /dev/null +++ b/simulation/gaz_sim.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 + +import sys +import time +import dronekit +import os +import math +import gz.math7 + +from gz.msgs10.boolean_pb2 import Boolean +from gz.msgs10.header_pb2 import Header +from gz.msgs10.pose_pb2 import Pose +from gz.msgs10.quaternion_pb2 import Quaternion +from gz.msgs10.vector3d_pb2 import Vector3d +from gz.transport13 import Node + +from AscentAeroSystems.ascent_sitl_launch_tool import AscentSitlLaunchTool + +script_dir = os.path.dirname(os.path.realpath(__file__)) + "/AscentAeroSystems" +sitl_tool = AscentSitlLaunchTool(script_dir) +sitl_tool.launch() + +world_name = "dron" +model_name = "drone" +#world_name = "empty" +#model_name = "box" +connection = "udp:127.0.0.1:14552" +timeout_ms = 9999 +update_rate_hz = 30.0 +pose_offset_x = 0.0 +pose_offset_y = 0.0 +pose_offset_z = 0.17 +def transform_ardupilot_to_gazebo(pose): + wldAToWldG = gz.math7.Pose3d(0, 0, 0, -math.pi, 0, 0) + bdyAToBdyG = gz.math7.Pose3d(0, 0, 0, -math.pi, 0, 0) + wldGToBdyG = wldAToWldG.inverse() * pose * bdyAToBdyG + return wldGToBdyG +def main(): + connection_string = "udp:127.0.0.1:14552" + print("Connecting to vehicle on: {}".format(connection_string)) + vehicle = dronekit.connect(connection_string, wait_ready=True, baud=57600) + print("Vehicle connected: {}".format(vehicle)) + try: + # create a transport node + node = Node() + # set service details + service = "/world/{}/set_pose".format(world_name) + reqtype = "gz.msgs10.Pose" + reptype = "gz.msgs10.Boolean" + timeout = 30 + # configure update loop + now_s = time.time() + start_time_s = now_s + # update_rate_hz = 1.0 + update_period_s = 1.0 / update_rate_hz + last_update_time_s = now_s + sim_time_s = now_s - start_time_s + while True: + now_s = time.time() + time_s = now_s - start_time_s + if now_s - last_update_time_s >= update_period_s: + last_update_time_s = now_s + # check we have valid data + if ((vehicle.location.local_frame.north is not None) + and (vehicle.attitude.roll is not None)): + # ardupilot pose + wldAToBdyA = gz.math7.Pose3d( + vehicle.location.local_frame.north, + vehicle.location.local_frame.east, + vehicle.location.local_frame.down, + vehicle.attitude.roll, + vehicle.attitude.pitch, + vehicle.attitude.yaw) + # ignition pose + wldGToBdyG = transform_ardupilot_to_gazebo(wldAToBdyA) + print("[{}] ned_xyz: {:.3f} {:.3f} {:.3f}, ned_rpy {:.3f} {:.3f} {:.3f}".format( + sim_time_s, wldAToBdyA.x(), wldAToBdyA.y(), wldAToBdyA.z(), + wldAToBdyA.roll(), wldAToBdyA.pitch(), wldAToBdyA.yaw())) + # create request message + vector3d_msg = Vector3d() + vector3d_msg.x = wldGToBdyG.x() + pose_offset_x + vector3d_msg.y = wldGToBdyG.y() + pose_offset_y + vector3d_msg.z = wldGToBdyG.z() + pose_offset_z + quat_msg = Quaternion() + quat_msg.x = wldGToBdyG.rot().x() + quat_msg.y = wldGToBdyG.rot().y() + quat_msg.z = wldGToBdyG.rot().z() + quat_msg.w = wldGToBdyG.rot().w() + pose_msg = Pose() + pose_msg.name = model_name + pose_msg.position.CopyFrom(vector3d_msg) + pose_msg.orientation.CopyFrom(quat_msg) + # submit request (blocking) + result = node.request(service, pose_msg, Pose, Boolean, timeout_ms) + if not result: + print("[{:.1f}] update failed".format(time_s)) + except KeyboardInterrupt: + pass + vehicle.close() +if __name__ == "__main__": + main()