From 8998aaf4916995b1eca124c4991f32929d47a752 Mon Sep 17 00:00:00 2001 From: Andrew Jong Date: Fri, 1 Nov 2024 18:10:11 -0400 Subject: [PATCH] Integrates local planner. Still need to tune parameters, but it runs. (#118) * changing global planning docs and cleaning up random walk planner * adding testing docs * changing global planning docs and cleaning up random walk planner * adding testing docs * refactoring code for new interface * adding conversions for new message types * fixing build issues for new interfaces * chaning logic to take the max voxzel size instead of assuming uniform * cleaning up distance calculations * making build changes for curr_location * more build fixes * more and more build fixes * more and more build fixes * asdf * a;alala * asdfallsdlfkn * fixed build * Do proper docker containers now with variable number of robots (#40) * Updated dockerfile to copy simulation directly into docker image, updating docker compose to refer to images on server, updated documentation to reflect that you don't have to build the docker image and can just pull it * Updating docker compose to refer to images on server, updated documentation to reflect that you don't have to build the docker image and can just pull it * added environment info * Proper docker compose now * Add .gitignore for omni_pass.env file * Update with Basti's instructions * Update dockerfiles and docs * Delete deprecated simulation/ folder * use net=host * oops net=host doesn't work with dynamic robots * Fix ros2 with isaac sim. make isaac launch not by default with the main command * Change isaac to use installed ros2 humble instead of built-in one * Sadly: isaac sim REQUIRES network=host to launch from docker. Sadly, this means our robots need to also network=host and we must hardcode our docker compose * Add ROS_LOCALHOST_ONLY=1; confirmed works through docker when network=host * fix docker warnings * Change path to use ros_ws under /root/AirStack/ros_ws * Add stereo-image-proc, image-view * Add isaacsim ros2 package, add entrypoint * Change docker compose so that it just launches the containers with bash * got ascent sitl working in isaacsim * Updated, now works with with --scale and separate containers per robot - Isaac can run headless in a container - robots can run in separate containers, each with their own hostname and ROS_DOMAIN_ID - all containers live on the private docker network, isolated from host - Can connect to Isaac GUI via Omniverse Streaming client - ROS communicates properly between all the containers - connects successfully to nucleus * reverted omni_pass * Change the file to TEMPLATE so that the actual one isn't commited * updated so guis work in docker * Enable ssh into docker containers * Add docs on network * running mavros in isaacsim container, robot launch files moved to robot_bringup * Update docs * Delete deprecated central/folder, organize --------- Co-authored-by: Sebastian Scherer Co-authored-by: John Keller * Aj/update docs (#45) * Update docs move robot with video * Update docs * Add footer (#46) * Add more docs * welcome page, boilerplate (#47) * welcome page, boilerplate * changing global planning docs and cleaning up random walk planner * adding service call * finishing clean up of old map callback * deleting irrelevant testing doc * removing trivial_planner package for random walk and add xml launch file * removing airstack_msgs dependency * adding plan reception mode to behaviro tree * buildable chanes * working action based random walk * working global plan toggle * fixing read_parameter formatting * adding ensemble planner * first test of the new global planner format with random walk publishing multiple paths * adding rad2deg and logging in random walk logic * working random walk * cleaning up junk * Remove fluff from droan * Connect disparity expansion to other core stack topics; works with gt depth image, doesn't work with stereo_image_proc Also remove unused disparity conversion package (disparity_conv) * Remove tf_prefix * Clean up launch, start migrating to use nav_msgs/Path for global plan * Remove deprecated traj mode client * Progress * Enable permalinks for headers * Update to use relative path * Add xml extensions * Update comments * Factor out parameters to config file; also clean up code * Add expansion point cloud visualizer back * Add missing marker publisher * Fix bugs and topic connections * Fix bugs and code cleanup * Fix typos add clarity * Add rviz for debug droan * Update docs * Rename core.rviz to robot.rviz * Enable SITL extension automatically in docker image * Remove node name override from launch files; instead let node names be in code * Pass node names * Refactor; nested nodes didn't work with subscribers, pass node ptr instead * Publishes a local traj omg * Set mavros use_sim_time at launch instead of with separate pane * Manually set trajectory marker namespace to prevent accumulating explosion * Add to global plan ICD sending feedback for ETA to waypoints * Try make tmux extension auto refresh. didn't work * Customize for debug * Attempt fix and refactor trajectory collision checking write a lot of helper functions change input to be Trajectory, not airstack::msgs::TrajectoryXYZVYaw * Clean up and make debugging clearer * Update cpp standard to cpp17, add docstring to recommended extensions * yay now the disparity graph collision markers are correct * No more segfault * Update * Comment out main rviz for now * Make lib shared * idk what this does * Remove dependency on missing global_planner_msgs * Comment out 5 sec wait for mavros launch * Reorder buttons * Publish frustum from disparity expansion for debugging * Successfully integrate droan with tyler's random walk planner * Rename SEGMENT to ADD_SEGMENT * Extract params to yaml * Clean code * Update rviz --------- Co-authored-by: Tyler Harp Co-authored-by: Sebastian Scherer Co-authored-by: John Keller --- .vscode/c_cpp_properties.json | 38 +- .vscode/extensions.json | 6 +- .vscode/launch.json | 3 +- .vscode/settings.json | 6 +- .vscode/tasks.json | 2 +- docker/Dockerfile.isaac-ros | 3 + docker/docker-compose.yaml | 2 +- docker/extras/drag_and_drop/ascent_sitl.yaml | 23 +- .../airlab/tmux_manager/extension.py | 11 +- docker/isaac-sim/user.config.json | 2858 +++++++++++++++++ docs/development/docker_usage.md | 90 +- docs/getting_started.md | 15 +- docs/robot/autonomy/4_global/planning.md | 14 +- mkdocs.yml | 2 + ros_ws/src/airstack_common/CMakeLists.txt | 2 +- .../src/airstack_msgs/srv/PlanToWaypoint.srv | 5 + .../src/airstack_msgs/srv/TrajectoryMode.srv | 2 +- .../launch/interface.launch.xml | 6 +- .../launch/odometry_conversion.xml | 2 +- .../launch/perception.launch.xml | 14 +- .../disparity_expansion/CMakeLists.txt | 29 +- .../disparity_expansion/README.md | 2 - .../config/disparity_expansion_params.yaml | 20 +- .../disparity_expansion.hpp | 101 + .../launch/disparity_expansion.launch.xml | 7 +- .../src/disparity_conv.cpp | 320 -- .../src/disparity_expansion.cpp | 1004 +++--- .../disparity_graph/CMakeLists.txt | 2 + .../disparity_graph/disparity_graph.hpp | 316 +- .../disparity_graph/package.xml | 1 + .../CMakeLists.txt | 2 + .../disparity_map_representation.hpp | 63 +- .../disparity_map_representation/package.xml | 2 + .../src/disparity_map_representation.cpp | 348 +- .../CMakeLists.txt | 2 + .../map_representation.hpp | 39 +- .../map_representation_interface/package.xml | 1 + .../droan_local_planner/CMakeLists.txt | 3 + .../droan_local_planner/config/droan.yaml | 54 +- .../droan_local_planner.hpp | 606 ++-- .../launch/droan_local_planner.launch.xml | 5 + .../launch/droan_local_planner_launch.xml | 81 - .../src/droan_local_planner.cpp | 4 +- .../takeoff_landing_planner/CMakeLists.txt | 4 + .../config/takeoff_landing_planner.yaml | 14 + .../launch/takeoff_landing_planner.launch.xml | 8 +- .../src/takeoff_landing_planner.cpp | 3 +- .../trajectory_library/CMakeLists.txt | 2 +- .../config/acceleration_magnitudes.yaml | 1800 ++++++----- .../trajectory_library/config/test.yaml | 1301 +++++++- .../trajectory_library/trajectory_library.hpp | 120 +- .../src/trajectory_library.cpp | 356 +- .../launch/trajectory_controller.launch.xml | 8 +- .../src/trajectory_controller.cpp | 78 +- .../srv/TrajectoryMode.srv | 2 +- .../3_local/local_bringup/CMakeLists.txt | 2 +- .../local_bringup/launch/local.launch.xml | 67 +- .../3_local/local_bringup/rviz/droan.rviz | 1085 +++---- .../ensemble_planner/CMakeLists.txt | 75 + .../ensemble_global_planner_config.yaml | 8 + .../include/ensemble_global_planner_node.hpp | 58 + .../b_planners/ensemble_planner/package.xml | 46 + .../src/ensemble_global_planner_node.cpp | 41 + .../CMakeLists.txt | 14 +- .../launch/global_planner.launch.xml | 6 + .../package.xml | 4 +- .../b_planners/random_walk/CMakeLists.txt | 6 +- .../config/random_walk_config.yaml | 30 +- .../random_walk/include/random_walk_logic.hpp | 18 +- .../random_walk/include/random_walk_node.hpp | 63 +- .../random_walk/launch/random_walk_launch.xml | 11 + .../launch/random_walk_launcher.py | 23 - .../b_planners/random_walk/package.xml | 41 +- .../random_walk/src/random_walk_logic.cpp | 78 +- .../random_walk/src/random_walk_node.cpp | 409 ++- .../src/random_walk_planner_node.cpp | 10 - .../global_bringup/launch/global.launch.xml | 10 +- .../launch/behavior.launch.xml | 17 +- .../behavior_executive/CMakeLists.txt | 10 +- .../behavior_executive/behavior_executive.hpp | 13 +- .../5_behavior/behavior_executive/package.xml | 3 + .../src/behavior_executive.cpp | 33 +- .../behavior_tree/config/drone.tree | 4 +- .../launch/behavior_tree_example.xml | 16 +- .../config/gui_config.yaml | 14 +- .../src/robot/robot_bringup/config/core.rviz | 452 --- .../robot_bringup/launch/robot.launch.xml | 6 +- .../src/robot/robot_bringup/rviz/robot.rviz | 580 ++-- .../launch/simulation.launch.xml | 2 +- 89 files changed, 8502 insertions(+), 4565 deletions(-) create mode 100644 docker/isaac-sim/user.config.json create mode 100644 ros_ws/src/airstack_msgs/srv/PlanToWaypoint.srv create mode 100644 ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp delete mode 100644 ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_conv.cpp create mode 100644 ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml delete mode 100644 ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml create mode 100644 ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/package.xml create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp rename ros_ws/src/robot/autonomy/4_global/b_planners/{trivial_planners => global_planner_bringup}/CMakeLists.txt (67%) create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml rename ros_ws/src/robot/autonomy/4_global/b_planners/{trivial_planners => global_planner_bringup}/package.xml (87%) create mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml delete mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launcher.py delete mode 100644 ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp delete mode 100644 ros_ws/src/robot/robot_bringup/config/core.rviz diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index c6bd9d23..74233dfd 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -6,31 +6,31 @@ "limitSymbolsToIncludedHeaders": false }, "includePath": [ - "/root/AirStack/ros_ws/install/vdb_mapping_interfaces/include/**", - "/root/AirStack/ros_ws/install/trajectory_library/include/**", - "/root/AirStack/ros_ws/install/mavros_interface/include/**", - "/root/AirStack/ros_ws/install/robot_interface/include/**", - "/root/AirStack/ros_ws/install/px4_msgs/include/**", - "/root/AirStack/ros_ws/install/mav_system_msgs/include/**", - "/root/AirStack/ros_ws/install/mav_state_machine_msgs/include/**", - "/root/AirStack/ros_ws/install/mav_planning_msgs/include/**", - "/root/AirStack/ros_ws/install/mav_msgs/include/**", - "/root/AirStack/ros_ws/install/disparity_map_representation/include/**", - "/root/AirStack/ros_ws/install/map_representation_interface/include/**", - "/root/AirStack/ros_ws/install/disparity_graph/include/**", - "/root/AirStack/ros_ws/install/behavior_tree/include/**", - "/root/AirStack/ros_ws/install/behavior_tree_msgs/include/**", - "/root/AirStack/ros_ws/install/airstack_common/include/**", - "/root/AirStack/ros_ws/install/airstack_msgs/include/**", + "${workspaceFolder}/ros_ws/install/vdb_mapping_interfaces/include/**", + "${workspaceFolder}/ros_ws/install/trajectory_library/include/**", + "${workspaceFolder}/ros_ws/install/mavros_interface/include/**", + "${workspaceFolder}/ros_ws/install/robot_interface/include/**", + "${workspaceFolder}/ros_ws/install/px4_msgs/include/**", + "${workspaceFolder}/ros_ws/install/mav_system_msgs/include/**", + "${workspaceFolder}/ros_ws/install/mav_state_machine_msgs/include/**", + "${workspaceFolder}/ros_ws/install/mav_planning_msgs/include/**", + "${workspaceFolder}/ros_ws/install/mav_msgs/include/**", + "${workspaceFolder}/ros_ws/install/disparity_map_representation/include/**", + "${workspaceFolder}/ros_ws/install/map_representation_interface/include/**", + "${workspaceFolder}/ros_ws/install/disparity_graph/include/**", + "${workspaceFolder}/ros_ws/install/behavior_tree/include/**", + "${workspaceFolder}/ros_ws/install/behavior_tree_msgs/include/**", + "${workspaceFolder}/ros_ws/install/airstack_common/include/**", + "${workspaceFolder}/ros_ws/install/airstack_msgs/include/**", + "${workspaceFolder}/ros_ws/src/**", "/opt/ros/humble/include/**", - "/usr/include/**", - "/root/AirStack/ros_ws/src/**" + "/usr/include/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", "compilerPath": "/usr/bin/gcc", "cStandard": "gnu11", - "cppStandard": "c++14" + "cppStandard": "c++17" } ], "version": 4 diff --git a/.vscode/extensions.json b/.vscode/extensions.json index c4792536..da9aa2fe 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -4,6 +4,10 @@ "ms-iot.vscode-ros", "ms-vscode-remote.remote-ssh", "ms-python.python", - "ms-vscode-remote.remote-containers" + "ms-vscode-remote.remote-containers", + "redhat.vscode-xml", + "dotjoshjohnson.xml", + "ms-vscode.cmake-tools", + "cschlosser.doxdocgen" ] } \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index b112a2d9..b01b6ca7 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -2,9 +2,10 @@ "version": "0.2.0", "configurations": [ { - "name": "ROS2: Launch robot.launch.py", + "name": "ROS2: Launch robot.launch.xml", "type": "ros", "request": "launch", + // "preLaunchTask": "build ros_ws debug", "target": "${workspaceFolder}/ros_ws/install/robot_bringup/share/robot_bringup/launch/robot.launch.xml", } ] diff --git a/.vscode/settings.json b/.vscode/settings.json index 15a7ef36..d8db72a6 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -14,6 +14,7 @@ "**/install": true, "**/log": true }, + "C_Cpp.errorSquiggles": "enabled", // ISAAC SIM "python.analysis.extraPaths": [ "~/.local/share/ov/pkg/isaac-sim-4.0.0/exts/omni.exporter.urdf", @@ -820,8 +821,9 @@ "stop_token": "cpp", "cfenv": "cpp", "typeindex": "cpp", - "valarray": "cpp" + "valarray": "cpp", + "*.ipp": "cpp" }, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 100 }", "editor.formatOnSave": true -} +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 04dac578..24593785 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -2,7 +2,7 @@ "version": "2.0.0", "tasks": [ { - "label": "build ros_ws", + "label": "build ros_ws debug", "type": "shell", "options": { "cwd": "${workspaceFolder}/ros_ws" diff --git a/docker/Dockerfile.isaac-ros b/docker/Dockerfile.isaac-ros index be169bc9..2a87aa2c 100644 --- a/docker/Dockerfile.isaac-ros +++ b/docker/Dockerfile.isaac-ros @@ -65,6 +65,9 @@ RUN cd /tmp/ && \ # copy over the AscentAeroSystemsSITLPackage COPY docker/isaac-sim/AscentAeroSystemsSITLPackage /AscentAeroSystemsSITLPackage +# the user.config.json file specifies to auto load the SITL extensions +COPY docker/isaac-sim/user.config.json /root/.local/share/ov/data/Kit/Isaac-Sim/4.1/user.config.json + COPY ros_ws/fastdds.xml /isaac-sim/fastdds.xml # Cleanup. Prevent people accidentally doing git commits as root in Docker diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index f48c7b15..0878acd2 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -2,7 +2,7 @@ services: # ============================================================================== isaac-sim: - image: &isaac-sim-image airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.1 + image: &isaac-sim-image airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.2 build: context: ../ dockerfile: docker/Dockerfile.isaac-ros diff --git a/docker/extras/drag_and_drop/ascent_sitl.yaml b/docker/extras/drag_and_drop/ascent_sitl.yaml index a4235fe2..c114eeb4 100644 --- a/docker/extras/drag_and_drop/ascent_sitl.yaml +++ b/docker/extras/drag_and_drop/ascent_sitl.yaml @@ -12,10 +12,21 @@ windows: - 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 + # - sleep 5 - export ROS_DOMAIN_ID=$DOMAIN_ID - - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" namespace:=$NAMESPACE/interface/mavros - - shell_command: - - sleep 7 - - export ROS_DOMAIN_ID=$DOMAIN_ID - - ros2 param set $NAMESPACE/interface/mavros/mavros use_sim_time true + # - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" namespace:=$NAMESPACE/interface/mavros --ros-args -p use_sim_time:=true + - > + ros2 run mavros mavros_node + --ros-args + -r __ns:=/$NAMESPACE/interface/mavros + -p fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" + -p tgt_system:=1 + -p tgt_component:=1 + -p fcu_protocol:=v2.0 + --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_pluginlists.yaml + --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_config.yaml + -p use_sim_time:=true + # - shell_command: + # - sleep 7 + # - export ROS_DOMAIN_ID=$DOMAIN_ID + # - ros2 param set $NAMESPACE/interface/mavros/mavros use_sim_time true diff --git a/docker/extras/kit-app-template/source/extensions/airlab.tmux_manager/airlab/tmux_manager/extension.py b/docker/extras/kit-app-template/source/extensions/airlab.tmux_manager/airlab/tmux_manager/extension.py index 6d484e52..f0f215a5 100644 --- a/docker/extras/kit-app-template/source/extensions/airlab.tmux_manager/airlab/tmux_manager/extension.py +++ b/docker/extras/kit-app-template/source/extensions/airlab.tmux_manager/airlab/tmux_manager/extension.py @@ -217,8 +217,16 @@ def on_startup(self, ext_id): ui.Spacer(height=5) self.sessions_stack = ui.VStack() #''' - self.sessions_dict = {} + + self.play_listener = self.timeline.get_timeline_event_stream().create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), self.refresh_tmux_sessions) + self.pause_listener = self.timeline.get_timeline_event_stream().create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PAUSE), self.refresh_tmux_sessions) + self.stop_listener = self.timeline.get_timeline_event_stream().create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), self.refresh_tmux_sessions) + + self.refresh_tmux_sessions() ''' with self.window.frame: @@ -291,4 +299,5 @@ def kill(): del self.sessions_dict[k] def on_shutdown(self): + self.refresh_tmux_sessions() print("[airlab.tmux_manager] Extension shutdown") diff --git a/docker/isaac-sim/user.config.json b/docker/isaac-sim/user.config.json new file mode 100644 index 00000000..8ef2eafd --- /dev/null +++ b/docker/isaac-sim/user.config.json @@ -0,0 +1,2858 @@ +{ + "persistent": { + "simulation": { + "minFrameRate": 60, + "defaultMetersPerUnit": 1.0 + }, + "omni": { + "replicator": { + "captureOnPlay": true + } + }, + "renderer": { + "startupMessageDisplayed": false, + "raytracingOmm": { + "enabled": false + } + }, + "resourcemonitor": { + "timeBetweenQueries": 100, + "sendDeviceMemoryWarning": true, + "deviceMemoryWarnMB": 1, + "deviceMemoryWarnFraction": 0.10000000149011612, + "sendHostMemoryWarning": true, + "hostMemoryWarnMB": 1, + "hostMemoryWarnFraction": 0.10000000149011612 + }, + "audio": { + "context": { + "streamerFile": "", + "speakerMode": "auto-detect", + "deviceName": "", + "captureDeviceName": "", + "autoStreamThreshold": 0, + "audioPlayerAutoStreamThreshold": 256, + "masterVolume": 1.0, + "usdVolume": 1.0, + "spatialVolume": 1.0, + "nonSpatialVolume": 1.0, + "closeAudioPlayerOnStop": false, + "uiVolume": 1.0 + } + }, + "ext": { + "omni.kit.usd.layers": { + "auto_reload_sublayers": false + }, + "omni.kit.widget.stage": { + "show_prim_displayname": false, + "auto_reload_non_sublayers": false + }, + "omni.usd": { + "keep_children_order": false + } + }, + "omnigraph": { + "updateToUsd": false, + "useSchemaPrims": true, + "disablePrimNodes": true, + "updateMeshPointsToHydra": false, + "generator": { + "pyOptimize": false + }, + "enablePathChangedCallback": true, + "deprecationsAreErrors": false, + "disableInfoNoticeHandlingInPlayback": false, + "autoInstancingEnabled": true, + "settingsVersion": 5, + "showNameAsType": true + }, + "physics": { + "visualizationDisplayJoints": false, + "visualizationSimulationOutput": false, + "useActiveCudaContext": false, + "visualizationQueryUsdrtForTraversal": false, + "visualizationDisplayAttachmentsHideActor1": false, + "visualizationDisplayAttachmentsHideActor0": false, + "visualizationDisplayParticlesShowDeformableMesh": false, + "visualizationDisplayParticlesShowFluidSurface": false, + "visualizationDisplayParticlesClothMeshLines": false, + "visualizationDisplayParticlesShowDiffuseParticles": false, + "visualizationDisplayColliderNormals": false, + "visualizationSimulationDataVisualizer": false, + "visualizationDisplayParticlesShowDeformableParticles": true, + "pvdStreamToFile": false, + "pvdEnabled": false, + "pvdDebug": true, + "pvdProfile": true, + "visualizationDisplayParticlesShowParticleSetParticles": true, + "pvdMemory": true, + "useLocalMeshCache": true, + "autocreatePhysicsScene": true, + "massDistributionManipulator": false, + "visualizationDisplayParticlesParticleRadius": 2, + "visualizationDisplayParticles": 0, + "visualizationDisplayAttachments": 0, + "visualizationDisplayDeformableBodyType": 0, + "visualizationDisplayTendons": 0, + "visualizationDisplayColliders": 0, + "backwardCompatibilityCheckMode": 2, + "localMeshCacheSizeMB": 1024, + "cooking": { + "ujitsoCookingMaxProcessCount": 16 + }, + "visualizationDisplayMassProperties": 0, + "visualizationDisplayParticlesParticlePositions": 0, + "numThreads": 8, + "visualizationDisplayDeformableBodies": 0, + "overrideGPUSettings": -1, + "visualizationSimplifyAtDistance": 2500.0, + "visualizationGap": 0.20000000298023224, + "omniPvdOvdRecordingDirectory": "", + "testRunnerFilter": "", + "pvdOutputDirectory": "", + "pvdIP": "127.0.0.1", + "testRunnerSelection": {}, + "supportUiActionBarEnabled": false, + "supportUiLoggingEnabled": false, + "supportUiFloatingNotificationsEnabled": true, + "supportUiAutomaticCollisionCreationEnabled": false, + "supportUiAsyncCookingAtStageLoad": true, + "supportUiCustomManipulatorEnabled": true, + "supportUiManipModeAllowRigidBodyTraversal": 0, + "supportUiManipModeAllowRotWhileTranslating": 1, + "supportUiManipModeAllowTranOnOtherAxesWhileTranslating": 1, + "supportUiManipModeAllowTranWhileRotating": 1, + "supportUiManipModeAllowRotOnOtherAxesWhileRotating": 1, + "supportUiToolbarButtonsWithText": false, + "supportUiAvoidChangingExistingColliders": false, + "supportUiStaticColliderSimplificationType": 0, + "supportUiDynamicColliderSimplificationType": 0, + "resetOnStop": true + }, + "omnihydra": { + "useFastSceneDelegate": true, + "useSceneGraphInstancing": false, + "useCachedXformTimeSamples": false, + "useSkelAdapter": true, + "useSkelAdapterBlendShape": true, + "useSkelAdapterDeformGraph": false, + "skinDeformerExtLoaded": false + }, + "rtx": { + "resourcemanager": { + "placeholderTextureColor": { + "0": 0.0, + "1": 1.0, + "2": 1.0 + } + }, + "mdltranslator": { + "mdlDistilling": true + }, + "sceneDb": { + "allowDuplicateAhsInvocation": true + }, + "hydra": { + "points": { + "defaultWidth": 1.0 + }, + "readTransformsFromFabricInRenderDelegate": false + } + }, + "exts": { + "omni.anim.navigation.core": { + "navMesh": { + "viewNavMesh": false, + "config": { + "autoRebakeOnChanges": false + } + } + }, + "omni.kit.manipulator.camera": { + "lookSpeed": { + "0": 180.0, + "1": 90.0 + }, + "tumbleSpeed": 360.0, + "moveSpeed": { + "0": 1.0, + "1": 1.0, + "2": 1.0 + }, + "flyViewLock": false + }, + "omni.kit.window.script_editor": { + "clearAfterExecute": false, + "editorPalette": "Dark", + "executeOnReload": false, + "fontSize": 14, + "font": "" + }, + "omni.kit.thumbnails.usd": { + "thumbnail_on_save": true, + "wait_stage_load": false, + "wait_stage_load_timeout": 50 + }, + "omni.kit.manipulator.selector": { + "orders": { + "omni.physxsupportui": -10, + "omni.kit.manipulator.prim.core": -1 + } + }, + "omni.kit.manipulator.prim.core": { + "manipulator": { + "placement": "Authored Pivot" + } + }, + "omni.kit.viewport.manipulator.transform": { + "manipulator": { + "freeRotationType": "Clamped", + "freeRotationEnabled": true, + "scaleMultiplier": 1.4, + "intersectionThickness": 10 + } + }, + "omni.kit.manipulator.tool.snap": { + "conformToTarget": false, + "conformUpAxis": "Stage", + "explicitTransform": { + "rotate": 1.0, + "scale": 1.0, + "translate": 1.0 + }, + "keepSpacing": true + }, + "omni.kit.manipulator.transform": { + "manipulator": { + "freeRotationType": "Clamped", + "freeRotationEnabled": true, + "scaleMultiplier": 1.4, + "intersectionThickness": 10, + "omniScaleDirection": "XY" + }, + "tools": { + "defaultCollapsed": true + } + }, + "omni.kit.window.extensions": { + "publishingEnabled": false, + "openInVSCode": false + }, + "omni.kit.property.usd": { + "large_selection": 100, + "raw_widget_multi_selection_limit": 1, + "references_check_missing": true, + "references_hide_max": 5 + }, + "omni.kit.viewport.window": { + "cameraSpeedMessage": { + "collapsed": false + } + }, + "omni.kit.widget.stage": { + "showColumnVisibility": true, + "showColumnReload": true, + "showColumnType": true + }, + "omni.kit.material.library": { + "browserPath": "" + }, + "omni.kit.window.filepicker": { + "window_split": 306, + "show_grid_view": true, + "grid_view_scale": 2 + }, + "omni.importer.onshape.settings": { + "filter_unsupported": false, + "import_physics": true, + "chord_tolerance": 0.001, + "angle_tolerance": 15, + "max_chord": 0.2, + "default_import_folder": "/root/Documents", + "import_in_place": true, + "USE_API_KEY": false, + "API_KEY": "", + "API_SECRET": "", + "BASE_URL": "https://cad.onshape.com", + "AUTH_URL": "https://oauth.onshape.com/oauth/authorize", + "TOKEN_URL": "https://oauth.onshape.com/oauth/token" + }, + "omni.kit.viewport.menubar.core": { + "spacer": { + "order": 0 + } + }, + "omni.kit.viewport.menubar.display": { + "visible": true, + "order": -80 + }, + "omni.syntheticdata": { + "menubar": { + "visible": true, + "order": -1 + } + }, + "omni.kit.viewport.menubar.lighting": { + "autoLightRig": { + "enabled": true, + "searchFromDefaultPrim": false + }, + "visible": true, + "order": 100 + }, + "omni.kit.window.stage": { + "columns": "Visibility|Type" + }, + "omni.kit.widget.extended_searchfield": { + "engine": "" + }, + "omni.kit.viewport.menubar.camera": { + "expand": false, + "visible": true, + "order": -10 + }, + "omni.kit.viewport.menubar.render": { + "visible": true, + "order": -90 + }, + "omni.kit.viewport.menubar.settings": { + "visible": true, + "order": -100 + }, + "omni.kit.window.content_browser": { + "show_grid_view": true, + "grid_view_scale": 2, + "current_directory": "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack" + }, + "omni.kit.registry.nucleus": { + "userRegistries": {} + }, + "omni.anim.curve.core": { + "defaultTangentType": "auto", + "defaultTangentWeighted": false, + "defaultTangentBroken": false, + "defaultTangentPreInfinity": "constant", + "defaultTangentPostInfinity": "constant" + } + }, + "isaac": { + "asset_root": { + "nvidia": "omniverse://localhost/NVIDIA", + "isaac": "omniverse://localhost/NVIDIA/Assets/Isaac/4.1/Isaac", + "cloud": "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.1", + "timeout": 5.0, + "default": "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.1" + } + }, + "app": { + "transform": { + "gizmoUseSRT": true + }, + "camera": { + "controllerUseSRT": true + }, + "renderer": { + "resolution": { + "custom": { + "list": {} + } + } + }, + "newStage": { + "defaultTemplate": "sunlight", + "templatePath": [ + "${app_documents}/scripts/new_stage" + ] + }, + "rendering": { + "whiteModeExceptions": "GizmoTex, Gizmo, OmniGlass, SunsetSkyMat" + }, + "usd": { + "muteUsdCodingError": false, + "usdLuxUnprefixedCompat": true + }, + "layersinterface": { + "continousUpdate": true + }, + "primCreation": { + "DefaultXformOpType": "Scale, Orient, Translate", + "DefaultXformOpOrder": "xformOp:translate, xformOp:orient, xformOp:scale", + "typedDefaults": { + "camera": { + "clippingRange": [ + 0.01, + 10000000.0 + ], + "focalLength": 18.147562, + "focusDistance": 400 + }, + "orthoCamera": { + "clippingRange": [ + 1.0, + 10000000.0 + ] + } + }, + "DefaultRotationOrder": "XYZ", + "DefaultCameraRotationOrder": "YXZ", + "highQuality": true, + "PrimCreationWithDefaultXformOps": true, + "DefaultXformOpPrecision": "Double" + }, + "datetime": { + "format": "MM/DD/YYYY" + }, + "material": { + "dragDropMaterialPath": "Relative" + }, + "layerwindow": { + "showLayerContents": true, + "showSessionLayer": false, + "showMetricsAssemblerLayer": false, + "showLayerFileExtension": true, + "filedialog": { + "showRootLayerLocation": true + }, + "showInfoNotification": true, + "showWarningNotification": true, + "enableAutoAuthoringMode": false, + "enableSpecLinkingMode": false, + "showMissingReference": false, + "showMergeOrFlattenWarning": true + }, + "captureFrame": { + "path": "/root/Documents/Kit/shared/screenshots/", + "viewport": true + }, + "thumbnailsGenerator": { + "usd": { + "renderer": "RayTracing", + "iterations": 8 + }, + "mdl": { + "renderer": "PathTracing", + "iterations": 512, + "usd_template_path": "/Library/AEC/Materials/Library_Default.thumbnail.usd", + "standin_usd_path": "/Stage/ShaderKnob" + } + }, + "hydra": { + "displayPurpose": { + "guide": false, + "proxy": false, + "render": true + }, + "material": { + "renderContext": "mdl" + } + }, + "uiSettings": { + "DisplayOrientAsRotate": true + }, + "window": { + "uiStyle": "NvidiaDark", + "maximized": true, + "width": -1, + "height": -1 + }, + "omniverse": { + "content_browser": { + "options_menu": { + "show_details": true + } + }, + "filepicker": { + "options_menu": { + "show_details": true + } + }, + "gamepadCameraControl": false, + "lastOpenDirectory": "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack", + "fileEventHistory": {} + }, + "file_importer": { + "directory": "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/" + }, + "file": { + "recentFiles": {}, + "save": { + "showUnsavedLayersDialog": true + } + }, + "file_exporter": { + "directory": "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/" + }, + "exts": { + "enabled": { + "0": "airlab.tmux_manager-0.1.0", + "1": "airlab.airstack-1.19.1" + } + }, + "mesh_generator": { + "shapes": { + "cube": { + "object_half_scale": 50, + "u_scale": 1, + "v_scale": 1, + "w_scale": 1 + }, + "cone": { + "object_half_scale": 50, + "u_scale": 1, + "v_scale": 3, + "w_scale": 1 + }, + "plane": { + "object_half_scale": 50, + "u_scale": 1, + "v_scale": 1 + } + } + }, + "anim": { + "snapToFrame": true, + "autoKeyAllXform": false + }, + "omni.anim.curve.core": { + "deleteEmptyCurve": true + }, + "stage": { + "upAxis": "Z", + "movePrimInPlace": false, + "instanceableOnCreatingReference": false, + "materialStrength": "weakerThanDescendants", + "interpolationType": "Linear", + "timeCodesPerSecond": 60.0, + "defaultPrimName": "World", + "dragDropImport": "payload", + "nestedGprimsAuthoring": false, + "timeCodeRange": [ + 0, + 1000000 + ] + }, + "extensions": { + "console": { + "filterLevel": 0, + "sources": { + "carb.windowing-glfw.gamepad": true, + "omni.replicator.core.scripts.annotators": true, + "omni.kit.widget.cache_indicator.cache_state_menu": true, + "omni.kit.profiler.window": true, + "omni.isaac.ros2_bridge.plugin": true, + "carb.audio.device": true, + "carb.audio.output": true, + "carb.audio.context": true, + "omni.hydra.scene_delegate.plugin": true, + "dronekit": true, + "carb.crashreporter-breakpad.plugin": true, + "omni.isaac.ros2_bridge.scripts.extension": true, + "omni.ext._impl._internal": true, + "omni.kit.app._impl": true, + "omni.platforminfo.plugin": true, + "carb.windowing-glfw.plugin": true, + "carb": true, + "omni.kvdb.plugin": true, + "omni.isaac.core.simulation_context.simulation_context": true, + "rtx.neuraylib.plugin": true, + "gpu.foundation.plugin": true, + "omni.hydra": true, + "omni.fabric.plugin": true, + "omni.kit.window.filepicker.model": true, + "omni.usd": true, + "omni.timeline.plugin": true, + "omni.replicator.core.scripts.orchestrator": true, + "autopilot": true, + "omni.graph": true, + "rtx.rtxsensor.plugin": true, + "dronekit.mavlink": true, + "omni.isaac.core_nodes.impl.base_writer_node": true, + "omni.graph.window.core.graph_config": true, + "omni.syntheticdata.plugin": true, + "omni.isaac.sensor.plugin": true, + "omni.kit.widget.settings.settings_model": true, + "omni.kit.widget.versioning.checkpoints_model": true + } + } + }, + "viewport": { + "defaults": { + "tickRate": 120 + }, + "pickingMode": "kind:model.ALL", + "camMoveVelocity": 0.03156265702871778, + "previewOnPeek": false, + "snapToSurface": false, + "displayOptions": 1024, + "pickingModeNoKinds": true, + "pickingModeIncludeRef": true, + "autoFrame": { + "mode": "", + "singleCamera": true, + "implicitOnly": true + }, + "outline": { + "width": 2, + "intersection": { + "color": { + "0": 1.0, + "1": 0.6, + "2": 0.0, + "3": 1.0 + } + }, + "color": { + "0": 0.0, + "1": 0.0, + "2": 0.5714285969734192, + "3": 1.0, + "4": 0.0, + "5": 0.8999999761581421, + "6": 0.0, + "7": 1.0, + "8": 0.8999999761581421, + "9": 0.0, + "10": 0.0, + "11": 1.0, + "12": 1.0, + "13": 0.6000000238418579, + "14": 0.0, + "15": 1.0, + "16": 1.0, + "17": 0.6000000238418579, + "18": 0.0, + "19": 1.0, + "20": 0.0, + "21": 0.1428571492433548, + "22": 0.7142857313156128, + "23": 1.0, + "24": 0.0, + "25": 0.1428571492433548, + "26": 0.8571428656578064, + "27": 1.0, + "28": 0.0, + "29": 0.1428571492433548, + "30": 1.0, + "31": 1.0, + "32": 0.0, + "33": 0.2857142984867096, + "34": 0.5714285969734192, + "35": 1.0, + "36": 0.0, + "37": 0.2857142984867096, + "38": 0.7142857313156128, + "39": 1.0, + "40": 0.0, + "41": 0.2857142984867096, + "42": 0.8571428656578064, + "43": 1.0, + "44": 0.0, + "45": 0.2857142984867096, + "46": 1.0, + "47": 1.0, + "48": 0.0, + "49": 0.4285714328289032, + "50": 0.5714285969734192, + "51": 1.0, + "52": 0.0, + "53": 0.4285714328289032, + "54": 0.7142857313156128, + "55": 1.0, + "56": 0.0, + "57": 0.4285714328289032, + "58": 0.8571428656578064, + "59": 1.0, + "60": 0.0, + "61": 0.4285714328289032, + "62": 1.0, + "63": 1.0, + "64": 0.0, + "65": 0.5714285969734192, + "66": 0.5714285969734192, + "67": 1.0, + "68": 0.0, + "69": 0.5714285969734192, + "70": 0.7142857313156128, + "71": 1.0, + "72": 0.0, + "73": 0.5714285969734192, + "74": 0.8571428656578064, + "75": 1.0, + "76": 0.0, + "77": 0.5714285969734192, + "78": 1.0, + "79": 1.0, + "80": 0.0, + "81": 0.7142857313156128, + "82": 0.5714285969734192, + "83": 1.0, + "84": 0.0, + "85": 0.7142857313156128, + "86": 0.7142857313156128, + "87": 1.0, + "88": 0.0, + "89": 0.7142857313156128, + "90": 0.8571428656578064, + "91": 1.0, + "92": 0.0, + "93": 0.7142857313156128, + "94": 1.0, + "95": 1.0, + "96": 0.0, + "97": 0.8571428656578064, + "98": 0.5714285969734192, + "99": 1.0, + "100": 0.0, + "101": 0.8571428656578064, + "102": 0.7142857313156128, + "103": 1.0, + "104": 0.0, + "105": 0.8571428656578064, + "106": 0.8571428656578064, + "107": 1.0, + "108": 0.0, + "109": 0.8571428656578064, + "110": 1.0, + "111": 1.0, + "112": 0.0, + "113": 1.0, + "114": 0.5714285969734192, + "115": 1.0, + "116": 0.0, + "117": 1.0, + "118": 0.7142857313156128, + "119": 1.0, + "120": 0.0, + "121": 1.0, + "122": 0.8571428656578064, + "123": 1.0, + "124": 0.0, + "125": 1.0, + "126": 1.0, + "127": 1.0, + "128": 0.1428571492433548, + "129": 0.0, + "130": 0.5714285969734192, + "131": 1.0, + "132": 0.1428571492433548, + "133": 0.0, + "134": 0.7142857313156128, + "135": 1.0, + "136": 0.1428571492433548, + "137": 0.0, + "138": 0.8571428656578064, + "139": 1.0, + "140": 0.1428571492433548, + "141": 0.0, + "142": 1.0, + "143": 1.0, + "144": 0.1428571492433548, + "145": 0.1428571492433548, + "146": 0.5714285969734192, + "147": 1.0, + "148": 0.1428571492433548, + "149": 0.1428571492433548, + "150": 0.7142857313156128, + "151": 1.0, + "152": 0.1428571492433548, + "153": 0.1428571492433548, + "154": 0.8571428656578064, + "155": 1.0, + "156": 0.1428571492433548, + "157": 0.1428571492433548, + "158": 1.0, + "159": 1.0, + "160": 0.1428571492433548, + "161": 0.2857142984867096, + "162": 0.5714285969734192, + "163": 1.0, + "164": 0.1428571492433548, + "165": 0.2857142984867096, + "166": 0.7142857313156128, + "167": 1.0, + "168": 0.1428571492433548, + "169": 0.2857142984867096, + "170": 0.8571428656578064, + "171": 1.0, + "172": 0.1428571492433548, + "173": 0.2857142984867096, + "174": 1.0, + "175": 1.0, + "176": 0.1428571492433548, + "177": 0.4285714328289032, + "178": 0.5714285969734192, + "179": 1.0, + "180": 0.1428571492433548, + "181": 0.4285714328289032, + "182": 0.7142857313156128, + "183": 1.0, + "184": 0.1428571492433548, + "185": 0.4285714328289032, + "186": 0.8571428656578064, + "187": 1.0, + "188": 0.1428571492433548, + "189": 0.4285714328289032, + "190": 1.0, + "191": 1.0, + "192": 0.1428571492433548, + "193": 0.5714285969734192, + "194": 0.5714285969734192, + "195": 1.0, + "196": 0.1428571492433548, + "197": 0.5714285969734192, + "198": 0.7142857313156128, + "199": 1.0, + "200": 0.1428571492433548, + "201": 0.5714285969734192, + "202": 0.8571428656578064, + "203": 1.0, + "204": 0.1428571492433548, + "205": 0.5714285969734192, + "206": 1.0, + "207": 1.0, + "208": 0.1428571492433548, + "209": 0.7142857313156128, + "210": 0.5714285969734192, + "211": 1.0, + "212": 0.1428571492433548, + "213": 0.7142857313156128, + "214": 0.7142857313156128, + "215": 1.0, + "216": 0.1428571492433548, + "217": 0.7142857313156128, + "218": 0.8571428656578064, + "219": 1.0, + "220": 0.1428571492433548, + "221": 0.7142857313156128, + "222": 1.0, + "223": 1.0, + "224": 0.1428571492433548, + "225": 0.8571428656578064, + "226": 0.5714285969734192, + "227": 1.0, + "228": 0.1428571492433548, + "229": 0.8571428656578064, + "230": 0.7142857313156128, + "231": 1.0, + "232": 0.1428571492433548, + "233": 0.8571428656578064, + "234": 0.8571428656578064, + "235": 1.0, + "236": 0.1428571492433548, + "237": 0.8571428656578064, + "238": 1.0, + "239": 1.0, + "240": 0.1428571492433548, + "241": 1.0, + "242": 0.5714285969734192, + "243": 1.0, + "244": 0.1428571492433548, + "245": 1.0, + "246": 0.7142857313156128, + "247": 1.0, + "248": 0.1428571492433548, + "249": 1.0, + "250": 0.8571428656578064, + "251": 1.0, + "252": 0.1428571492433548, + "253": 1.0, + "254": 1.0, + "255": 1.0, + "256": 0.2857142984867096, + "257": 0.0, + "258": 0.5714285969734192, + "259": 1.0, + "260": 0.2857142984867096, + "261": 0.0, + "262": 0.7142857313156128, + "263": 1.0, + "264": 0.2857142984867096, + "265": 0.0, + "266": 0.8571428656578064, + "267": 1.0, + "268": 0.2857142984867096, + "269": 0.0, + "270": 1.0, + "271": 1.0, + "272": 0.2857142984867096, + "273": 0.1428571492433548, + "274": 0.5714285969734192, + "275": 1.0, + "276": 0.2857142984867096, + "277": 0.1428571492433548, + "278": 0.7142857313156128, + "279": 1.0, + "280": 0.2857142984867096, + "281": 0.1428571492433548, + "282": 0.8571428656578064, + "283": 1.0, + "284": 0.2857142984867096, + "285": 0.1428571492433548, + "286": 1.0, + "287": 1.0, + "288": 0.2857142984867096, + "289": 0.2857142984867096, + "290": 0.5714285969734192, + "291": 1.0, + "292": 0.2857142984867096, + "293": 0.2857142984867096, + "294": 0.7142857313156128, + "295": 1.0, + "296": 0.2857142984867096, + "297": 0.2857142984867096, + "298": 0.8571428656578064, + "299": 1.0, + "300": 0.2857142984867096, + "301": 0.2857142984867096, + "302": 1.0, + "303": 1.0, + "304": 0.2857142984867096, + "305": 0.4285714328289032, + "306": 0.5714285969734192, + "307": 1.0, + "308": 0.2857142984867096, + "309": 0.4285714328289032, + "310": 0.7142857313156128, + "311": 1.0, + "312": 0.2857142984867096, + "313": 0.4285714328289032, + "314": 0.8571428656578064, + "315": 1.0, + "316": 0.2857142984867096, + "317": 0.4285714328289032, + "318": 1.0, + "319": 1.0, + "320": 0.2857142984867096, + "321": 0.5714285969734192, + "322": 0.5714285969734192, + "323": 1.0, + "324": 0.2857142984867096, + "325": 0.5714285969734192, + "326": 0.7142857313156128, + "327": 1.0, + "328": 0.2857142984867096, + "329": 0.5714285969734192, + "330": 0.8571428656578064, + "331": 1.0, + "332": 0.2857142984867096, + "333": 0.5714285969734192, + "334": 1.0, + "335": 1.0, + "336": 0.2857142984867096, + "337": 0.7142857313156128, + "338": 0.5714285969734192, + "339": 1.0, + "340": 0.2857142984867096, + "341": 0.7142857313156128, + "342": 0.7142857313156128, + "343": 1.0, + "344": 0.2857142984867096, + "345": 0.7142857313156128, + "346": 0.8571428656578064, + "347": 1.0, + "348": 0.2857142984867096, + "349": 0.7142857313156128, + "350": 1.0, + "351": 1.0, + "352": 0.2857142984867096, + "353": 0.8571428656578064, + "354": 0.5714285969734192, + "355": 1.0, + "356": 0.2857142984867096, + "357": 0.8571428656578064, + "358": 0.7142857313156128, + "359": 1.0, + "360": 0.2857142984867096, + "361": 0.8571428656578064, + "362": 0.8571428656578064, + "363": 1.0, + "364": 0.2857142984867096, + "365": 0.8571428656578064, + "366": 1.0, + "367": 1.0, + "368": 0.2857142984867096, + "369": 1.0, + "370": 0.5714285969734192, + "371": 1.0, + "372": 0.2857142984867096, + "373": 1.0, + "374": 0.7142857313156128, + "375": 1.0, + "376": 0.2857142984867096, + "377": 1.0, + "378": 0.8571428656578064, + "379": 1.0, + "380": 0.2857142984867096, + "381": 1.0, + "382": 1.0, + "383": 1.0, + "384": 0.4285714328289032, + "385": 0.0, + "386": 0.5714285969734192, + "387": 1.0, + "388": 0.4285714328289032, + "389": 0.0, + "390": 0.7142857313156128, + "391": 1.0, + "392": 0.4285714328289032, + "393": 0.0, + "394": 0.8571428656578064, + "395": 1.0, + "396": 0.4285714328289032, + "397": 0.0, + "398": 1.0, + "399": 1.0, + "400": 0.4285714328289032, + "401": 0.1428571492433548, + "402": 0.5714285969734192, + "403": 1.0, + "404": 0.4285714328289032, + "405": 0.1428571492433548, + "406": 0.7142857313156128, + "407": 1.0, + "408": 0.4285714328289032, + "409": 0.1428571492433548, + "410": 0.8571428656578064, + "411": 1.0, + "412": 0.4285714328289032, + "413": 0.1428571492433548, + "414": 1.0, + "415": 1.0, + "416": 0.4285714328289032, + "417": 0.2857142984867096, + "418": 0.5714285969734192, + "419": 1.0, + "420": 0.4285714328289032, + "421": 0.2857142984867096, + "422": 0.7142857313156128, + "423": 1.0, + "424": 0.4285714328289032, + "425": 0.2857142984867096, + "426": 0.8571428656578064, + "427": 1.0, + "428": 0.4285714328289032, + "429": 0.2857142984867096, + "430": 1.0, + "431": 1.0, + "432": 0.4285714328289032, + "433": 0.4285714328289032, + "434": 0.5714285969734192, + "435": 1.0, + "436": 0.4285714328289032, + "437": 0.4285714328289032, + "438": 0.7142857313156128, + "439": 1.0, + "440": 0.4285714328289032, + "441": 0.4285714328289032, + "442": 0.8571428656578064, + "443": 1.0, + "444": 0.4285714328289032, + "445": 0.4285714328289032, + "446": 1.0, + "447": 1.0, + "448": 0.4285714328289032, + "449": 0.5714285969734192, + "450": 0.5714285969734192, + "451": 1.0, + "452": 0.4285714328289032, + "453": 0.5714285969734192, + "454": 0.7142857313156128, + "455": 1.0, + "456": 0.4285714328289032, + "457": 0.5714285969734192, + "458": 0.8571428656578064, + "459": 1.0, + "460": 0.4285714328289032, + "461": 0.5714285969734192, + "462": 1.0, + "463": 1.0, + "464": 0.4285714328289032, + "465": 0.7142857313156128, + "466": 0.5714285969734192, + "467": 1.0, + "468": 0.4285714328289032, + "469": 0.7142857313156128, + "470": 0.7142857313156128, + "471": 1.0, + "472": 0.4285714328289032, + "473": 0.7142857313156128, + "474": 0.8571428656578064, + "475": 1.0, + "476": 0.4285714328289032, + "477": 0.7142857313156128, + "478": 1.0, + "479": 1.0, + "480": 0.4285714328289032, + "481": 0.8571428656578064, + "482": 0.5714285969734192, + "483": 1.0, + "484": 0.4285714328289032, + "485": 0.8571428656578064, + "486": 0.7142857313156128, + "487": 1.0, + "488": 0.4285714328289032, + "489": 0.8571428656578064, + "490": 0.8571428656578064, + "491": 1.0, + "492": 0.4285714328289032, + "493": 0.8571428656578064, + "494": 1.0, + "495": 1.0, + "496": 0.4285714328289032, + "497": 1.0, + "498": 0.5714285969734192, + "499": 1.0, + "500": 0.4285714328289032, + "501": 1.0, + "502": 0.7142857313156128, + "503": 1.0, + "504": 0.4285714328289032, + "505": 1.0, + "506": 0.8571428656578064, + "507": 1.0, + "508": 0.4285714328289032, + "509": 1.0, + "510": 1.0, + "511": 1.0, + "512": 0.5714285969734192, + "513": 0.0, + "514": 0.5714285969734192, + "515": 1.0, + "516": 0.5714285969734192, + "517": 0.0, + "518": 0.7142857313156128, + "519": 1.0, + "520": 0.5714285969734192, + "521": 0.0, + "522": 0.8571428656578064, + "523": 1.0, + "524": 0.5714285969734192, + "525": 0.0, + "526": 1.0, + "527": 1.0, + "528": 0.5714285969734192, + "529": 0.1428571492433548, + "530": 0.5714285969734192, + "531": 1.0, + "532": 0.5714285969734192, + "533": 0.1428571492433548, + "534": 0.7142857313156128, + "535": 1.0, + "536": 0.5714285969734192, + "537": 0.1428571492433548, + "538": 0.8571428656578064, + "539": 1.0, + "540": 0.5714285969734192, + "541": 0.1428571492433548, + "542": 1.0, + "543": 1.0, + "544": 0.5714285969734192, + "545": 0.2857142984867096, + "546": 0.5714285969734192, + "547": 1.0, + "548": 0.5714285969734192, + "549": 0.2857142984867096, + "550": 0.7142857313156128, + "551": 1.0, + "552": 0.5714285969734192, + "553": 0.2857142984867096, + "554": 0.8571428656578064, + "555": 1.0, + "556": 0.5714285969734192, + "557": 0.2857142984867096, + "558": 1.0, + "559": 1.0, + "560": 0.5714285969734192, + "561": 0.4285714328289032, + "562": 0.5714285969734192, + "563": 1.0, + "564": 0.5714285969734192, + "565": 0.4285714328289032, + "566": 0.7142857313156128, + "567": 1.0, + "568": 0.5714285969734192, + "569": 0.4285714328289032, + "570": 0.8571428656578064, + "571": 1.0, + "572": 0.5714285969734192, + "573": 0.4285714328289032, + "574": 1.0, + "575": 1.0, + "576": 0.5714285969734192, + "577": 0.5714285969734192, + "578": 0.5714285969734192, + "579": 1.0, + "580": 0.5714285969734192, + "581": 0.5714285969734192, + "582": 0.7142857313156128, + "583": 1.0, + "584": 0.5714285969734192, + "585": 0.5714285969734192, + "586": 0.8571428656578064, + "587": 1.0, + "588": 0.5714285969734192, + "589": 0.5714285969734192, + "590": 1.0, + "591": 1.0, + "592": 0.5714285969734192, + "593": 0.7142857313156128, + "594": 0.5714285969734192, + "595": 1.0, + "596": 0.5714285969734192, + "597": 0.7142857313156128, + "598": 0.7142857313156128, + "599": 1.0, + "600": 0.5714285969734192, + "601": 0.7142857313156128, + "602": 0.8571428656578064, + "603": 1.0, + "604": 0.5714285969734192, + "605": 0.7142857313156128, + "606": 1.0, + "607": 1.0, + "608": 0.5714285969734192, + "609": 0.8571428656578064, + "610": 0.5714285969734192, + "611": 1.0, + "612": 0.5714285969734192, + "613": 0.8571428656578064, + "614": 0.7142857313156128, + "615": 1.0, + "616": 0.5714285969734192, + "617": 0.8571428656578064, + "618": 0.8571428656578064, + "619": 1.0, + "620": 0.5714285969734192, + "621": 0.8571428656578064, + "622": 1.0, + "623": 1.0, + "624": 0.5714285969734192, + "625": 1.0, + "626": 0.5714285969734192, + "627": 1.0, + "628": 0.5714285969734192, + "629": 1.0, + "630": 0.7142857313156128, + "631": 1.0, + "632": 0.5714285969734192, + "633": 1.0, + "634": 0.8571428656578064, + "635": 1.0, + "636": 0.5714285969734192, + "637": 1.0, + "638": 1.0, + "639": 1.0, + "640": 0.7142857313156128, + "641": 0.0, + "642": 0.5714285969734192, + "643": 1.0, + "644": 0.7142857313156128, + "645": 0.0, + "646": 0.7142857313156128, + "647": 1.0, + "648": 0.7142857313156128, + "649": 0.0, + "650": 0.8571428656578064, + "651": 1.0, + "652": 0.7142857313156128, + "653": 0.0, + "654": 1.0, + "655": 1.0, + "656": 0.7142857313156128, + "657": 0.1428571492433548, + "658": 0.5714285969734192, + "659": 1.0, + "660": 0.7142857313156128, + "661": 0.1428571492433548, + "662": 0.7142857313156128, + "663": 1.0, + "664": 0.7142857313156128, + "665": 0.1428571492433548, + "666": 0.8571428656578064, + "667": 1.0, + "668": 0.7142857313156128, + "669": 0.1428571492433548, + "670": 1.0, + "671": 1.0, + "672": 0.7142857313156128, + "673": 0.2857142984867096, + "674": 0.5714285969734192, + "675": 1.0, + "676": 0.7142857313156128, + "677": 0.2857142984867096, + "678": 0.7142857313156128, + "679": 1.0, + "680": 0.7142857313156128, + "681": 0.2857142984867096, + "682": 0.8571428656578064, + "683": 1.0, + "684": 0.7142857313156128, + "685": 0.2857142984867096, + "686": 1.0, + "687": 1.0, + "688": 0.7142857313156128, + "689": 0.4285714328289032, + "690": 0.5714285969734192, + "691": 1.0, + "692": 0.7142857313156128, + "693": 0.4285714328289032, + "694": 0.7142857313156128, + "695": 1.0, + "696": 0.7142857313156128, + "697": 0.4285714328289032, + "698": 0.8571428656578064, + "699": 1.0, + "700": 0.7142857313156128, + "701": 0.4285714328289032, + "702": 1.0, + "703": 1.0, + "704": 0.7142857313156128, + "705": 0.5714285969734192, + "706": 0.5714285969734192, + "707": 1.0, + "708": 0.7142857313156128, + "709": 0.5714285969734192, + "710": 0.7142857313156128, + "711": 1.0, + "712": 0.7142857313156128, + "713": 0.5714285969734192, + "714": 0.8571428656578064, + "715": 1.0, + "716": 0.7142857313156128, + "717": 0.5714285969734192, + "718": 1.0, + "719": 1.0, + "720": 0.7142857313156128, + "721": 0.7142857313156128, + "722": 0.5714285969734192, + "723": 1.0, + "724": 0.7142857313156128, + "725": 0.7142857313156128, + "726": 0.7142857313156128, + "727": 1.0, + "728": 0.7142857313156128, + "729": 0.7142857313156128, + "730": 0.8571428656578064, + "731": 1.0, + "732": 0.7142857313156128, + "733": 0.7142857313156128, + "734": 1.0, + "735": 1.0, + "736": 0.7142857313156128, + "737": 0.8571428656578064, + "738": 0.5714285969734192, + "739": 1.0, + "740": 0.7142857313156128, + "741": 0.8571428656578064, + "742": 0.7142857313156128, + "743": 1.0, + "744": 0.7142857313156128, + "745": 0.8571428656578064, + "746": 0.8571428656578064, + "747": 1.0, + "748": 0.7142857313156128, + "749": 0.8571428656578064, + "750": 1.0, + "751": 1.0, + "752": 0.7142857313156128, + "753": 1.0, + "754": 0.5714285969734192, + "755": 1.0, + "756": 0.7142857313156128, + "757": 1.0, + "758": 0.7142857313156128, + "759": 1.0, + "760": 0.7142857313156128, + "761": 1.0, + "762": 0.8571428656578064, + "763": 1.0, + "764": 0.7142857313156128, + "765": 1.0, + "766": 1.0, + "767": 1.0, + "768": 0.8571428656578064, + "769": 0.0, + "770": 0.5714285969734192, + "771": 1.0, + "772": 0.8571428656578064, + "773": 0.0, + "774": 0.7142857313156128, + "775": 1.0, + "776": 0.8571428656578064, + "777": 0.0, + "778": 0.8571428656578064, + "779": 1.0, + "780": 0.8571428656578064, + "781": 0.0, + "782": 1.0, + "783": 1.0, + "784": 0.8571428656578064, + "785": 0.1428571492433548, + "786": 0.5714285969734192, + "787": 1.0, + "788": 0.8571428656578064, + "789": 0.1428571492433548, + "790": 0.7142857313156128, + "791": 1.0, + "792": 0.8571428656578064, + "793": 0.1428571492433548, + "794": 0.8571428656578064, + "795": 1.0, + "796": 0.8571428656578064, + "797": 0.1428571492433548, + "798": 1.0, + "799": 1.0, + "800": 0.8571428656578064, + "801": 0.2857142984867096, + "802": 0.5714285969734192, + "803": 1.0, + "804": 0.8571428656578064, + "805": 0.2857142984867096, + "806": 0.7142857313156128, + "807": 1.0, + "808": 0.8571428656578064, + "809": 0.2857142984867096, + "810": 0.8571428656578064, + "811": 1.0, + "812": 0.8571428656578064, + "813": 0.2857142984867096, + "814": 1.0, + "815": 1.0, + "816": 0.8571428656578064, + "817": 0.4285714328289032, + "818": 0.5714285969734192, + "819": 1.0, + "820": 0.8571428656578064, + "821": 0.4285714328289032, + "822": 0.7142857313156128, + "823": 1.0, + "824": 0.8571428656578064, + "825": 0.4285714328289032, + "826": 0.8571428656578064, + "827": 1.0, + "828": 0.8571428656578064, + "829": 0.4285714328289032, + "830": 1.0, + "831": 1.0, + "832": 0.8571428656578064, + "833": 0.5714285969734192, + "834": 0.5714285969734192, + "835": 1.0, + "836": 0.8571428656578064, + "837": 0.5714285969734192, + "838": 0.7142857313156128, + "839": 1.0, + "840": 0.8571428656578064, + "841": 0.5714285969734192, + "842": 0.8571428656578064, + "843": 1.0, + "844": 0.8571428656578064, + "845": 0.5714285969734192, + "846": 1.0, + "847": 1.0, + "848": 0.8571428656578064, + "849": 0.7142857313156128, + "850": 0.5714285969734192, + "851": 1.0, + "852": 0.8571428656578064, + "853": 0.7142857313156128, + "854": 0.7142857313156128, + "855": 1.0, + "856": 0.8571428656578064, + "857": 0.7142857313156128, + "858": 0.8571428656578064, + "859": 1.0, + "860": 0.8571428656578064, + "861": 0.7142857313156128, + "862": 1.0, + "863": 1.0, + "864": 0.8571428656578064, + "865": 0.8571428656578064, + "866": 0.5714285969734192, + "867": 1.0, + "868": 0.8571428656578064, + "869": 0.8571428656578064, + "870": 0.7142857313156128, + "871": 1.0, + "872": 0.8571428656578064, + "873": 0.8571428656578064, + "874": 0.8571428656578064, + "875": 1.0, + "876": 0.8571428656578064, + "877": 0.8571428656578064, + "878": 1.0, + "879": 1.0, + "880": 0.8571428656578064, + "881": 1.0, + "882": 0.5714285969734192, + "883": 1.0, + "884": 0.8571428656578064, + "885": 1.0, + "886": 0.7142857313156128, + "887": 1.0, + "888": 0.8571428656578064, + "889": 1.0, + "890": 0.8571428656578064, + "891": 1.0, + "892": 0.8571428656578064, + "893": 1.0, + "894": 1.0, + "895": 1.0, + "896": 1.0, + "897": 0.0, + "898": 0.5714285969734192, + "899": 1.0, + "900": 1.0, + "901": 0.0, + "902": 0.7142857313156128, + "903": 1.0, + "904": 1.0, + "905": 0.0, + "906": 0.8571428656578064, + "907": 1.0, + "908": 1.0, + "909": 0.0, + "910": 1.0, + "911": 1.0, + "912": 1.0, + "913": 0.1428571492433548, + "914": 0.5714285969734192, + "915": 1.0, + "916": 1.0, + "917": 0.1428571492433548, + "918": 0.7142857313156128, + "919": 1.0, + "920": 1.0, + "921": 0.1428571492433548, + "922": 0.8571428656578064, + "923": 1.0, + "924": 1.0, + "925": 0.1428571492433548, + "926": 1.0, + "927": 1.0, + "928": 1.0, + "929": 0.2857142984867096, + "930": 0.5714285969734192, + "931": 1.0, + "932": 1.0, + "933": 0.2857142984867096, + "934": 0.7142857313156128, + "935": 1.0, + "936": 1.0, + "937": 0.2857142984867096, + "938": 0.8571428656578064, + "939": 1.0, + "940": 1.0, + "941": 0.2857142984867096, + "942": 1.0, + "943": 1.0, + "944": 1.0, + "945": 0.4285714328289032, + "946": 0.5714285969734192, + "947": 1.0, + "948": 1.0, + "949": 0.4285714328289032, + "950": 0.7142857313156128, + "951": 1.0, + "952": 1.0, + "953": 0.4285714328289032, + "954": 0.8571428656578064, + "955": 1.0, + "956": 1.0, + "957": 0.4285714328289032, + "958": 1.0, + "959": 1.0, + "960": 1.0, + "961": 0.5714285969734192, + "962": 0.5714285969734192, + "963": 1.0, + "964": 1.0, + "965": 0.5714285969734192, + "966": 0.7142857313156128, + "967": 1.0, + "968": 1.0, + "969": 0.5714285969734192, + "970": 0.8571428656578064, + "971": 1.0, + "972": 1.0, + "973": 0.5714285969734192, + "974": 1.0, + "975": 1.0, + "976": 1.0, + "977": 0.7142857313156128, + "978": 0.5714285969734192, + "979": 1.0, + "980": 1.0, + "981": 0.7142857313156128, + "982": 0.7142857313156128, + "983": 1.0, + "984": 1.0, + "985": 0.7142857313156128, + "986": 0.8571428656578064, + "987": 1.0, + "988": 1.0, + "989": 0.7142857313156128, + "990": 1.0, + "991": 1.0, + "992": 1.0, + "993": 0.8571428656578064, + "994": 0.5714285969734192, + "995": 1.0, + "996": 1.0, + "997": 0.8571428656578064, + "998": 0.7142857313156128, + "999": 1.0, + "1000": 1.0, + "1001": 0.8571428656578064, + "1002": 0.8571428656578064, + "1003": 1.0, + "1004": 1.0, + "1005": 0.8571428656578064, + "1006": 1.0, + "1007": 1.0, + "1008": 1.0, + "1009": 1.0, + "1010": 0.5714285969734192, + "1011": 1.0, + "1012": 1.0, + "1013": 1.0, + "1014": 0.7142857313156128, + "1015": 1.0, + "1016": 1.0, + "1017": 0.6000000238418579, + "1018": 0.0, + "1019": 1.0, + "1020": 1.0, + "1021": 0.6000000238418579, + "1022": 0.0, + "1023": 1.0 + }, + "shadeColor": { + "0": 0.0, + "1": 0.0, + "2": 0.5714285969734192, + "3": 1.0, + "4": 0.0, + "5": 0.0, + "6": 0.0, + "7": 0.0, + "8": 0.0, + "9": 0.0, + "10": 0.0, + "11": 0.0, + "12": 1.0, + "13": 1.0, + "14": 1.0, + "15": 0.0, + "16": 1.0, + "17": 1.0, + "18": 1.0, + "19": 0.0, + "20": 0.0, + "21": 0.1428571492433548, + "22": 0.7142857313156128, + "23": 1.0, + "24": 0.0, + "25": 0.1428571492433548, + "26": 0.8571428656578064, + "27": 1.0, + "28": 0.0, + "29": 0.1428571492433548, + "30": 1.0, + "31": 1.0, + "32": 0.0, + "33": 0.2857142984867096, + "34": 0.5714285969734192, + "35": 1.0, + "36": 0.0, + "37": 0.2857142984867096, + "38": 0.7142857313156128, + "39": 1.0, + "40": 0.0, + "41": 0.2857142984867096, + "42": 0.8571428656578064, + "43": 1.0, + "44": 0.0, + "45": 0.2857142984867096, + "46": 1.0, + "47": 1.0, + "48": 0.0, + "49": 0.4285714328289032, + "50": 0.5714285969734192, + "51": 1.0, + "52": 0.0, + "53": 0.4285714328289032, + "54": 0.7142857313156128, + "55": 1.0, + "56": 0.0, + "57": 0.4285714328289032, + "58": 0.8571428656578064, + "59": 1.0, + "60": 0.0, + "61": 0.4285714328289032, + "62": 1.0, + "63": 1.0, + "64": 0.0, + "65": 0.5714285969734192, + "66": 0.5714285969734192, + "67": 1.0, + "68": 0.0, + "69": 0.5714285969734192, + "70": 0.7142857313156128, + "71": 1.0, + "72": 0.0, + "73": 0.5714285969734192, + "74": 0.8571428656578064, + "75": 1.0, + "76": 0.0, + "77": 0.5714285969734192, + "78": 1.0, + "79": 1.0, + "80": 0.0, + "81": 0.7142857313156128, + "82": 0.5714285969734192, + "83": 1.0, + "84": 0.0, + "85": 0.7142857313156128, + "86": 0.7142857313156128, + "87": 1.0, + "88": 0.0, + "89": 0.7142857313156128, + "90": 0.8571428656578064, + "91": 1.0, + "92": 0.0, + "93": 0.7142857313156128, + "94": 1.0, + "95": 1.0, + "96": 0.0, + "97": 0.8571428656578064, + "98": 0.5714285969734192, + "99": 1.0, + "100": 0.0, + "101": 0.8571428656578064, + "102": 0.7142857313156128, + "103": 1.0, + "104": 0.0, + "105": 0.8571428656578064, + "106": 0.8571428656578064, + "107": 1.0, + "108": 0.0, + "109": 0.8571428656578064, + "110": 1.0, + "111": 1.0, + "112": 0.0, + "113": 1.0, + "114": 0.5714285969734192, + "115": 1.0, + "116": 0.0, + "117": 1.0, + "118": 0.7142857313156128, + "119": 1.0, + "120": 0.0, + "121": 1.0, + "122": 0.8571428656578064, + "123": 1.0, + "124": 0.0, + "125": 1.0, + "126": 1.0, + "127": 1.0, + "128": 0.1428571492433548, + "129": 0.0, + "130": 0.5714285969734192, + "131": 1.0, + "132": 0.1428571492433548, + "133": 0.0, + "134": 0.7142857313156128, + "135": 1.0, + "136": 0.1428571492433548, + "137": 0.0, + "138": 0.8571428656578064, + "139": 1.0, + "140": 0.1428571492433548, + "141": 0.0, + "142": 1.0, + "143": 1.0, + "144": 0.1428571492433548, + "145": 0.1428571492433548, + "146": 0.5714285969734192, + "147": 1.0, + "148": 0.1428571492433548, + "149": 0.1428571492433548, + "150": 0.7142857313156128, + "151": 1.0, + "152": 0.1428571492433548, + "153": 0.1428571492433548, + "154": 0.8571428656578064, + "155": 1.0, + "156": 0.1428571492433548, + "157": 0.1428571492433548, + "158": 1.0, + "159": 1.0, + "160": 0.1428571492433548, + "161": 0.2857142984867096, + "162": 0.5714285969734192, + "163": 1.0, + "164": 0.1428571492433548, + "165": 0.2857142984867096, + "166": 0.7142857313156128, + "167": 1.0, + "168": 0.1428571492433548, + "169": 0.2857142984867096, + "170": 0.8571428656578064, + "171": 1.0, + "172": 0.1428571492433548, + "173": 0.2857142984867096, + "174": 1.0, + "175": 1.0, + "176": 0.1428571492433548, + "177": 0.4285714328289032, + "178": 0.5714285969734192, + "179": 1.0, + "180": 0.1428571492433548, + "181": 0.4285714328289032, + "182": 0.7142857313156128, + "183": 1.0, + "184": 0.1428571492433548, + "185": 0.4285714328289032, + "186": 0.8571428656578064, + "187": 1.0, + "188": 0.1428571492433548, + "189": 0.4285714328289032, + "190": 1.0, + "191": 1.0, + "192": 0.1428571492433548, + "193": 0.5714285969734192, + "194": 0.5714285969734192, + "195": 1.0, + "196": 0.1428571492433548, + "197": 0.5714285969734192, + "198": 0.7142857313156128, + "199": 1.0, + "200": 0.1428571492433548, + "201": 0.5714285969734192, + "202": 0.8571428656578064, + "203": 1.0, + "204": 0.1428571492433548, + "205": 0.5714285969734192, + "206": 1.0, + "207": 1.0, + "208": 0.1428571492433548, + "209": 0.7142857313156128, + "210": 0.5714285969734192, + "211": 1.0, + "212": 0.1428571492433548, + "213": 0.7142857313156128, + "214": 0.7142857313156128, + "215": 1.0, + "216": 0.1428571492433548, + "217": 0.7142857313156128, + "218": 0.8571428656578064, + "219": 1.0, + "220": 0.1428571492433548, + "221": 0.7142857313156128, + "222": 1.0, + "223": 1.0, + "224": 0.1428571492433548, + "225": 0.8571428656578064, + "226": 0.5714285969734192, + "227": 1.0, + "228": 0.1428571492433548, + "229": 0.8571428656578064, + "230": 0.7142857313156128, + "231": 1.0, + "232": 0.1428571492433548, + "233": 0.8571428656578064, + "234": 0.8571428656578064, + "235": 1.0, + "236": 0.1428571492433548, + "237": 0.8571428656578064, + "238": 1.0, + "239": 1.0, + "240": 0.1428571492433548, + "241": 1.0, + "242": 0.5714285969734192, + "243": 1.0, + "244": 0.1428571492433548, + "245": 1.0, + "246": 0.7142857313156128, + "247": 1.0, + "248": 0.1428571492433548, + "249": 1.0, + "250": 0.8571428656578064, + "251": 1.0, + "252": 0.1428571492433548, + "253": 1.0, + "254": 1.0, + "255": 1.0, + "256": 0.2857142984867096, + "257": 0.0, + "258": 0.5714285969734192, + "259": 1.0, + "260": 0.2857142984867096, + "261": 0.0, + "262": 0.7142857313156128, + "263": 1.0, + "264": 0.2857142984867096, + "265": 0.0, + "266": 0.8571428656578064, + "267": 1.0, + "268": 0.2857142984867096, + "269": 0.0, + "270": 1.0, + "271": 1.0, + "272": 0.2857142984867096, + "273": 0.1428571492433548, + "274": 0.5714285969734192, + "275": 1.0, + "276": 0.2857142984867096, + "277": 0.1428571492433548, + "278": 0.7142857313156128, + "279": 1.0, + "280": 0.2857142984867096, + "281": 0.1428571492433548, + "282": 0.8571428656578064, + "283": 1.0, + "284": 0.2857142984867096, + "285": 0.1428571492433548, + "286": 1.0, + "287": 1.0, + "288": 0.2857142984867096, + "289": 0.2857142984867096, + "290": 0.5714285969734192, + "291": 1.0, + "292": 0.2857142984867096, + "293": 0.2857142984867096, + "294": 0.7142857313156128, + "295": 1.0, + "296": 0.2857142984867096, + "297": 0.2857142984867096, + "298": 0.8571428656578064, + "299": 1.0, + "300": 0.2857142984867096, + "301": 0.2857142984867096, + "302": 1.0, + "303": 1.0, + "304": 0.2857142984867096, + "305": 0.4285714328289032, + "306": 0.5714285969734192, + "307": 1.0, + "308": 0.2857142984867096, + "309": 0.4285714328289032, + "310": 0.7142857313156128, + "311": 1.0, + "312": 0.2857142984867096, + "313": 0.4285714328289032, + "314": 0.8571428656578064, + "315": 1.0, + "316": 0.2857142984867096, + "317": 0.4285714328289032, + "318": 1.0, + "319": 1.0, + "320": 0.2857142984867096, + "321": 0.5714285969734192, + "322": 0.5714285969734192, + "323": 1.0, + "324": 0.2857142984867096, + "325": 0.5714285969734192, + "326": 0.7142857313156128, + "327": 1.0, + "328": 0.2857142984867096, + "329": 0.5714285969734192, + "330": 0.8571428656578064, + "331": 1.0, + "332": 0.2857142984867096, + "333": 0.5714285969734192, + "334": 1.0, + "335": 1.0, + "336": 0.2857142984867096, + "337": 0.7142857313156128, + "338": 0.5714285969734192, + "339": 1.0, + "340": 0.2857142984867096, + "341": 0.7142857313156128, + "342": 0.7142857313156128, + "343": 1.0, + "344": 0.2857142984867096, + "345": 0.7142857313156128, + "346": 0.8571428656578064, + "347": 1.0, + "348": 0.2857142984867096, + "349": 0.7142857313156128, + "350": 1.0, + "351": 1.0, + "352": 0.2857142984867096, + "353": 0.8571428656578064, + "354": 0.5714285969734192, + "355": 1.0, + "356": 0.2857142984867096, + "357": 0.8571428656578064, + "358": 0.7142857313156128, + "359": 1.0, + "360": 0.2857142984867096, + "361": 0.8571428656578064, + "362": 0.8571428656578064, + "363": 1.0, + "364": 0.2857142984867096, + "365": 0.8571428656578064, + "366": 1.0, + "367": 1.0, + "368": 0.2857142984867096, + "369": 1.0, + "370": 0.5714285969734192, + "371": 1.0, + "372": 0.2857142984867096, + "373": 1.0, + "374": 0.7142857313156128, + "375": 1.0, + "376": 0.2857142984867096, + "377": 1.0, + "378": 0.8571428656578064, + "379": 1.0, + "380": 0.2857142984867096, + "381": 1.0, + "382": 1.0, + "383": 1.0, + "384": 0.4285714328289032, + "385": 0.0, + "386": 0.5714285969734192, + "387": 1.0, + "388": 0.4285714328289032, + "389": 0.0, + "390": 0.7142857313156128, + "391": 1.0, + "392": 0.4285714328289032, + "393": 0.0, + "394": 0.8571428656578064, + "395": 1.0, + "396": 0.4285714328289032, + "397": 0.0, + "398": 1.0, + "399": 1.0, + "400": 0.4285714328289032, + "401": 0.1428571492433548, + "402": 0.5714285969734192, + "403": 1.0, + "404": 0.4285714328289032, + "405": 0.1428571492433548, + "406": 0.7142857313156128, + "407": 1.0, + "408": 0.4285714328289032, + "409": 0.1428571492433548, + "410": 0.8571428656578064, + "411": 1.0, + "412": 0.4285714328289032, + "413": 0.1428571492433548, + "414": 1.0, + "415": 1.0, + "416": 0.4285714328289032, + "417": 0.2857142984867096, + "418": 0.5714285969734192, + "419": 1.0, + "420": 0.4285714328289032, + "421": 0.2857142984867096, + "422": 0.7142857313156128, + "423": 1.0, + "424": 0.4285714328289032, + "425": 0.2857142984867096, + "426": 0.8571428656578064, + "427": 1.0, + "428": 0.4285714328289032, + "429": 0.2857142984867096, + "430": 1.0, + "431": 1.0, + "432": 0.4285714328289032, + "433": 0.4285714328289032, + "434": 0.5714285969734192, + "435": 1.0, + "436": 0.4285714328289032, + "437": 0.4285714328289032, + "438": 0.7142857313156128, + "439": 1.0, + "440": 0.4285714328289032, + "441": 0.4285714328289032, + "442": 0.8571428656578064, + "443": 1.0, + "444": 0.4285714328289032, + "445": 0.4285714328289032, + "446": 1.0, + "447": 1.0, + "448": 0.4285714328289032, + "449": 0.5714285969734192, + "450": 0.5714285969734192, + "451": 1.0, + "452": 0.4285714328289032, + "453": 0.5714285969734192, + "454": 0.7142857313156128, + "455": 1.0, + "456": 0.4285714328289032, + "457": 0.5714285969734192, + "458": 0.8571428656578064, + "459": 1.0, + "460": 0.4285714328289032, + "461": 0.5714285969734192, + "462": 1.0, + "463": 1.0, + "464": 0.4285714328289032, + "465": 0.7142857313156128, + "466": 0.5714285969734192, + "467": 1.0, + "468": 0.4285714328289032, + "469": 0.7142857313156128, + "470": 0.7142857313156128, + "471": 1.0, + "472": 0.4285714328289032, + "473": 0.7142857313156128, + "474": 0.8571428656578064, + "475": 1.0, + "476": 0.4285714328289032, + "477": 0.7142857313156128, + "478": 1.0, + "479": 1.0, + "480": 0.4285714328289032, + "481": 0.8571428656578064, + "482": 0.5714285969734192, + "483": 1.0, + "484": 0.4285714328289032, + "485": 0.8571428656578064, + "486": 0.7142857313156128, + "487": 1.0, + "488": 0.4285714328289032, + "489": 0.8571428656578064, + "490": 0.8571428656578064, + "491": 1.0, + "492": 0.4285714328289032, + "493": 0.8571428656578064, + "494": 1.0, + "495": 1.0, + "496": 0.4285714328289032, + "497": 1.0, + "498": 0.5714285969734192, + "499": 1.0, + "500": 0.4285714328289032, + "501": 1.0, + "502": 0.7142857313156128, + "503": 1.0, + "504": 0.4285714328289032, + "505": 1.0, + "506": 0.8571428656578064, + "507": 1.0, + "508": 0.4285714328289032, + "509": 1.0, + "510": 1.0, + "511": 1.0, + "512": 0.5714285969734192, + "513": 0.0, + "514": 0.5714285969734192, + "515": 1.0, + "516": 0.5714285969734192, + "517": 0.0, + "518": 0.7142857313156128, + "519": 1.0, + "520": 0.5714285969734192, + "521": 0.0, + "522": 0.8571428656578064, + "523": 1.0, + "524": 0.5714285969734192, + "525": 0.0, + "526": 1.0, + "527": 1.0, + "528": 0.5714285969734192, + "529": 0.1428571492433548, + "530": 0.5714285969734192, + "531": 1.0, + "532": 0.5714285969734192, + "533": 0.1428571492433548, + "534": 0.7142857313156128, + "535": 1.0, + "536": 0.5714285969734192, + "537": 0.1428571492433548, + "538": 0.8571428656578064, + "539": 1.0, + "540": 0.5714285969734192, + "541": 0.1428571492433548, + "542": 1.0, + "543": 1.0, + "544": 0.5714285969734192, + "545": 0.2857142984867096, + "546": 0.5714285969734192, + "547": 1.0, + "548": 0.5714285969734192, + "549": 0.2857142984867096, + "550": 0.7142857313156128, + "551": 1.0, + "552": 0.5714285969734192, + "553": 0.2857142984867096, + "554": 0.8571428656578064, + "555": 1.0, + "556": 0.5714285969734192, + "557": 0.2857142984867096, + "558": 1.0, + "559": 1.0, + "560": 0.5714285969734192, + "561": 0.4285714328289032, + "562": 0.5714285969734192, + "563": 1.0, + "564": 0.5714285969734192, + "565": 0.4285714328289032, + "566": 0.7142857313156128, + "567": 1.0, + "568": 0.5714285969734192, + "569": 0.4285714328289032, + "570": 0.8571428656578064, + "571": 1.0, + "572": 0.5714285969734192, + "573": 0.4285714328289032, + "574": 1.0, + "575": 1.0, + "576": 0.5714285969734192, + "577": 0.5714285969734192, + "578": 0.5714285969734192, + "579": 1.0, + "580": 0.5714285969734192, + "581": 0.5714285969734192, + "582": 0.7142857313156128, + "583": 1.0, + "584": 0.5714285969734192, + "585": 0.5714285969734192, + "586": 0.8571428656578064, + "587": 1.0, + "588": 0.5714285969734192, + "589": 0.5714285969734192, + "590": 1.0, + "591": 1.0, + "592": 0.5714285969734192, + "593": 0.7142857313156128, + "594": 0.5714285969734192, + "595": 1.0, + "596": 0.5714285969734192, + "597": 0.7142857313156128, + "598": 0.7142857313156128, + "599": 1.0, + "600": 0.5714285969734192, + "601": 0.7142857313156128, + "602": 0.8571428656578064, + "603": 1.0, + "604": 0.5714285969734192, + "605": 0.7142857313156128, + "606": 1.0, + "607": 1.0, + "608": 0.5714285969734192, + "609": 0.8571428656578064, + "610": 0.5714285969734192, + "611": 1.0, + "612": 0.5714285969734192, + "613": 0.8571428656578064, + "614": 0.7142857313156128, + "615": 1.0, + "616": 0.5714285969734192, + "617": 0.8571428656578064, + "618": 0.8571428656578064, + "619": 1.0, + "620": 0.5714285969734192, + "621": 0.8571428656578064, + "622": 1.0, + "623": 1.0, + "624": 0.5714285969734192, + "625": 1.0, + "626": 0.5714285969734192, + "627": 1.0, + "628": 0.5714285969734192, + "629": 1.0, + "630": 0.7142857313156128, + "631": 1.0, + "632": 0.5714285969734192, + "633": 1.0, + "634": 0.8571428656578064, + "635": 1.0, + "636": 0.5714285969734192, + "637": 1.0, + "638": 1.0, + "639": 1.0, + "640": 0.7142857313156128, + "641": 0.0, + "642": 0.5714285969734192, + "643": 1.0, + "644": 0.7142857313156128, + "645": 0.0, + "646": 0.7142857313156128, + "647": 1.0, + "648": 0.7142857313156128, + "649": 0.0, + "650": 0.8571428656578064, + "651": 1.0, + "652": 0.7142857313156128, + "653": 0.0, + "654": 1.0, + "655": 1.0, + "656": 0.7142857313156128, + "657": 0.1428571492433548, + "658": 0.5714285969734192, + "659": 1.0, + "660": 0.7142857313156128, + "661": 0.1428571492433548, + "662": 0.7142857313156128, + "663": 1.0, + "664": 0.7142857313156128, + "665": 0.1428571492433548, + "666": 0.8571428656578064, + "667": 1.0, + "668": 0.7142857313156128, + "669": 0.1428571492433548, + "670": 1.0, + "671": 1.0, + "672": 0.7142857313156128, + "673": 0.2857142984867096, + "674": 0.5714285969734192, + "675": 1.0, + "676": 0.7142857313156128, + "677": 0.2857142984867096, + "678": 0.7142857313156128, + "679": 1.0, + "680": 0.7142857313156128, + "681": 0.2857142984867096, + "682": 0.8571428656578064, + "683": 1.0, + "684": 0.7142857313156128, + "685": 0.2857142984867096, + "686": 1.0, + "687": 1.0, + "688": 0.7142857313156128, + "689": 0.4285714328289032, + "690": 0.5714285969734192, + "691": 1.0, + "692": 0.7142857313156128, + "693": 0.4285714328289032, + "694": 0.7142857313156128, + "695": 1.0, + "696": 0.7142857313156128, + "697": 0.4285714328289032, + "698": 0.8571428656578064, + "699": 1.0, + "700": 0.7142857313156128, + "701": 0.4285714328289032, + "702": 1.0, + "703": 1.0, + "704": 0.7142857313156128, + "705": 0.5714285969734192, + "706": 0.5714285969734192, + "707": 1.0, + "708": 0.7142857313156128, + "709": 0.5714285969734192, + "710": 0.7142857313156128, + "711": 1.0, + "712": 0.7142857313156128, + "713": 0.5714285969734192, + "714": 0.8571428656578064, + "715": 1.0, + "716": 0.7142857313156128, + "717": 0.5714285969734192, + "718": 1.0, + "719": 1.0, + "720": 0.7142857313156128, + "721": 0.7142857313156128, + "722": 0.5714285969734192, + "723": 1.0, + "724": 0.7142857313156128, + "725": 0.7142857313156128, + "726": 0.7142857313156128, + "727": 1.0, + "728": 0.7142857313156128, + "729": 0.7142857313156128, + "730": 0.8571428656578064, + "731": 1.0, + "732": 0.7142857313156128, + "733": 0.7142857313156128, + "734": 1.0, + "735": 1.0, + "736": 0.7142857313156128, + "737": 0.8571428656578064, + "738": 0.5714285969734192, + "739": 1.0, + "740": 0.7142857313156128, + "741": 0.8571428656578064, + "742": 0.7142857313156128, + "743": 1.0, + "744": 0.7142857313156128, + "745": 0.8571428656578064, + "746": 0.8571428656578064, + "747": 1.0, + "748": 0.7142857313156128, + "749": 0.8571428656578064, + "750": 1.0, + "751": 1.0, + "752": 0.7142857313156128, + "753": 1.0, + "754": 0.5714285969734192, + "755": 1.0, + "756": 0.7142857313156128, + "757": 1.0, + "758": 0.7142857313156128, + "759": 1.0, + "760": 0.7142857313156128, + "761": 1.0, + "762": 0.8571428656578064, + "763": 1.0, + "764": 0.7142857313156128, + "765": 1.0, + "766": 1.0, + "767": 1.0, + "768": 0.8571428656578064, + "769": 0.0, + "770": 0.5714285969734192, + "771": 1.0, + "772": 0.8571428656578064, + "773": 0.0, + "774": 0.7142857313156128, + "775": 1.0, + "776": 0.8571428656578064, + "777": 0.0, + "778": 0.8571428656578064, + "779": 1.0, + "780": 0.8571428656578064, + "781": 0.0, + "782": 1.0, + "783": 1.0, + "784": 0.8571428656578064, + "785": 0.1428571492433548, + "786": 0.5714285969734192, + "787": 1.0, + "788": 0.8571428656578064, + "789": 0.1428571492433548, + "790": 0.7142857313156128, + "791": 1.0, + "792": 0.8571428656578064, + "793": 0.1428571492433548, + "794": 0.8571428656578064, + "795": 1.0, + "796": 0.8571428656578064, + "797": 0.1428571492433548, + "798": 1.0, + "799": 1.0, + "800": 0.8571428656578064, + "801": 0.2857142984867096, + "802": 0.5714285969734192, + "803": 1.0, + "804": 0.8571428656578064, + "805": 0.2857142984867096, + "806": 0.7142857313156128, + "807": 1.0, + "808": 0.8571428656578064, + "809": 0.2857142984867096, + "810": 0.8571428656578064, + "811": 1.0, + "812": 0.8571428656578064, + "813": 0.2857142984867096, + "814": 1.0, + "815": 1.0, + "816": 0.8571428656578064, + "817": 0.4285714328289032, + "818": 0.5714285969734192, + "819": 1.0, + "820": 0.8571428656578064, + "821": 0.4285714328289032, + "822": 0.7142857313156128, + "823": 1.0, + "824": 0.8571428656578064, + "825": 0.4285714328289032, + "826": 0.8571428656578064, + "827": 1.0, + "828": 0.8571428656578064, + "829": 0.4285714328289032, + "830": 1.0, + "831": 1.0, + "832": 0.8571428656578064, + "833": 0.5714285969734192, + "834": 0.5714285969734192, + "835": 1.0, + "836": 0.8571428656578064, + "837": 0.5714285969734192, + "838": 0.7142857313156128, + "839": 1.0, + "840": 0.8571428656578064, + "841": 0.5714285969734192, + "842": 0.8571428656578064, + "843": 1.0, + "844": 0.8571428656578064, + "845": 0.5714285969734192, + "846": 1.0, + "847": 1.0, + "848": 0.8571428656578064, + "849": 0.7142857313156128, + "850": 0.5714285969734192, + "851": 1.0, + "852": 0.8571428656578064, + "853": 0.7142857313156128, + "854": 0.7142857313156128, + "855": 1.0, + "856": 0.8571428656578064, + "857": 0.7142857313156128, + "858": 0.8571428656578064, + "859": 1.0, + "860": 0.8571428656578064, + "861": 0.7142857313156128, + "862": 1.0, + "863": 1.0, + "864": 0.8571428656578064, + "865": 0.8571428656578064, + "866": 0.5714285969734192, + "867": 1.0, + "868": 0.8571428656578064, + "869": 0.8571428656578064, + "870": 0.7142857313156128, + "871": 1.0, + "872": 0.8571428656578064, + "873": 0.8571428656578064, + "874": 0.8571428656578064, + "875": 1.0, + "876": 0.8571428656578064, + "877": 0.8571428656578064, + "878": 1.0, + "879": 1.0, + "880": 0.8571428656578064, + "881": 1.0, + "882": 0.5714285969734192, + "883": 1.0, + "884": 0.8571428656578064, + "885": 1.0, + "886": 0.7142857313156128, + "887": 1.0, + "888": 0.8571428656578064, + "889": 1.0, + "890": 0.8571428656578064, + "891": 1.0, + "892": 0.8571428656578064, + "893": 1.0, + "894": 1.0, + "895": 1.0, + "896": 1.0, + "897": 0.0, + "898": 0.5714285969734192, + "899": 1.0, + "900": 1.0, + "901": 0.0, + "902": 0.7142857313156128, + "903": 1.0, + "904": 1.0, + "905": 0.0, + "906": 0.8571428656578064, + "907": 1.0, + "908": 1.0, + "909": 0.0, + "910": 1.0, + "911": 1.0, + "912": 1.0, + "913": 0.1428571492433548, + "914": 0.5714285969734192, + "915": 1.0, + "916": 1.0, + "917": 0.1428571492433548, + "918": 0.7142857313156128, + "919": 1.0, + "920": 1.0, + "921": 0.1428571492433548, + "922": 0.8571428656578064, + "923": 1.0, + "924": 1.0, + "925": 0.1428571492433548, + "926": 1.0, + "927": 1.0, + "928": 1.0, + "929": 0.2857142984867096, + "930": 0.5714285969734192, + "931": 1.0, + "932": 1.0, + "933": 0.2857142984867096, + "934": 0.7142857313156128, + "935": 1.0, + "936": 1.0, + "937": 0.2857142984867096, + "938": 0.8571428656578064, + "939": 1.0, + "940": 1.0, + "941": 0.2857142984867096, + "942": 1.0, + "943": 1.0, + "944": 1.0, + "945": 0.4285714328289032, + "946": 0.5714285969734192, + "947": 1.0, + "948": 1.0, + "949": 0.4285714328289032, + "950": 0.7142857313156128, + "951": 1.0, + "952": 1.0, + "953": 0.4285714328289032, + "954": 0.8571428656578064, + "955": 1.0, + "956": 1.0, + "957": 0.4285714328289032, + "958": 1.0, + "959": 1.0, + "960": 1.0, + "961": 0.5714285969734192, + "962": 0.5714285969734192, + "963": 1.0, + "964": 1.0, + "965": 0.5714285969734192, + "966": 0.7142857313156128, + "967": 1.0, + "968": 1.0, + "969": 0.5714285969734192, + "970": 0.8571428656578064, + "971": 1.0, + "972": 1.0, + "973": 0.5714285969734192, + "974": 1.0, + "975": 1.0, + "976": 1.0, + "977": 0.7142857313156128, + "978": 0.5714285969734192, + "979": 1.0, + "980": 1.0, + "981": 0.7142857313156128, + "982": 0.7142857313156128, + "983": 1.0, + "984": 1.0, + "985": 0.7142857313156128, + "986": 0.8571428656578064, + "987": 1.0, + "988": 1.0, + "989": 0.7142857313156128, + "990": 1.0, + "991": 1.0, + "992": 1.0, + "993": 0.8571428656578064, + "994": 0.5714285969734192, + "995": 1.0, + "996": 1.0, + "997": 0.8571428656578064, + "998": 0.7142857313156128, + "999": 1.0, + "1000": 1.0, + "1001": 0.8571428656578064, + "1002": 0.8571428656578064, + "1003": 1.0, + "1004": 1.0, + "1005": 0.8571428656578064, + "1006": 1.0, + "1007": 1.0, + "1008": 1.0, + "1009": 1.0, + "1010": 0.5714285969734192, + "1011": 1.0, + "1012": 1.0, + "1013": 1.0, + "1014": 0.7142857313156128, + "1015": 1.0, + "1016": 0.0, + "1017": 0.0, + "1018": 0.0, + "1019": 0.0, + "1020": 1.0, + "1021": 0.6000000238418579, + "1022": 0.0, + "1023": 0.0 + } + }, + "memory": { + "lowFraction": 0.2 + }, + "stepMove": { + "x": 50.0, + "y": 50.0, + "z": 50.0 + }, + "stepRotate": 1.0, + "stepScale": 1.0, + "camInertiaEnabled": false, + "camInertiaAmount": 0.55, + "manipulator": { + "camera": { + "flyAcceleration": 1000.0, + "flyDampening": 10.0, + "lookAcceleration": 2000.0, + "lookDampening": 20.0 + } + }, + "camVelocityMin": 0.01, + "camVelocityMax": 50, + "camVelocityScalerMin": 1, + "camVelocityScalerMax": 10, + "camVelocityScalerMultAmount": 1.1, + "camShowSpeedOnStart": false, + "camVelocityCOINormalization": 0.0, + "camStopOnMouseUp": true, + "camRotSmoothEnabled": true, + "camRotSmoothScale": 20.0, + "camRotSmoothAlways": false, + "camGestureEnabled": false, + "camGestureTime": 0.12, + "camGestureRadius": 20, + "ui": { + "background": { + "opacity": 1.0 + }, + "brightness": 0.84 + }, + "objectCentricNavigation": 0, + "coiDoubleClick": false, + "grid": { + "scale": 1.0, + "lineWidth": 1, + "lineFadeOutStartDistance": 10.0, + "lineColor": { + "0": 0.3, + "1": 0.3, + "2": 0.3 + }, + "lineFadeOutEndDistance": 20.0, + "offset": 0.009999999776482582 + }, + "boundingBoxes": { + "lineColor": { + "0": 0.886, + "1": 0.447, + "2": 0.447 + }, + "hideWhenManipulating": false + }, + "gizmo": { + "scale": 0.01, + "lineWidth": 1.0, + "constantScaleEnabled": true, + "constantScale": 10.0, + "minFadeOut": 1.0, + "maxFadeOut": 50, + "constantScaleCamera": false + }, + "Viewport 2": { + "Viewport0": { + "guide": { + "axis": { + "visible": false + }, + "grid": { + "visible": false + }, + "selection": { + "visible": false + } + }, + "scene": { + "cameras": { + "visible": false + }, + "lights": { + "visible": false + }, + "skeletons": { + "visible": false + }, + "meshes": { + "visible": true + }, + "audio": { + "visible": true + } + }, + "hud": { + "renderFPS": { + "visible": false + }, + "renderResolution": { + "visible": false + }, + "renderProgress": { + "visible": false + }, + "deviceMemory": { + "visible": false + }, + "hostMemory": { + "visible": false + }, + "processMemory": { + "visible": false + }, + "visible": true, + "cameraSpeed": { + "visible": true + }, + "toastMessage": { + "visible": true + } + }, + "resolution": { + "0": 1280, + "1": 720 + } + } + }, + "Viewport": { + "Viewport0": { + "guide": { + "axis": { + "visible": false + }, + "grid": { + "visible": false + }, + "selection": { + "visible": false + } + }, + "scene": { + "cameras": { + "visible": false + }, + "lights": { + "visible": false + }, + "skeletons": { + "visible": false + }, + "meshes": { + "visible": true + }, + "audio": { + "visible": false + } + }, + "hud": { + "renderFPS": { + "visible": false + }, + "renderResolution": { + "visible": false + }, + "renderProgress": { + "visible": false + }, + "deviceMemory": { + "visible": false + }, + "hostMemory": { + "visible": false + }, + "processMemory": { + "visible": false + }, + "visible": true, + "cameraSpeed": { + "visible": true + }, + "toastMessage": { + "visible": true + } + }, + "resolution": { + "0": 1280, + "1": 720 + } + } + } + } + } + } +} \ No newline at end of file diff --git a/docs/development/docker_usage.md b/docs/development/docker_usage.md index f8445c2c..59d38c44 100644 --- a/docs/development/docker_usage.md +++ b/docs/development/docker_usage.md @@ -46,6 +46,7 @@ docker compose build ## Start, Stop, and Remove Start + ```bash docker compose up -d --scale robot=[NUM_ROBOTS] @@ -54,50 +55,17 @@ docker ps -a ``` Stop + ```bash docker compose stop ``` Remove -```bash -docker compose down -``` - -## SSH into Robots - -The containers mimic the robots' onboard computers on the same network. Therefore we intend to interface with the robots through ssh. - -The `ground-control-station` and `docker-robot-*` containers are setup with ssh daemon, so you can ssh into the containers using the IP address. - -You can get the IP address of each container by running the following command: - -```bash -docker inspect -f '{{range.NetworkSettings.Networks}}{{.IPAddress}}{{end}}' [CONTAINER-NAME] -``` - -Then ssh in, for example: ```bash -ssh root@172.18.0.6 +docker compose down ``` -The ssh password is `airstack`. - -## Container Details - -```mermaid -graph TD - A(Isaac Sim) <-- Sensors and Actuation --> B - A <-- Sensors and Actuation --> C - B(Robot 1) <-- Global Info --> D(Ground Control Station) - C(Robot 2) <-- Global Info --> D - - style A fill:#76B900,stroke:#333,stroke-width:2px - style B fill:#fbb,stroke:#333,stroke-width:2px - style C fill:#fbb,stroke:#333,stroke-width:2px - style D fill:#fbf,stroke:#333,stroke-width:2px - -``` ### Isaac Sim @@ -105,12 +73,12 @@ Start a bash shell in the Isaac Sim container: ```bash # if the isaac container is already running, execute a bash shell in it -docker compose exec isaac-sim bash +docker exec -it isaac-sim bash # or if not, start a new container docker compose run isaac-sim bash ``` -The alias `runapp` launches Isaac Sim. +Within the isaac-sim Docker container, the alias `runapp` launches Isaac Sim. The `--path` argument can be passed with a path to a `.usd` file to load a scene. It can also be run in headless mode with `./runheadless.native.sh` to stream to [Omniverse Streaming Client](https://docs.omniverse.nvidia.com/streaming-client/latest/user-manual.html) or `./runheadless.webrtc.sh` to [stream to a web browser](https://docs.omniverse.nvidia.com/extensions/latest/ext_livestream/webrtc.html). @@ -122,9 +90,13 @@ The container also has the isaacsim ROS2 package within that can be launched wit Start a bash shell in a robot container, e.g. for robot_1: ```bash -docker compose exec docker-robot-1 bash +docker exec -it docker-robot-1 bash ``` +The previous `docker compose up` launches robot_bringup in a tmux session. To attach to the session within the docker container, e.g. to inspect output, run `tmux attach`. + +The following commands are available within the robot container: + ```bash # in robot docker cws # cleans workspace @@ -134,7 +106,7 @@ sws # sources workspace ros2 launch robot_bringup robot.launch.xml # top-level launch ``` -These aliases are in the `~/.bashrc` file. +These aliases are in `AirStack/docker/robot/.bashrc`. Each robot has `ROS_DOMAIN_ID` set to its ID number. `ROBOT_NAME` is set to `robot_$ROS_DOMAIN_ID`. @@ -145,9 +117,45 @@ Currently the ground control station uses the same image as the robot container. Start a bash shell in a robot container: ```bash -docker compose exec ground-control-station bash +docker exec -it ground-control-station bash ``` -The commands are currently the same. +The available aliases within the container are currently the same. On the GCS `ROS_DOMAIN_ID` is set to 0. + +## SSH into Robots + +The containers mimic the robots' onboard computers on the same network. Therefore we intend to interface with the robots through ssh. + +The `ground-control-station` and `docker-robot-*` containers are setup with ssh daemon, so you can ssh into the containers using the IP address. + +You can get the IP address of each container by running the following command: + +```bash +docker inspect -f '{{range.NetworkSettings.Networks}}{{.IPAddress}}{{end}}' [CONTAINER-NAME] +``` + +Then ssh in, for example: + +```bash +ssh root@172.18.0.6 +``` + +The ssh password is `airstack`. + +## Container Details + +```mermaid +graph TD + A(Isaac Sim) <-- Sensors and Actuation --> B + A <-- Sensors and Actuation --> C + B(Robot 1) <-- Global Info --> D(Ground Control Station) + C(Robot 2) <-- Global Info --> D + + style A fill:#76B900,stroke:#333,stroke-width:2px + style B fill:#fbb,stroke:#333,stroke-width:2px + style C fill:#fbb,stroke:#333,stroke-width:2px + style D fill:#fbf,stroke:#333,stroke-width:2px + +``` diff --git a/docs/getting_started.md b/docs/getting_started.md index 093f8afc..7a42b822 100644 --- a/docs/getting_started.md +++ b/docs/getting_started.md @@ -33,8 +33,8 @@ Also set the default OMNI_SERVER and accept the license terms. (Basti: The omni_ Now you have two options on how to proceed. You can build the docker image from scratch or pull the existing image on the airlab docker registry. Building the image from scratch can be useful if you would like to add new dependencies or add new custom functionality. For most users just pulling the existing image will be more conveninent and fast since it doesn't require access to the Nvidia registry. -
Option 1: Use the Airlab Docker registry (Preferred) -To use the AirLab docker registry do the following +
Option 1: Pull From the Airlab Registry (Preferred) +To use the AirLab Docker registry do the following ```bash cd AirStack/docker/ @@ -46,16 +46,13 @@ docker login airlab-storage.andrew.cmu.edu:5001 docker compose pull ``` -When you execute docker compose pull in the next step the image will be pulled from the server automatically. This might take a while since the image is large. +The images will be pulled from the server automatically. This might take a while since the images are large.
-
Option 2: Build Docker Images From Scratch -1. - - Download the Ascent Spirit SITL software packages from this link. +1. Download the Ascent Spirit SITL software packages from this link. Then unzip the file AscentAeroSystemsSITLPackage.zip in this folder: @@ -73,7 +70,7 @@ When you execute docker compose pull in the next step the image will be pulled f docker compose build # build the images locally ``` -IF you have permission you can now push an updated images to the docker server (only if it's changed and is required) +If you have permission you can push updated images to the docker server. ```bash docker compose push @@ -106,7 +103,7 @@ Note you can also use the `ros2 topic pub` command to move the robot. For exampl ```bash # start another terminal in docker container -docker compose exec docker-robot-1 bash +docker exec -it docker-robot-1 bash # in docker # FLY TO POSITION. Put whatever position you want ros2 topic pub /robot_1/interface/mavros/setpoint_position/local geometry_msgs/PoseStamped \ diff --git a/docs/robot/autonomy/4_global/planning.md b/docs/robot/autonomy/4_global/planning.md index edb5fc1e..07ab015d 100644 --- a/docs/robot/autonomy/4_global/planning.md +++ b/docs/robot/autonomy/4_global/planning.md @@ -13,6 +13,8 @@ If a waypoint's header timestamp is empty, the local planner should assume there The global planner should make a trajectory that is collision-free according to the global map. However, avoiding fine obstacles is delegated to the local planner that operates at a faster rate. +For the structure of the package, the global planner node should not include any logic to generate the path. This should be located in a seperate logic class and be seperated from ROS. This will allow more modularity in the future for testing and easy interface changes. + We intend the global planners to be modular. _AirStack_ implements a basic Random Walk planner as a baseline. Feel free to implement your own through the following interfaces. @@ -30,12 +32,13 @@ The best global plan should then be forwarded or remapped to `/$(env ROBOT_NAME) ``` mermaid sequenceDiagram autonumber - Some Node->>Global Planner: ~/plan_request (your_planner/PlanRequest.msg) + Global Manager->>Global Planner: ~/plan_request (your_planner/PlanRequest.msg) loop Planning - Global Planner-->>Some Node: heartbeat feedback + Global Planner-->>Global Manager: heartbeat feedback end - Global Planner->>Some Node: ~/global_plan (nav_msgs/Path.msg) - Some Node->>Local Planner: /$ROBOT_NAME/global_plan (nav_msgs/Path.msg) + Global Planner->>Global Manager: ~/global_plan (nav_msgs/Path.msg) + Global Manager->>Local Planner: /$ROBOT_NAME/global_plan_reference (nav_msgs/Path.msg) + Local Planner->>Global Manager: /$ROBOT_NAME/global_plan_eta (nav_msgs/Path.msg) ``` ### Subscribe: Plan Request @@ -88,4 +91,5 @@ The global planner can do whatever it wants internally with this information. ### Random Walk planner -The random walk planner replans when the robot is getting close to the goal. The random walk planner is a trivial planner that generates a plan by randomly selecting a direction to move in. The random walk planner is useful for testing the robot's ability to follow a plan. \ No newline at end of file +The random walk planner replans when the robot is getting close to the goal. The random walk planner is a trivial planner that generates a plan by randomly selecting a direction to move in. The random walk planner is useful for testing the robot's ability to follow a plan. + diff --git a/mkdocs.yml b/mkdocs.yml index 6489a717..6462752a 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -28,6 +28,8 @@ markdown_extensions: - name: mermaid class: mermaid format: !!python/name:pymdownx.superfences.fence_code_format + - toc: + permalink: true nav: - Home: README.md - Getting Started: getting_started.md diff --git a/ros_ws/src/airstack_common/CMakeLists.txt b/ros_ws/src/airstack_common/CMakeLists.txt index 6dab3e9b..0b0d9d60 100644 --- a/ros_ws/src/airstack_common/CMakeLists.txt +++ b/ros_ws/src/airstack_common/CMakeLists.txt @@ -35,7 +35,7 @@ install( ament_export_libraries(ros2_helper) # tflib -add_library(tflib src/tflib.cpp) +add_library(tflib SHARED src/tflib.cpp) target_include_directories(tflib PUBLIC $ $) diff --git a/ros_ws/src/airstack_msgs/srv/PlanToWaypoint.srv b/ros_ws/src/airstack_msgs/srv/PlanToWaypoint.srv new file mode 100644 index 00000000..81e94f01 --- /dev/null +++ b/ros_ws/src/airstack_msgs/srv/PlanToWaypoint.srv @@ -0,0 +1,5 @@ +geometry_msgs/Pose goal_pose + +uint8 command +--- +bool success \ No newline at end of file diff --git a/ros_ws/src/airstack_msgs/srv/TrajectoryMode.srv b/ros_ws/src/airstack_msgs/srv/TrajectoryMode.srv index c67d38fb..9b354e2c 100644 --- a/ros_ws/src/airstack_msgs/srv/TrajectoryMode.srv +++ b/ros_ws/src/airstack_msgs/srv/TrajectoryMode.srv @@ -1,7 +1,7 @@ int32 PAUSE=0 int32 ROBOT_POSE=1 int32 TRACK=2 -int32 SEGMENT=3 +int32 ADD_SEGMENT=3 int32 REWIND=4 int32 mode diff --git a/ros_ws/src/robot/autonomy/0_interface/interface_bringup/launch/interface.launch.xml b/ros_ws/src/robot/autonomy/0_interface/interface_bringup/launch/interface.launch.xml index fc855b77..72f9cdfc 100644 --- a/ros_ws/src/robot/autonomy/0_interface/interface_bringup/launch/interface.launch.xml +++ b/ros_ws/src/robot/autonomy/0_interface/interface_bringup/launch/interface.launch.xml @@ -1,17 +1,17 @@ - - - diff --git a/ros_ws/src/robot/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml b/ros_ws/src/robot/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml index 0ca1c57c..384160a4 100644 --- a/ros_ws/src/robot/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml +++ b/ros_ws/src/robot/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml @@ -1,6 +1,6 @@ - + diff --git a/ros_ws/src/robot/autonomy/2_perception/perception_bringup/launch/perception.launch.xml b/ros_ws/src/robot/autonomy/2_perception/perception_bringup/launch/perception.launch.xml index 0458d99e..0363a73b 100644 --- a/ros_ws/src/robot/autonomy/2_perception/perception_bringup/launch/perception.launch.xml +++ b/ros_ws/src/robot/autonomy/2_perception/perception_bringup/launch/perception.launch.xml @@ -3,13 +3,19 @@ - + + + + + + - - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt index 4e499df5..717649f7 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt @@ -135,7 +135,10 @@ include_directories( # ########## # # Build ## # ########## -set(CMAKE_BUILD_TYPE Release) +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'Release'.") + set(CMAKE_BUILD_TYPE Release) +endif() # # Specify additional locations of header files # # Your package locations should be listed before other locations @@ -153,6 +156,10 @@ include_directories( # # Declare a cpp executable add_executable(disparity_expansion src/disparity_expansion.cpp) +target_include_directories(disparity_expansion PUBLIC + $ + $) + target_link_libraries(disparity_expansion ${OpenCV_LIBS} ${catkin_LIBRARIES} ) @@ -171,23 +178,6 @@ ament_target_dependencies(disparity_expansion stereo_msgs tf2_geometry_msgs ) -add_executable(disparity_conv src/disparity_conv.cpp) -target_link_libraries(disparity_conv ${OpenCV_LIBS} ${catkin_LIBRARIES} -) - -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} @@ -212,9 +202,6 @@ ament_target_dependencies(disparity_pcd install(TARGETS disparity_expansion DESTINATION lib/${PROJECT_NAME} ) -install(TARGETS disparity_conv - DESTINATION lib/${PROJECT_NAME} -) install(TARGETS disparity_pcd DESTINATION lib/${PROJECT_NAME} ) diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/README.md b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/README.md index fe8f1dcb..8ddb1380 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/README.md +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/README.md @@ -8,8 +8,6 @@ Currently this package also has the following nodes * `disparity_expansion`: Generate expanded disparity -* `disparity_conv`: Convert nerian disparity to float32 - * `disparity_pcd`: Disparity image to point cloud. ### Who do I talk to? ### diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml index a20abd3d..4c7c138e 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml @@ -1,13 +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 + # parameters + scale: 1.0 # units size of a meter. depth value corresponds to this many meters + downsample_scale: 1.0 # the ratio to scale down the disparity image before processing + baseline_fallback: 0.5 # if the baseline is 0 from the camera_info, use this value instead + lut_max_disparity: 180 # maximum disparity value in the disparity image + robot_radius: 1.0 + bg_multiplier: 1.0 + # sensor_pixel_error: 0.5 # what is this? and why was it 0.5? + sensor_pixel_error: 0.0 \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp new file mode 100644 index 00000000..65459a3a --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 DisparityExpansionNode : public rclcpp::Node { + protected: + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr disparity_sub_; + rclcpp::Subscription::SharedPtr depth_sub_; + + rclcpp::Publisher::SharedPtr expanded_disparity_fg_pub; + rclcpp::Publisher::SharedPtr expanded_disparity_bg_pub; + rclcpp::Publisher::SharedPtr expansion_poly_pub; + rclcpp::Publisher::SharedPtr expansion_cloud_pub; + rclcpp::Publisher::SharedPtr frustum_pub; + + bool LUT_ready = false; + bool got_cam_info = false; + + double downsample_scale; + image_geometry::PinholeCameraModel model_; + double baseline; + + struct LUTCell { + unsigned int idx1; + unsigned int idx2; + }; + int lut_max_disparity; + double robot_radius; + double padding; + double bg_multiplier; + double pixel_error; + double scale; + std::vector> table_u; + std::vector> table_v; + double table_d[200]; + double fx, fy, cx, cy; + unsigned int width, height; + + cv::Mat convert_depth_to_disparity(const cv::Mat& depth_image); + + void generate_expansion_lookup_table(); + + public: + DisparityExpansionNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + void set_cam_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_info); + + /** + * @brief Use depth image input instead of disparity. Converts depth to disparity, then call + * process_disparity_image + * + * @param msg + */ + void process_depth_image(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + + void process_disparity_image(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp); + + void publish_expansion_poly(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImagePtr& fg_msg, + const cv_bridge::CvImagePtr& bg_msg); + + void publish_expansion_cloud(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImageConstPtr& cv_ptrdisparity, + const cv_bridge::CvImagePtr& fg_msg, + const cv_bridge::CvImagePtr& bg_msg); + + void publish_frustum(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg); +}; diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml index 056d0d24..59c718d9 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml @@ -1,11 +1,10 @@ - - - + + - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_conv.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_conv.cpp deleted file mode 100644 index 218aa7a9..00000000 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_conv.cpp +++ /dev/null @@ -1,320 +0,0 @@ -/* - * Copyright (c) 2016 Carnegie Mellon University, Author - * - * For License information please see the LICENSE file in the root directory. - * - */ - -// Postprocessing of disparity image. -/* - -#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 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::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; - cv::Mat prev_disp; - bool first; -}; - -// ########################################################################################################### -// ########################################################################################################### -void disparity_conv::stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr &msg_disp) { - RCLCPP_INFO_ONCE(this->get_logger(), "Disparity CB"); - - 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 = std::make_shared(); - cv::Mat disparity32F; - 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) { - 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 - - // if(cv_ptrdisparity->image.type()!=CV_32F) - // { - // //convert invalidate disparity to zero - // for (int32_t j = 0; j< disparity32F.rows ; j++) - // { - // for (int32_t i = 0; i< disparity32F.cols ; i++) - // { - // u_int16_t val = cv_ptrdisparity->image.at(j,i); - // if (val == 0xFFF) - // { - // disparity32F.at(j,i) = 0.0f; - // } - // else - // { - // disparity32F.at(j,i) = float(val/16); - // } - // } - // } - // } - - int filt_mode = 2; - 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) { - 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::filter2D(disparity32F, di_msg->image, CV_32F, kernel); - } else { - disparity32F.copyTo(di_msg->image); - } - 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())); - return; -} - -// #################################################################################################################### -// ################################################################################################################### - -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 = 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); - - try { - cv_ptrdisparity = cv_bridge::toCvShare(msg_disp); - - 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) { - 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 - - 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::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 ); - di_msg->image = cv::abs(di_msg->image); - 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 - di_msg->image.setTo(NAN); - - 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 { - disparity32F.copyTo(di_msg->image); - } - - // TEMPORAL DIFFERENCING FILTER - cv::Mat mask2; - 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(prev_disp); - // disparity32F.copyTo(di_msg->image,mask); - } - - 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); - 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())); - return; -} - -// ############################################################################################################################################# -// ############################################################################################################################################ - -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, - // 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; - first = true; -} - -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 - 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/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp index 547fad67..253b53c8 100755 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp @@ -6,126 +6,114 @@ */ // Applies C-Space expansion on disparity images. +#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 "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 DisparityExpansionNode : public rclcpp::Node { - public: - DisparityExpansionNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - 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; - bool LUT_ready; - bool got_cam_info; - image_geometry::PinholeCameraModel model_; - - 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; - }; - int lut_max_disparity_; - double robot_radius_; - double padding_; - double bg_multiplier_; - double pixel_error_; - double scale_; - std::vector> table_u; - std::vector> table_v; - double table_d[200]; - double fx_, fy_, cx_, cy_; - unsigned int width, height; -}; - -void DisparityExpansionNode::generateExpansionLUT() { - bool debug = false; - if (debug) { - RCLCPP_WARN(this->get_logger(), "Expansion LUT Debug disabled creation"); - LUT_ready = true; +DisparityExpansionNode::DisparityExpansionNode(const rclcpp::NodeOptions& options) + : Node("DisparityExpansionNode", options) { + this->declare_parameter("scale", 2.0); + this->get_parameter("scale", this->scale); + this->declare_parameter("robot_radius", 2.0); + this->get_parameter("robot_radius", this->robot_radius); + this->declare_parameter("lut_max_disparity", 164); + this->get_parameter("lut_max_disparity", this->lut_max_disparity); + this->declare_parameter("padding", 2.0); + this->get_parameter("padding", this->padding); + this->declare_parameter("baseline_fallback", 0.5); + this->declare_parameter("bg_multiplier", 5.0); + this->get_parameter("bg_multiplier", this->bg_multiplier); + this->declare_parameter("sensor_pixel_error", 0.5); + this->get_parameter("sensor_pixel_error", this->pixel_error); + this->declare_parameter("downsample_scale", 2.0); + this->get_parameter("downsample_scale", this->downsample_scale); + + // subscribers + this->cam_info_sub_ = this->create_subscription( + "camera_info", 1, + std::bind(&DisparityExpansionNode::set_cam_info, this, std::placeholders::_1)); + + this->disparity_sub_ = this->create_subscription( + "disparity", 1, + std::bind(&DisparityExpansionNode::process_disparity_image, this, std::placeholders::_1)); + + this->depth_sub_ = this->create_subscription( + "depth", 1, + std::bind(&DisparityExpansionNode::process_depth_image, this, std::placeholders::_1)); + + // publishers + this->expanded_disparity_fg_pub = + this->create_publisher("expanded_disparity_fg", 10); + this->expanded_disparity_bg_pub = + this->create_publisher("expanded_disparity_bg", 10); + this->expansion_poly_pub = + this->create_publisher("expansion_poly", 10); + this->expansion_cloud_pub = + this->create_publisher("expansion_cloud", 10); + this->frustum_pub = this->create_publisher("frustum", 10); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " waiting"); +} + +void DisparityExpansionNode::set_cam_info( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_info) { + if (this->got_cam_info) return; + this->model_.fromCameraInfo(msg_info); + RCLCPP_INFO_ONCE(this->get_logger(), "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f", + this->model_.fx(), this->model_.fy(), this->model_.cx(), this->model_.cy()); + this->cx = this->model_.cx() / this->downsample_scale; + this->cy = this->model_.cy() / this->downsample_scale; + this->fx = this->fy = this->model_.fx() / this->downsample_scale; + this->width = msg_info->width / this->downsample_scale; + this->height = msg_info->height / this->downsample_scale; + this->baseline = -msg_info->p[3] / msg_info->p[0]; + if (this->baseline == 0.0) { + this->get_parameter("baseline_fallback", this->baseline); + RCLCPP_ERROR_STREAM_ONCE( + this->get_logger(), + "Baseline from camera_info was 0, setting to this->baseline_fallback:" + << this->baseline); } - if (LUT_ready) { + this->baseline *= this->downsample_scale; + this->generate_expansion_lookup_table(); + this->got_cam_info = true; + RCLCPP_INFO(this->get_logger(), "Baseline: %.4f meters", this->baseline); + RCLCPP_INFO(this->get_logger(), "Focal length (fx): %.4f pixels", this->fx); +} + +void DisparityExpansionNode::generate_expansion_lookup_table() { + if (this->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_; - 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)); + RCLCPP_INFO(this->get_logger(), "Fx Fy Cx Cy: %f %f , %f %f \nW H this->baseline: %d %d %f", + this->fx, this->fy, this->cx, this->cy, this->width, this->height, this->baseline); + double r = this->robot_radius; // expansion radius in cm + this->table_u = std::vector>(this->lut_max_disparity, + std::vector(this->width)); + this->table_v = std::vector>(this->lut_max_disparity, + std::vector(this->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++) { - disparity = disp_idx / scale_; // 1 cell = 0.5m, z is in meters - r = robot_radius_; // * exp(DEPTH_ERROR_COEFF*z); - z = baseline * fx_ / disparity; + for (unsigned int disp_idx = 1; disp_idx < lut_max_disparity; disp_idx++) { + disparity = disp_idx / this->scale; // 1 cell = 0.5m, z is in meters + r = this->robot_radius; // * exp(DEPTH_ERROR_COEFF*z); + z = this->baseline * this->fx / disparity; - double disp_new = baseline * fx_ / (z - robot_radius_) + 0.5; + double disp_new = this->baseline * this->fx / (z - this->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; + y = (v - this->cy) * z / this->fy; 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; + v1 = this->fy * r1 / z + this->cy; + v2 = this->fy * r2 / z + this->cy; if ((v2 - v1) < 0) RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", v1, v2, disp_idx); @@ -136,61 +124,42 @@ void DisparityExpansionNode::generateExpansionLUT() { 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; + this->table_v[disp_idx][v].idx1 = v1; + this->table_v[disp_idx][v].idx2 = v2; } - for (int u = (int)width - 1; u >= 0; --u) { - x = (u - center_x) * z * constant_x; + for (int u = (int)this->width - 1; u >= 0; --u) { + x = (u - this->cx) * z / this->fx; 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 = fx_ * r1 / z + center_x; - u2 = fx_ * r2 / z + center_x; + u1 = this->fx * r1 / z + this->cx; + u2 = this->fx * r2 / z + this->cx; if ((u2 - u1) < 0) RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", u1, u2, disp_idx); if (u1 < 0) u1 = 0; - if (u1 > (width - 1)) u1 = width - 1; + if (u1 > (this->width - 1)) u1 = this->width - 1; if (u2 < 0) u2 = 0; - if (u2 > (width - 1)) u2 = width - 1; + if (u2 > (this->width - 1)) u2 = this->width - 1; - table_u[disp_idx][u].idx1 = u1; - table_u[disp_idx][u].idx2 = u2; + this->table_u[disp_idx][u].idx1 = u1; + this->table_u[disp_idx][u].idx2 = u2; } } RCLCPP_WARN(this->get_logger(), "Expansion LUT created: LUT MAX: %d , ROBOT SIZE: %f", - lut_max_disparity_ / 2, robot_radius_); - LUT_ready = true; -} - -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_); + lut_max_disparity / 2, this->robot_radius); + this->LUT_ready = true; } -void DisparityExpansionNode::depthImageCb(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { +void DisparityExpansionNode::process_depth_image( + 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); @@ -203,50 +172,34 @@ void DisparityExpansionNode::depthImageCb(const sensor_msgs::msg::Image::ConstSh 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); + cv::Mat disparity_image = this->convert_depth_to_disparity(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_; + disparity_msg->f = this->fx; // Call the existing disparity processing function - stereoDisparityCb(disparity_msg); + this->process_disparity_image(disparity_msg); } -cv::Mat DisparityExpansionNode::depthToDisparity(const cv::Mat& depth_image) { +cv::Mat DisparityExpansionNode::convert_depth_to_disparity(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); + cv::Mat disparity_image(depth_image.size(), CV_32F); // DISPARITY FORMULA - disparity_image = (baseline * fx_) / (depth_image); // MULT BY 100 + disparity_image = (this->baseline * this->fx) / (depth_image); // MULT BY 100 - RCLCPP_INFO(this->get_logger(), "Baseline: %.4f", baseline); - RCLCPP_INFO(this->get_logger(), "Focal length (fx): %.4f", fx_); + RCLCPP_INFO_ONCE(this->get_logger(), "Baseline: %.4f", this->baseline); + RCLCPP_INFO_ONCE(this->get_logger(), "Focal length (fx): %.4f", this->fx); cv::patchNaNs(disparity_image, 0.0f); disparity_image.setTo(0.0f, disparity_image == std::numeric_limits::infinity()); @@ -254,28 +207,11 @@ cv::Mat DisparityExpansionNode::depthToDisparity(const cv::Mat& depth_image) { 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, +void DisparityExpansionNode::process_disparity_image( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp) { + if (!this->LUT_ready) { + auto& clock = *this->get_clock(); + RCLCPP_INFO_THROTTLE(this->get_logger(), clock, 1, "LUT not ready yet, not processing disparity"); return; } @@ -304,7 +240,8 @@ void DisparityExpansionNode::stereoDisparityCb( RCLCPP_ERROR(this->get_logger(), "Received empty disparity image"); return; } - cv::resize(disparity32F, disparity32F, cv::Size(), 0.5, 0.5, cv::INTER_AREA); + cv::resize(disparity32F, disparity32F, cv::Size(), 1.0 / this->downsample_scale, + 1.0 / this->downsample_scale, cv::INTER_AREA); if (disparity32F.empty()) { RCLCPP_ERROR(this->get_logger(), "Resized disparity image is empty"); return; @@ -331,375 +268,386 @@ void DisparityExpansionNode::stereoDisparityCb( bg_msg->header = msg_disp->header; bg_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; - if (1) // make cloud - { - // Use correct principal point from calibration - float center_x = cx_; - float center_y = cy_; - - float constant_x = 1.0 / fx_; - float constant_y = 1.0 / fy_; - - { - 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 (!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; - - if (disparity32F.empty()) { - RCLCPP_ERROR(this->get_logger(), "disparity32F matrix is empty."); - return; - } + RCLCPP_INFO_ONCE(this->get_logger(), "IMG TYPE 32FC, GOOD"); - 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::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; - - cv::Mat submat; - cv::divide(baseline * fx_, submat_t, submat); - submat = (submat - disp_to_depth); /// robot_radius_; - - if (padding < 0.0) { - float range = bg_multiplier_ * robot_radius_; - float max_depth = 0.0; - bool found = true; - int ctr = 1; - while (found) { - found = false; - 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; - } - } - } - ctr++; - } - disp_to_depth += max_depth; - } else - disp_to_depth += padding; // baseline * fx_/min; - - 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); - - int u_temp = u1 + max_idx; - if (u_temp >= u) - u = u1; - else - 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_); - } - } + rclcpp::Time start = this->get_clock()->now(); + + // In a first step, an intermediate disparity image is generated by horizontally expanding all + // disparity values from the stereo disparity map using the u1/u2 look-up table. + + // The first step expands disparities along the image XY axis Figure 3 (right) + for (int v = (int)this->height - 2; (v >= 0); v -= 1) { + for (int u = (int)this->width - 1; u >= 0; u -= 1) { + float disparity_value = disparity32F.at(v, u); + + if (std::isnan(double(disparity_value * this->scale)) || + ((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) || + ((int(disparity_value * this->scale) + 1) <= 0)) { + RCLCPP_INFO_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Step 1 Expand u: Disparity out of range: " + << disparity_value << ", lut_max_disparity: " << this->lut_max_disparity + << " Scale: " << this->scale); + continue; } - 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 (!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; - - 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); - 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_; - - 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_; - - if (padding < 0.0) { - float range = bg_multiplier_ * robot_radius_; - float max_depth = 0.0; - bool found = true; - int ctr = 1; - while (found) { - found = false; - 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; - } - } - } - ctr++; - } - disp_to_depth += max_depth; - } else - disp_to_depth += padding; // baseline * fx_/min; - - 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; - if (v_temp >= v) - v = v1; - else - 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_); - } - } + unsigned int u1 = this->table_u[int(disparity_value * this->scale) + 1][u].idx1; + unsigned int u2 = this->table_u[int(disparity_value * this->scale) + 1][u].idx2; + + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "disparity32F matrix is empty."); + return; } - 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())); - - if (expansion_poly_pub->get_subscription_count() > 0) // create expansion PCD - { - 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::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.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - - 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); - prev_depth = depth_value; - 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; - gm_p.z = depth_value; - marker.points.push_back(gm_p); - - } else { - marker_arr.markers.push_back(marker); - marker.points.clear(); - marker.id++; + // top left corner, then width and height + 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::minMaxLoc(disparity32F(roi), &min, &max, &p1, &p2); + min_idx = p1.x; + max_idx = p2.x; + float disp_new_fg = max; + float disp_new_bg = min; + float disp_to_depth = this->baseline * this->fx / max; + + cv::Mat submat; + cv::divide(baseline * this->fx, submat_t, submat); + submat = (submat - disp_to_depth); /// this->robot_radius; + + if (this->padding < 0.0) { + float range = this->bg_multiplier * this->robot_radius; + float max_depth = 0.0; + bool found = true; + int ctr = 1; + while (found) { + found = false; + 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; + } } } + ctr++; } - marker_arr.markers.push_back(marker); - expansion_poly_pub->publish(marker_arr); + disp_to_depth += max_depth; + } else + disp_to_depth += this->padding; + + disp_new_bg = this->baseline * this->fx / (disp_to_depth); + disparity_fg(roi).setTo(disp_new_fg); + disparity_bg(roi).setTo(disp_new_bg); + + int u_temp = u1 + max_idx; + if (u_temp >= u) + u = u1; + else + u = u_temp + 1; + } + } + + disparity_fg.copyTo(disparity32F); + disparity_bg.copyTo(disparity32F_bg); + + // In a second step, each pixel in the intermediate disparity image is expanded vertically using + // the v1/v2 look-up table and the expansion column is stored with a new disparity value from + // the dnew look-up table in the final C-space disparity map + 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) + + this->pixel_error; // disparity_row[v * row_step + u];// + 0.5; + + if (std::isnan(double(disparity_value * this->scale)) || + ((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) || + ((int(disparity_value * this->scale) + 1) <= 0)) { + RCLCPP_INFO_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Step 2 Expand v: Disparity out of range: " + << disparity_value << ", lut_max_disparity: " << this->lut_max_disparity + << " Scale: " << this->scale); + continue; } - if (expansion_cloud_pub->get_subscription_count() > 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, 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; - 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; - pt_real.z = depth_value; - pt_real.intensity = 170; //*disparity32F.at(v,u)/200; - - { - point_counter++; - cloud->points.push_back(pt_fg); - point_counter++; - cloud->points.push_back(pt_bg); - point_counter++; - cloud->points.push_back(pt_real); + unsigned int v1 = this->table_v[int(disparity_value * this->scale) + 1][v].idx1; + unsigned int v2 = this->table_v[int(disparity_value * this->scale) + 1][v].idx2; + + 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); + min_idx = p1.y; + max_idx = p2.y; + float disp_new_fg; + float disp_new_bg; + float disp_to_depth = this->baseline * this->fx / max; + + disp_new_fg = this->baseline * this->fx / (disp_to_depth - this->robot_radius) + + this->pixel_error; + + cv::Mat submat; + cv::divide(baseline * this->fx, submat_t, submat); + cv::minMaxLoc(disparity32F_bg(roi), &min, &max, &p1, &p2); + disp_to_depth = this->baseline * this->fx / max; + submat = (submat - disp_to_depth); /// this->robot_radius; + + if (this->padding < 0.0) { + float range = this->bg_multiplier * this->robot_radius; + float max_depth = 0.0; + bool found = true; + int ctr = 1; + while (found) { + found = false; + 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; + } } } + ctr++; } - cloud->width = point_counter; + disp_to_depth += max_depth; + } else + disp_to_depth += this->padding; // this->baseline * this->fx/min; + + disp_new_bg = this->baseline * this->fx / (disp_to_depth + this->robot_radius) - + this->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; + if (v_temp >= v) + v = v1; + else + v = v_temp + 1; + } + } + + 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())); + + this->publish_frustum(msg_disp); + // only visualize expansion polygon if there's a subscriber + if (expansion_poly_pub->get_subscription_count() > 0) + this->publish_expansion_poly(msg_disp, fg_msg, bg_msg); + // only visualize expansion cloud if there's a subscriber + if (expansion_cloud_pub->get_subscription_count() > 0) + this->publish_expansion_cloud(msg_disp, cv_ptrdisparity, fg_msg, bg_msg); + + return; +} - cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); - cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); +void DisparityExpansionNode::publish_frustum( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg) { + visualization_msgs::msg::Marker marker; + auto sensor_frame = msg->header.frame_id; + marker.header.frame_id = sensor_frame; + marker.ns = "frustum"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = marker.pose.position.y = marker.pose.position.z = 0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, 0.0); + marker.pose.orientation = tf2::toMsg(q); + + marker.scale.x = marker.scale.y = marker.scale.z = 1.; + marker.color.a = 0.3; + marker.color.r = 0.5; + marker.color.g = 1.0; + marker.color.b = 0.9; + + double max_depth = this->lut_max_disparity * this->baseline * this->fx; + // v, u + std::tuple top_left = {0.0, 0.0}; + std::tuple top_right = {0.0, width}; + std::tuple bottom_left = {height, 0.0}; + std::tuple bottom_right = {height, width}; + std::vector> fov = {top_left, top_right, bottom_right, bottom_left}; + + std::vector points_3d; + for (auto& [v, u] : fov) { + geometry_msgs::msg::Point point; + point.x = (u - this->cx) * max_depth / this->fx; + point.y = (v - this->cy) * max_depth / this->fy; + point.z = max_depth; + points_3d.push_back(point); + } + geometry_msgs::msg::Point top_left_p = points_3d.at(0); + geometry_msgs::msg::Point top_right_p = points_3d.at(1); + geometry_msgs::msg::Point bottom_right_p = points_3d.at(2); + geometry_msgs::msg::Point bottom_left_p = points_3d.at(3); + geometry_msgs::msg::Point center_p; // 0,0,0 + + // push back the triangles that make up the frustum. 4 sides, and 2 triangles to make the box in + // left face + marker.points.push_back(top_left_p); + marker.points.push_back(bottom_left_p); + marker.points.push_back(center_p); + + // top face + marker.points.push_back(top_left_p); + marker.points.push_back(top_right_p); + marker.points.push_back(center_p); + + // right face + marker.points.push_back(top_right_p); + marker.points.push_back(bottom_right_p); + marker.points.push_back(center_p); + + // bottom face + marker.points.push_back(bottom_left_p); + marker.points.push_back(bottom_right_p); + marker.points.push_back(center_p); + + // frustum base + // upper right triangle + marker.points.push_back(top_left_p); + marker.points.push_back(top_right_p); + marker.points.push_back(bottom_right_p); + // lower left triangle + marker.points.push_back(top_left_p); + marker.points.push_back(bottom_left_p); + marker.points.push_back(bottom_right_p); + + this->frustum_pub->publish(marker); +} - sensor_msgs::msg::PointCloud2 cloud_PC2; - pcl::toROSMsg(*cloud, cloud_PC2); - expansion_cloud_pub->publish(cloud_PC2); +void DisparityExpansionNode::publish_expansion_poly( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImagePtr& fg_msg, const cv_bridge::CvImagePtr& bg_msg) { + 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::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); + + marker.scale.x = marker.scale.y = marker.scale.z = 0.5; + marker.color.a = 0.3; // Don't forget to set the alpha! + + geometry_msgs::msg::PolygonStamped poly; + poly.header = msg_disp->header; + int v = 120; + float prev_depth = 0.0; + // TODO: what is this magic number 239? wtf + for (int v = (int)0; v <= 239; v += 10) { + for (int u = (int)width - 1; u >= 0; u--) { + float depth_value = + this->baseline * this->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 (!std::isnan(depth_value) && !std::isinf(depth_value) && depth_diff < 0.5) { + marker.color.r = 1.0 * (fg_msg->image.at(v, u) - this->pixel_error) / + fg_msg->image.at(v, u); + marker.color.g = 1.0 * (this->pixel_error) / fg_msg->image.at(v, u); + geometry_msgs::msg::Point gm_p; + gm_p.x = (u - this->cx) * depth_value / this->fx; + gm_p.y = (v - this->cy) * depth_value / this->fy; + gm_p.z = depth_value; + marker.points.push_back(gm_p); + + depth_value = this->baseline * this->fx / bg_msg->image.at(v, u); + gm_p.x = (u - this->cx) * depth_value / this->fx; + gm_p.y = (v - this->cy) * depth_value / this->fy; + gm_p.z = depth_value; + marker.points.push_back(gm_p); + + } else { + marker_arr.markers.push_back(marker); + marker.points.clear(); + marker.id++; } } } - return; + marker_arr.markers.push_back(marker); + this->expansion_poly_pub->publish(marker_arr); } -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); +void DisparityExpansionNode::publish_expansion_cloud( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImageConstPtr& cv_ptrdisparity, const cv_bridge::CvImagePtr& fg_msg, + const cv_bridge::CvImagePtr& bg_msg) { + 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, pt_real; + + for (int v = (int)this->height - 1; v >= 0; v -= 4) { + for (int u = (int)this->width - 1; u >= 0; u -= 4) { + // foreground + // free_msg->image.at(v,u)[0]; + float depth_value = this->baseline * this->fx / fg_msg->image.at(v, u); + pt_fg.x = (u - this->cx) * depth_value / this->fx; + pt_fg.y = (v - this->cy) * depth_value / this->fy; + pt_fg.z = depth_value; + pt_fg.intensity = 220; + + // background + // free_msg->image.at(v,u)[1]; + depth_value = this->baseline * this->fx / bg_msg->image.at(v, u); + pt_bg.x = (u - this->cx) * depth_value / this->fx; + pt_bg.y = (v - this->cy) * depth_value / this->fy; + pt_bg.z = depth_value; + pt_bg.intensity = 120; + + // actual depth + // new_disparity_bridgePtr->image.at(v,u); + depth_value = this->baseline * this->fx / cv_ptrdisparity->image.at(v, u); + pt_real.x = (u - this->cx) * depth_value / this->fx; + pt_real.y = (v - this->cy) * depth_value / this->fy; + pt_real.z = depth_value; + pt_real.intensity = 170; //*disparity32F.at(v,u)/200; + + point_counter++; + cloud->points.push_back(pt_fg); + point_counter++; + cloud->points.push_back(pt_bg); + point_counter++; + cloud->points.push_back(pt_real); + } + } + cloud->width = point_counter; + + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); - 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_ = 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)); + sensor_msgs::msg::PointCloud2 cloud_PC2; + pcl::toROSMsg(*cloud, cloud_PC2); + this->expansion_cloud_pub->publish(cloud_PC2); } int main(int argc, char** argv) { diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt index 85503b6b..8478015a 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_geometry REQUIRED) find_package(nav_msgs REQUIRED) @@ -38,6 +39,7 @@ ament_target_dependencies( "std_msgs" "tf2" "tf2_ros" + "tf2_geometry_msgs" "geometry_msgs" "cv_bridge" "image_geometry" diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp index 2f829210..f97a6a0d 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -17,11 +18,12 @@ #include #include #include +#include #include namespace disparity_graph { -class DisparityGraph : rclcpp::Node { +class DisparityGraph { struct DisparityGraphNode { cv_bridge::CvImagePtr Im_fg; cv_bridge::CvImagePtr Im_bg; @@ -31,22 +33,24 @@ class DisparityGraph : rclcpp::Node { }; std::deque disp_graph_; + rclcpp::Node::SharedPtr node_ptr; + size_t graph_size_; double thresh_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - std::string sensor_frame_, fixed_frame_, stabilized_frame_; + std::shared_ptr tf_buffer_ptr; + 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; + double angle_tol, displacement_tol, assume_seen_within_radius; 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; + rclcpp::Subscription::SharedPtr cam_info_sub_; + message_filters::Subscriber disp_fg_sub_, disp_bg_sub_; typedef message_filters::sync_policies::ExactTime @@ -57,20 +61,59 @@ class DisparityGraph : rclcpp::Node { 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"), + : graph_size_(10), angle_tol(30.0), displacement_tol(1.5), first(true), got_cam_info(false) { + } + + // we need this separate initialization function because we're taking a node_ptr. shared_from_this() only works after constructor exits. + // ideally we switch to use nodelets so there's a proper setup/activate phase + void initialize(const rclcpp::Node::SharedPtr &node_ptr, + const std::shared_ptr tf_buffer_ptr) { + RCLCPP_INFO(node_ptr->get_logger(), "DisparityGraph initialize called"); + this->node_ptr = node_ptr; + this->tf_buffer_ptr = tf_buffer_ptr; disparity_graph_marker_pub_ = - create_publisher("disparity_marker", 10); - marker_.header.frame_id = fixed_frame_; - marker_.header.stamp = this->get_clock()->now(); + node_ptr->create_publisher("disparity_graph", 10); + + node_ptr->declare_parameter("baseline_fallback", 0.5); + + node_ptr->declare_parameter("fixed_frame", "map"); + node_ptr->get_parameter("fixed_frame", this->fixed_frame); + node_ptr->declare_parameter("stabilized_frame", "base_link_stabilized"); + node_ptr->get_parameter("stabilized_frame", this->stabilized_frame); + node_ptr->declare_parameter("assume_seen_within_radius", 1.0); + node_ptr->get_parameter("assume_seen_within_radius", assume_seen_within_radius); + node_ptr->declare_parameter("downsample_scale", 1.0); + node_ptr->get_parameter("downsample_scale", downsample_scale); + node_ptr->declare_parameter("low_occ_thresh", 0.9); + node_ptr->get_parameter("low_occ_thresh", thresh_); + node_ptr->declare_parameter("graph_size", 10); + node_ptr->get_parameter("graph_size", graph_size_); + node_ptr->declare_parameter("displacement_tolerance", 1.5); + node_ptr->get_parameter("displacement_tolerance", displacement_tol); + node_ptr->declare_parameter("angle_tolerance", 30.0); + angle_tol = node_ptr->get_parameter("angle_tolerance").as_double() * M_PI / 180.0; + + node_ptr->declare_parameter("expanded_disparity_fg_topic", "expanded_disparity_fg"); + disp_fg_sub_.subscribe(node_ptr, + node_ptr->get_parameter("expanded_disparity_fg_topic").as_string()); + node_ptr->declare_parameter("expanded_disparity_bg_topic", "expanded_disparity_bg"); + disp_bg_sub_.subscribe(node_ptr, + node_ptr->get_parameter("expanded_disparity_bg_topic").as_string()); + + exact_sync.reset(new ExactSync(ExactPolicy(50), disp_fg_sub_, disp_bg_sub_)); + exact_sync->registerCallback(std::bind(&DisparityGraph::disp_cb, this, + std::placeholders::_1, std::placeholders::_2)); + + node_ptr->declare_parameter("camera_info_topic", "camera_info"); + cam_info_sub_ = node_ptr->create_subscription( + node_ptr->get_parameter("camera_info_topic").as_string(), 1, + std::bind(&DisparityGraph::get_cam_info, this, std::placeholders::_1)); + + marker_.header.frame_id = this->fixed_frame; marker_.ns = "disparityGraph"; marker_.id = 0; marker_.type = visualization_msgs::msg::Marker::ARROW; @@ -83,114 +126,101 @@ class DisparityGraph : rclcpp::Node { 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_.scale.y = 0.2; + marker_.scale.z = 0.2; 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)); + RCLCPP_INFO(node_ptr->get_logger(), "Disparity Graph Node Started"); } - //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; + // RCLCPP_INFO(node_ptr->get_logger(), "Received disp stamp: %lf", + // (node_ptr->get_clock()->now() - disp_fg->header.stamp).seconds()); + this->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); + auto tf_msg = + tf_buffer_ptr->lookupTransform(this->sensor_frame, this->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()); + RCLCPP_ERROR(node_ptr->get_logger(), "DG disp_cb %s", ex.what()); return; } std::scoped_lock lock(io_mutex); + DisparityGraphNode new_graph_node; + + new_graph_node.header = disp_bg->header; + new_graph_node.w2s_tf = transform; + new_graph_node.s2w_tf = transform.inverse(); + try { + new_graph_node.Im_fg = + cv_bridge::toCvCopy(disp_fg, sensor_msgs::image_encodings::TYPE_32FC1); + new_graph_node.Im_bg = + cv_bridge::toCvCopy(disp_bg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (const std::exception &ex) { + RCLCPP_ERROR(node_ptr->get_logger(), "\n\n\n\n\n\t\t\tDEQUE ERR\n\n\n\n: %s", + ex.what()); + return; + } + if (first) { + // if first, initialize the graph by adding the node to the front and back of the queue 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); + disp_graph_.push_front(new_graph_node); + disp_graph_.push_back(new_graph_node); } else { - tf2::Vector3 curr_p = transform.inverse().getOrigin(); + tf2::Vector3 new_position = new_graph_node.s2w_tf.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; - } + tf2::Vector3 graph_position = it->s2w_tf.getOrigin(); + tf2Scalar angle_difference = tf2::angleShortestPath(new_graph_node.s2w_tf.getRotation(), + it->s2w_tf.getRotation()); + tf2Scalar position_difference = new_position.distance(graph_position); - n.header = disp_bg->header; - n.w2s_tf = transform; - n.s2w_tf = transform.inverse(); + if (fabs(angle_difference) >= angle_tol || + fabs(position_difference) > displacement_tol) { + RCLCPP_INFO(node_ptr->get_logger(), "Adding new node, graph size now %d", + (int)disp_graph_.size()); - 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_.push_front(new_graph_node); } } disp_graph_.pop_back(); - disp_graph_.push_back(n); + disp_graph_.push_back(new_graph_node); + } + + publish_disparity_graph_marker(); + } + + void publish_disparity_graph_marker() { + // create marker + visualization_msgs::msg::MarkerArray marker_arr; + + for (uint i = 0; i < disp_graph_.size(); i++) { + auto position = disp_graph_.at(i).s2w_tf.getOrigin(); + tf2::toMsg(position, marker_.pose.position); + auto rotation = disp_graph_.at(i).s2w_tf.getRotation(); + marker_.pose.orientation = tf2::toMsg(rotation); + marker_.header.stamp = node_ptr->now(); + marker_.ns = "pose"; + marker_.id = i; + marker_arr.markers.push_back(marker_); } + + disparity_graph_marker_pub_->publish(marker_arr); } void get_cam_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr &cam_info_msg) { @@ -200,45 +230,59 @@ class DisparityGraph : rclcpp::Node { 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_); + RCLCPP_INFO(node_ptr->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_ = baseline; + if (this->baseline_ == 0.0) { + node_ptr->get_parameter("baseline_fallback", this->baseline_); + RCLCPP_ERROR_STREAM_ONCE( + node_ptr->get_logger(), + "Baseline from camera_info was 0, setting to baseline_fallback:" + << this->baseline_); } baseline_ *= downsample_scale; - RCLCPP_INFO(this->get_logger(), + RCLCPP_INFO(node_ptr->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(), + RCLCPP_WARN(node_ptr->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; + /** + * @brief checks if the pose is seen by any of the disparity images in the graph and if it is, + * check if it is free + * + * @param pose_to_check the pose to check + * @param thresh threshold for occupancy + * @return std::tuple is_seen, is_free, occupancy value [0, inf] + */ + std::tuple is_pose_seen_and_free( + geometry_msgs::msg::PoseStamped pose_to_check, double thresh) { + double occupancy = 0.0; // init geometry_msgs::msg::PointStamped checked_point_stamped; - checked_point_stamped.point = checked_state.pose.position; - checked_point_stamped.header = checked_state.header; + checked_point_stamped.point = pose_to_check.pose.position; + checked_point_stamped.header = pose_to_check.header; geometry_msgs::msg::PointStamped world_point_stamped; try { - tf_buffer_.transform(checked_point_stamped, world_point_stamped, fixed_frame_); + tf_buffer_ptr->transform(checked_point_stamped, world_point_stamped, this->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; + RCLCPP_ERROR(node_ptr->get_logger(), "Could not transform %s to %s: %s", + this->fixed_frame.c_str(), pose_to_check.header.frame_id.c_str(), ex.what()); + return {false, false, 0.0}; } tf2::Vector3 optical_point; @@ -246,54 +290,84 @@ class DisparityGraph : rclcpp::Node { 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; + bool is_free = true; // whether the point is unoccupied. we will go through all disparity + // graph nodes to check + bool is_seen = false; // whether the point is is_seen by any of the disparity images - float x, y, z; + if (disp_graph_.size() == 0) { + RCLCPP_WARN_STREAM_ONCE( + node_ptr->get_logger(), + "No disparity images in graph, can't see anything, everything is invalid"); + } - x = local_point.getX(); - y = local_point.getY(); - z = local_point.getZ(); + for (const auto &graph_node : disp_graph_) { + tf2::Vector3 local_point = graph_node.w2s_tf * optical_point; + + float x = local_point.getX(); + float y = local_point.getY(); + float z = local_point.getZ(); + + // RCLCPP_INFO_STREAM(node_ptr->get_logger(), "Local Point: " << x << " " << y << " " << + // z); - if (local_point.length() < 1.0) { - seen = true; + if (local_point.length() < this->assume_seen_within_radius) { + is_seen = true; // assume the point is seen if it is close to the camera continue; } + // project the 3D point to the image plane int u = x / z * fx_ + cx_; int v = y / z * fy_ + cy_; + // check that the point is within the image bounds, and that it is in front of the + // camera if (u >= 0 && u < width_ && v >= 0 && v < height_ && z > 0.0) { - seen = true; + is_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)) { + // RCLCPP_INFO_STREAM(node_ptr->get_logger(), "State Disparity: " << + // state_disparity); + + // see Table 1 of Dubey et al. 2017 DROAN. it seems a little different + bool is_state_between_fg_and_bg = + (state_disparity < graph_node.Im_fg->image.at(v, u)) && + (state_disparity > graph_node.Im_bg->image.at(v, u)); + if (is_state_between_fg_and_bg) { + // if the disparity point lies between the disparity images, it is occupied and + // we add to the occupancy value. + // TODO: what does this value mean? is it the sensor model? It seems to be a + // heuristic based on distance. also shouldn't we clamp this at 1.0? or is it + // not really a probability? occupancy += (state_disparity - 0.5) / state_disparity; } else { + // otherwise it's outside, we subtdroan.rvizract from the occupancy value occupancy -= 0.5 * (state_disparity - 0.5) / state_disparity; - occupancy = occupancy < 0.0 ? 0.0 : occupancy; + occupancy = std::clamp(occupancy, 0.0, 1.0); } } if (occupancy >= thresh) { - return false; + is_free = false; + break; } } - return seen; + // RCLCPP_INFO_STREAM(node_ptr->get_logger(),"occupancy:" << occupancy << " is_free: " << + // is_free << " is_seen: " << is_seen); + + return {is_seen, is_free, occupancy}; } 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.header.stamp = node_ptr->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_); + tf_buffer_ptr->transform(checked_point_stamped, world_point_stamped, this->fixed_frame); } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "TF to fixed frame failed: %s", ex.what()); + RCLCPP_ERROR(node_ptr->get_logger(), "TF to fixed frame failed: %s", ex.what()); return false; } @@ -338,7 +412,7 @@ class DisparityGraph : rclcpp::Node { std::lock_guard lock(io_mutex); disp_graph_.clear(); first = true; - RCLCPP_INFO_STREAM(this->get_logger(), "<<< DISP GRAPH CLEARED >>>"); + RCLCPP_INFO_STREAM(node_ptr->get_logger(), "<<< DISP GRAPH CLEARED >>>"); }; rclcpp::Publisher::SharedPtr pcdPub, occPub_; diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/package.xml index 774d51d3..481d5a5c 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/package.xml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_graph/package.xml @@ -14,6 +14,7 @@ std_msgs tf2 tf2_ros + tf2_geometry_msgs geometry_msgs cv_bridge image_geometry diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt index c9abf61d..7b6cfa2d 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(trajectory_library REQUIRED) add_library(disparity_map_representation src/disparity_map_representation.cpp) @@ -60,6 +61,7 @@ ament_target_dependencies( tf2_geometry_msgs tf2_ros visualization_msgs + trajectory_library ) pluginlib_export_plugin_description_file(map_representation_interface disparity_map_representation_plugin.xml) diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp index 60506328..bc59ef45 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp @@ -5,8 +5,9 @@ #include #include -#include +#include #include +#include #include #include #include @@ -14,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -21,30 +23,59 @@ namespace disparity_map_representation { class DisparityMapRepresentation : public map_representation_interface::MapRepresentation { private: - std::unique_ptr disp_graph; + disparity_graph::DisparityGraph disp_graph; - visualization_msgs::msg::MarkerArray markers; - visualization_msgs::msg::Marker points; + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker points_marker; + std_msgs::msg::ColorRGBA green; + std_msgs::msg::ColorRGBA gray; + std_msgs::msg::ColorRGBA red; - // tf::TransformListener* listener; - std::shared_ptr listener; + int obstacle_check_num_points; + double obstacle_check_radius; + tf2::Quaternion Q_UP, Q_DOWN, Q_LEFT, Q_RIGHT; + std::set Q_DIRECTIONS; - // ros::Publisher debug_pub; - rclcpp::Publisher::SharedPtr debug_pub; + tf2::Vector3 quaternion_to_unit_vector(tf2::Quaternion q) { + tf2::Vector3 unit(1, 0, 0); + return tf2::Transform(q) * unit; + } - int obstacle_check_points; - double obstacle_check_radius; + auto create_side_and_up_vectors(tf2::Vector3 direction) { + // Choose an arbitrary up vector + tf2::Vector3 forward = direction.normalized(); + tf2::Vector3 arbitrary_up(0.0, 0.0, 1.0); + + // If forward is parallel to arbitrary_up, choose a different up vector + if (fabs(forward.dot(arbitrary_up)) > 0.999) { + arbitrary_up = tf2::Vector3(0.0, 1.0, 0.0); + } + + // Side vector (perpendicular to both forward and arbitrary up) + tf2::Vector3 side = forward.cross(arbitrary_up).normalized(); + + // True up vector (perpendicular to both forward and side) + tf2::Vector3 up = side.cross(forward).normalized(); + + return std::make_pair(side, up); + } + void check_pose_and_add_marker(const Trajectory& trajectory, const Waypoint& waypoint, + double dist, const tf2::Vector3 direction, + double& closest_obstacle_distance); +tf2::Vector3 determine_waypoint_direction(const Trajectory& trajectory, + const Waypoint& waypoint, + size_t waypoint_index) ; 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 const visualization_msgs::msg::MarkerArray& get_debug_markerarray() const override; + virtual std::vector > get_values( - std::vector > trajectories); + const std::vector& trajectories) override; + + virtual void initialize(const rclcpp::Node::SharedPtr& node_ptr, + const std::shared_ptr tf_buffer_ptr) override; }; } // namespace disparity_map_representation diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml index 2b50fd34..3a7c5fe5 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/package.xml @@ -25,7 +25,9 @@ stereo_msgs tf2 tf2_ros + tf2_geometry_msgs visualization_msgs + trajectory_library ament_lint_auto ament_lint_common diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp index 1ec59963..c78209d4 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp @@ -1,260 +1,162 @@ #include namespace disparity_map_representation { -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; +DisparityMapRepresentation::DisparityMapRepresentation() : MapRepresentation(), disp_graph() { + points_marker.ns = "obstacles"; + points_marker.id = 0; + points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + points_marker.action = visualization_msgs::msg::Marker::ADD; + points_marker.scale.x = 0.03; + points_marker.scale.y = 0.03; + points_marker.scale.z = 0.03; - // debug_pub = nh->advertise("disparity_map_debug", 1); - debug_pub = - this->create_publisher("disparity_map_debug", 1); + green.r = 0; + green.b = 0; + green.g = 1; + green.a = 0.8; + gray.r = 0.7; + gray.b = 0.7; + gray.g = 0.7; + gray.a = 0.6; + red.r = 1; + red.b = 0; + red.g = 0; + red.a = 0.6; - this->declare_parameter("disparity_map/obstacle_check_points", 5); - this->declare_parameter("disparity_map/obstacle_check_radius", 2.0); + 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); + Q_DIRECTIONS = {Q_UP, Q_DOWN, Q_LEFT, Q_RIGHT}; +} - this->get_parameter("disparity_map/obstacle_check_points", obstacle_check_points); - this->get_parameter("disparity_map/obstacle_check_radius", obstacle_check_radius); +void DisparityMapRepresentation::initialize(const rclcpp::Node::SharedPtr& node_ptr, + const std::shared_ptr tf_buffer_ptr) { + RCLCPP_INFO(node_ptr->get_logger(), "DisparityMapRepresentation initialize called"); + MapRepresentation::initialize(node_ptr, tf_buffer_ptr); + node_ptr->declare_parameter("obstacle_check_num_points", 69); + node_ptr->get_parameter("obstacle_check_num_points", this->obstacle_check_num_points); + RCLCPP_INFO_STREAM(node_ptr->get_logger(), + "obstacle_check_num_points: " << this->obstacle_check_num_points); + node_ptr->get_parameter("obstacle_check_radius", this->obstacle_check_radius); + RCLCPP_INFO_STREAM(node_ptr->get_logger(), + "obstacle_check_radius: " << this->obstacle_check_radius); + disp_graph.initialize(node_ptr, tf_buffer_ptr); +} - // 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_); +tf2::Vector3 DisparityMapRepresentation::determine_waypoint_direction(const Trajectory& trajectory, + const Waypoint& waypoint, + size_t waypoint_index) { + tf2::Vector3 waypoint_direction; + if (trajectory.get_num_waypoints() == 1) { + // edge case: only 1 waypoint, make direction match the waypoint's heading + waypoint_direction = quaternion_to_unit_vector(waypoint.quaternion()); + } else { + if (&waypoint == &(trajectory.get_waypoint(0))) { + // edge case: first waypoint, direction is from waypoint 0 to waypoint 1 + waypoint_direction = trajectory.get_waypoint(1).position() - waypoint.position(); + } else { + // otherwise direction is from previous waypoint to current waypoint + waypoint_direction = + waypoint.position() - trajectory.get_waypoint(waypoint_index - 1).position(); + } + } + return waypoint_direction; } std::vector > DisparityMapRepresentation::get_values( - std::vector > trajectories) { - std::vector > values(trajectories.size()); + const std::vector &trajectories) { + marker_array.markers.clear(); + points_marker.points.clear(); + points_marker.colors.clear(); - for (int i = 0; i < trajectories.size(); i++) { - for (int j = 0; j < trajectories[i].size(); j++) { - values[i].push_back(0); - } + // initialize values to 0 + std::vector > values(trajectories.size()); + for (size_t i = 0; i < trajectories.size(); i++) { + values[i] = std::vector(trajectories[i].get_num_waypoints(), 0.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); + size_t trajectory_index = -1; + for (const Trajectory& trajectory : trajectories) { + ++trajectory_index; + points_marker.header.frame_id = trajectory.get_frame_id(); + size_t waypoint_index = -1; + for (const Waypoint& waypoint : trajectory.get_waypoints()) { + ++waypoint_index; // 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(); + // this direction will be used to check for obstacles in a perpendicular direction. + // i.e. left, right, up, down + tf2::Vector3 waypoint_direction = + determine_waypoint_direction(trajectory, waypoint, waypoint_index); - tf2::Vector3 side = up.cross(direction); - side.normalize(); + auto [side, up] = create_side_and_up_vectors(waypoint_direction); - up = side.cross(direction); - up.normalize(); + std::set direction_vectors = {up, side, -up, -side}; - std::vector direction_vectors; - direction_vectors.push_back(up); - direction_vectors.push_back(side); - direction_vectors.push_back(-up); - direction_vectors.push_back(-side); + double closest_obstacle_distance = std::numeric_limits::infinity(); + for (auto direction : direction_vectors) { + for (int k = 1; k < obstacle_check_num_points + 1; k++) { + double dist = k * obstacle_check_radius / (obstacle_check_num_points + 1); - 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); + check_pose_and_add_marker(trajectory, waypoint, dist, direction, + closest_obstacle_distance); } } - 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; + // check the waypoint itself + check_pose_and_add_marker(trajectory, waypoint, 0, waypoint_direction, + closest_obstacle_distance); - 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; + values[trajectory_index][waypoint_index] = closest_obstacle_distance; } } + points_marker.header.stamp = node_ptr->now(); + marker_array.markers.push_back(points_marker); + 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); +void DisparityMapRepresentation::check_pose_and_add_marker(const Trajectory& trajectory, + const Waypoint& waypoint, double dist, + const tf2::Vector3 direction, + double& closest_obstacle_distance) { + tf2::Vector3 point_to_check = waypoint.position() + dist * direction; + geometry_msgs::msg::PoseStamped pose_to_check; + pose_to_check.header.frame_id = trajectory.get_frame_id(); + pose_to_check.header.stamp = trajectory.get_stamp(); + pose_to_check.pose.position.x = point_to_check.x(); + pose_to_check.pose.position.y = point_to_check.y(); + pose_to_check.pose.position.z = point_to_check.z(); + pose_to_check.pose.orientation.w = 1.0; + // RCLCPP_INFO_STREAM(node_ptr->get_logger(), + // "check_pose: " << check_pose.pose.position.x << " " + // << check_pose.pose.position.y << " " + // << check_pose.pose.position.z); + + std_msgs::msg::ColorRGBA color; + + auto [is_seen, is_free, occupancy] = disp_graph.is_pose_seen_and_free(pose_to_check, 0.9); + if (is_seen && is_free) { + color = green; + } else { + closest_obstacle_distance = std::min(dist, closest_obstacle_distance); + if (!is_seen) { + color = gray; + } else if (!is_free) { + color = red; } } - 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; + points_marker.points.push_back(pose_to_check.pose.position); + points_marker.colors.push_back(color); } -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(); +const visualization_msgs::msg::MarkerArray& DisparityMapRepresentation::get_debug_markerarray() + const { + return marker_array; } + } // namespace disparity_map_representation #include diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt index 42c0ed7b..8c176d6b 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(trajectory_library REQUIRED) add_library(map_representation_interface src/map_representation_interface.cpp) @@ -35,6 +36,7 @@ ament_target_dependencies( tf2 tf2_ros visualization_msgs + trajectory_library ) ament_export_targets(map_representation_interface HAS_LIBRARY_TARGET) ament_export_dependencies(rclcpp geometry_msgs tf2 tf2_ros) diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp index d46ff9ff..b2bd4a8b 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp @@ -1,15 +1,23 @@ #ifndef _CORE_MAP_REPRESENTATION_H_ #define _CORE_MAP_REPRESENTATION_H_ -#include +#include +#include + +#include #include #include #include +#include namespace map_representation_interface { -class MapRepresentation : public rclcpp::Node { - private: +class MapRepresentation { + protected: + rclcpp::Node::SharedPtr node_ptr; + std::shared_ptr tf_buffer_ptr; + MapRepresentation() {} + public: /** Takes in a list of trajectories and outputs a value for each waypoint in each trajectory. @@ -17,30 +25,27 @@ class MapRepresentation : public rclcpp::Node { 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; - } - + virtual std::vector > get_values(const std::vector &trajectories) = 0; /** Clears the map. */ - virtual void clear() { RCLCPP_ERROR(this->get_logger(), "clear CALLED BUT NOT IMPLEMENTED"); } + virtual void clear() { + RCLCPP_ERROR(node_ptr->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 const visualization_msgs::msg::MarkerArray& get_debug_markerarray() const {} virtual ~MapRepresentation() {} - protected: - // MapRepresentation() {} - MapRepresentation() : Node("map_representation") {} + virtual void initialize(const rclcpp::Node::SharedPtr& node_ptr, + const std::shared_ptr tf_buffer_ptr) { + RCLCPP_INFO(node_ptr->get_logger(), "MapRepresentation initialize called"); + this->node_ptr = node_ptr; + this->tf_buffer_ptr = tf_buffer_ptr; + } }; } // namespace map_representation_interface diff --git a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml index be4abab7..d2da32b6 100644 --- a/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml +++ b/ros_ws/src/robot/autonomy/3_local/a_world_models/map_representation_interface/package.xml @@ -21,6 +21,7 @@ visualization_msgs pluginlib airstack_msgs + trajectory_library ament_lint_auto ament_lint_common diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt index 6cf1cd8b..5e8c571c 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(airstack_common REQUIRED) find_package(airstack_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(map_representation_interface REQUIRED) # find_package(disparity_map_representation REQUIRED) find_package(pluginlib REQUIRED) @@ -25,10 +26,12 @@ 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" + "nav_msgs" "map_representation_interface" # "disparity_map_representation" "trajectory_controller" diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml index ebfea010..ca4e7fd8 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml @@ -1,27 +1,43 @@ /**: ros__parameters: + + # execution rate in hz + execute_rate: 5.0 + + + # Trajectory parameters + trajectory_library_config: "$(find-pkg-share trajectory_library)/config/acceleration_magnitudes.yaml" + dt: 0.2 + ht: 2.4 + ht_long: 3.0 + max_velocity: 0.7 + magnitude: 0.5 + + # DROAN parameters + robot_radius: 1.0 obstacle_distance_reward: 1.0 forward_progress_reward_weight: 0.5 - robot_radius: 0.75 - look_past_distance: 2.0 - # height parameters - height_mode: 0 - height_above_ground: 1.0 - fixed_height: 1.0 + # map parameters. see disparity_map_representation.cpp + map_representation: disparity_map_representation::DisparityMapRepresentation + obstacle_check_radius: 1.0 # this value MUST be strictly greater than the robot radius + obstacle_check_num_points: 5 + # disparity graph parameters. see disparity_graph.hpp + fixed_frame: "map" + stabilized_frame: "base_link_stabilized" # the frame of the stabilized robot (roll and pitch removed) + baseline_fallback: 0.5 # if the baseline is 0 from the camera_info, use this value instead + assume_seen_within_radius: 2.0 # anything within this radius, assume it is seen and don't check for obstacles + + # GLOBAL_PLAN mode parameters # 0: use the yaw of the subscribed traj, 1: smoothly vary the yaw in the direction of the subscribed trajectory - yaw_mode: 0 - map_representation: PointCloudMapRepresentation - waypoint_buffer_duration: 30.0 - waypoint_angle_threshold: 30.0 - - # trajectory parameters - # trajectory_library_config: "$(find trajectory_library)/config/acceleration_magnitudes.yaml" - # tf_prefix: robot1 - trajectory_library_config: "/root/AirStack/ros_ws/install/trajectory_library/share/trajectory_library/config/acceleration_magnitudes.yaml" - waypoint_spacing: 0.5 - waypoint_spacing_threshold: 0.5 - - # custom waypoint parameters + # either SMOOTH_YAW or TRAJECTORY_YAW + yaw_mode: SMOOTH_YAW + + # AUTO_WAYPOINT mode parameters + auto_waypoint_buffer_duration: 30.0 + auto_waypoint_spacing_threshold: 0.5 + auto_waypoint_angle_threshold: 30.0 + + # CUSTOM_WAYPOINT mode parameters custom_waypoint_timeout_factor: 0.3 custom_waypoint_distance_threshold: 0.5 \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp index 0aaba25d..953d1623 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp @@ -1,21 +1,21 @@ #pragma once -#include +#include +#include + #include #include +#include #include +#include #include -#include -// #include -#include - -#include #include #include #include #include #include #include +#include #include #include #include @@ -24,81 +24,70 @@ using std::placeholders::_1; class DroanLocalPlanner : public rclcpp::Node { private: - const std::string ROBOT_NAME = - std::getenv("ROBOT_NAME") == NULL ? "" : std::getenv("ROBOT_NAME"); std::unique_ptr traj_lib; std::string map_representation_class_string; bool is_global_plan_received; - airstack_msgs::msg::TrajectoryXYZVYaw global_plan_msg; + nav_msgs::msg::Path global_plan_msg; double global_plan_trajectory_distance; bool is_look_ahead_received, is_tracking_point_received; - airstack_msgs::msg::Odometry look_ahead_odom, tracking_point_odom; - std::vector static_trajectories; + // look ahead is + airstack_msgs::msg::Odometry look_ahead_odom, tracking_point_odom; - double waypoint_spacing, obstacle_check_radius, obstacle_distance_reward, + double execute_rate, obstacle_check_radius, obstacle_distance_reward, forward_progress_reward_weight; double robot_radius; - int obstacle_check_points; - double look_past_distance; - - float waypoint_buffer_duration, waypoint_spacing_threshold, waypoint_angle_threshold; + float auto_waypoint_buffer_duration, auto_waypoint_spacing_threshold, + auto_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; - - const int TRAJECTORY_YAW = 0; - const int SMOOTH_YAW = 1; - int yaw_mode; + enum YawMode { TRAJECTORY_YAW, SMOOTH_YAW }; + YawMode yaw_mode; - // custom waypoint params - enum GoalMode { CUSTOM_WAYPOINT, AUTO_WAYPOINT, TRAJECTORY }; + // whether to follow the global plan, a custom waypoint, or automatically interpolated waypoints + enum GoalMode { GLOBAL_PLAN, CUSTOM_WAYPOINT, AUTO_WAYPOINT }; GoalMode goal_mode; double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold; std::shared_ptr map_representation; - rclcpp::Subscription::SharedPtr global_plan_sub; + std::shared_ptr tf_buffer_ptr; + tf2_ros::TransformListener tf_listener; + + 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 custom_waypoint_sub; - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener; - - rclcpp::Publisher::SharedPtr vis_pub; + rclcpp::Publisher::SharedPtr traj_lib_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; + rclcpp::Publisher::SharedPtr map_debug_pub; + + rclcpp::TimerBase::SharedPtr execute_timer; public: DroanLocalPlanner() : Node("droan_local_planner"), - goal_mode(TRAJECTORY), - tf_buffer(this->get_clock()), - tf_listener(tf_buffer) { - // subscribers - global_plan_sub = this->create_subscription( + goal_mode(GLOBAL_PLAN), + tf_buffer_ptr(new tf2_ros::Buffer(this->get_clock())), + tf_listener(*tf_buffer_ptr) { + // follow the global plan + global_plan_sub = this->create_subscription( "global_plan", 10, std::bind(&DroanLocalPlanner::global_plan_callback, this, _1)); waypoint_sub = this->create_subscription( "way_point", 10, std::bind(&DroanLocalPlanner::waypoint_callback, this, _1)); + // from the trajectory controller, the expected look ahead point to start the next + // trajectory look_ahead_sub = this->create_subscription( "look_ahead", 10, std::bind(&DroanLocalPlanner::look_ahead_callback, this, _1)); + // from the tracking controller, the current position of the drone tracking_point_sub = this->create_subscription( "tracking_point", 10, std::bind(&DroanLocalPlanner::tracking_point_callback, this, _1)); custom_waypoint_sub = this->create_subscription( @@ -107,96 +96,93 @@ class DroanLocalPlanner : public rclcpp::Node { // publishers - vis_pub = this->create_publisher( + traj_lib_vis_pub = this->create_publisher( "trajectory_library_vis", 10); obst_vis_pub = this->create_publisher("obstacle_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_segment_to_add", 10); traj_track_pub = this->create_publisher( "trajectory_override", 10); - // services - traj_mode_client = - this->create_client("set_trajectory_mode"); + map_debug_pub = + this->create_publisher("disparity_map_debug", 1); // init parameters - this->declare_parameter("waypoint_spacing", 0.5); - waypoint_spacing = this->get_parameter("waypoint_spacing").as_double(); + this->declare_parameter("execute_rate", 5.); + this->get_parameter("execute_rate", this->execute_rate); this->declare_parameter("obstacle_distance_reward", 1.); - obstacle_distance_reward = this->get_parameter("obstacle_distance_reward").as_double(); + this->get_parameter("obstacle_distance_reward", obstacle_distance_reward); + this->declare_parameter("obstacle_check_radius", 1.); + this->get_parameter("obstacle_check_radius", obstacle_check_radius); this->declare_parameter("forward_progress_reward_weight", 0.5); - forward_progress_reward_weight = - this->get_parameter("forward_progress_reward_weight").as_double(); + this->get_parameter("forward_progress_reward_weight", forward_progress_reward_weight); this->declare_parameter("robot_radius", 0.75); - robot_radius = this->get_parameter("robot_radius").as_double(); - this->declare_parameter("look_past_distance", 0.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->get_parameter("robot_radius", robot_radius); + this->declare_parameter("yaw_mode", "SMOOTH_YAW"); + auto yaw_mode_str = this->get_parameter("yaw_mode").as_string(); + if (yaw_mode_str == "TRAJECTORY_YAW") { + this->yaw_mode = TRAJECTORY_YAW; + } else if (yaw_mode_str == "SMOOTH_YAW") { + this->yaw_mode = SMOOTH_YAW; + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid yaw_mode parameter"); + } this->declare_parameter("map_representation", std::string("PointCloudMapRepresentation")); - map_representation_class_string = 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->get_parameter("map_representation", map_representation_class_string); + this->declare_parameter("auto_waypoint_buffer_duration", 30.); + this->get_parameter("auto_waypoint_buffer_duration", auto_waypoint_buffer_duration); + this->declare_parameter("auto_waypoint_spacing_threshold", 0.5); + this->get_parameter("auto_waypoint_spacing_threshold", auto_waypoint_spacing_threshold); + this->declare_parameter("auto_waypoint_angle_threshold", 30. * M_PI / 180.); + this->get_parameter("auto_waypoint_angle_threshold", auto_waypoint_angle_threshold); this->declare_parameter("custom_waypoint_timeout_factor", 0.3); - custom_waypoint_timeout_factor = - this->get_parameter("custom_waypoint_timeout_factor").as_double(); + this->get_parameter("custom_waypoint_timeout_factor", custom_waypoint_timeout_factor); this->declare_parameter("custom_waypoint_distance_threshold", 0.5); - custom_waypoint_distance_threshold = - this->get_parameter("custom_waypoint_distance_threshold").as_double(); + this->get_parameter("custom_waypoint_distance_threshold", + custom_waypoint_distance_threshold); + RCLCPP_INFO_STREAM(this->get_logger(), "DROAN node name is: " << this->get_name()); this->declare_parameter("trajectory_library_config", std::string("")); - traj_lib = std::make_unique( - this->get_parameter("trajectory_library_config").as_string(), this); + } + void initialize() { pluginlib::ClassLoader map_representation_loader("map_representation_interface", "map_representation_interface::MapRepresentation"); - try { - map_representation = - map_representation_loader.createSharedInstance(map_representation_class_string); - } catch (pluginlib::PluginlibException& ex) { - RCLCPP_INFO(this->get_logger(), - "The MapRepresentation plugin failed to load. Error: %s", ex.what()); - } + auto node_ptr = this->shared_from_this(); + this->map_representation = + map_representation_loader.createSharedInstance(map_representation_class_string); + this->map_representation->initialize(node_ptr, tf_buffer_ptr); + this->traj_lib = std::make_unique( + this->get_parameter("trajectory_library_config").as_string(), node_ptr); + double interval = 1. / this->execute_rate; + this->execute_timer = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(interval), + std::bind(&DroanLocalPlanner::execute, this)); } - virtual ~DroanLocalPlanner() {} - /** - * Sets the global plan height based on the height mode - */ - bool set_global_plan_height_inplace(Trajectory& global_plan) { - // fixed height mode - if (this->height_mode == FIXED_HEIGHT) { - global_plan.set_fixed_height(fixed_height); - } - } + virtual ~DroanLocalPlanner() {} virtual bool execute() { - update_waypoint_mode(); + this->update_waypoint_mode(); - if (!is_global_plan_received) return true; + if (!this->is_global_plan_received) { + RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for global plan"); + return true; + } + if (!this->is_look_ahead_received) { + RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for look ahead point"); + return true; + } Trajectory global_plan(this, global_plan_msg); - set_global_plan_height_inplace(global_plan); // set the height of the global plan // transform the look ahead point to the global plan frame tf2::Vector3 look_ahead_position = tflib::to_tf(look_ahead_odom.pose.position); - bool success = tflib::to_frame(&tf_buffer, look_ahead_position, + bool success = tflib::to_frame(tf_buffer_ptr.get(), look_ahead_position, look_ahead_odom.header.frame_id, global_plan.get_frame_id(), look_ahead_odom.header.stamp, &look_ahead_position); if (!success) { @@ -207,19 +193,21 @@ class DroanLocalPlanner : public rclcpp::Node { // increment how far along the global plan we are double trajectory_distance; - bool valid = global_plan.get_trajectory_distance_at_closest_point(look_ahead_position, - &trajectory_distance); - if (valid) { - global_plan_trajectory_distance += trajectory_distance; - global_plan = global_plan.get_subtrajectory_distance( - global_plan_trajectory_distance, global_plan_trajectory_distance + 10.0); + bool is_valid = global_plan.get_trajectory_distance_at_closest_point(look_ahead_position, + &trajectory_distance); + // trim the global plan to only the next 10 meters + if (is_valid) { + this->global_plan_trajectory_distance += trajectory_distance; + global_plan = global_plan.trim_trajectory_between_distances( + this->global_plan_trajectory_distance, + this->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 + // publish the trimmed segment of the global plan currently being used, for visualization visualization_msgs::msg::MarkerArray global_markers = - global_plan.get_markers(this->now(), 0, 0, 1); + global_plan.get_markers(this->now(), "global_plan", 0, 0, 1); global_plan_vis_pub->publish(global_markers); // get the dynamic trajectories @@ -227,97 +215,50 @@ class DroanLocalPlanner : public rclcpp::Node { 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, global_plan, best_traj); + auto [is_success, best_traj] = this->get_best_trajectory(dynamic_trajectories, global_plan); // publish the trajectory - if (!all_in_collision) { - airstack_msgs::msg::TrajectoryXYZVYaw best_traj_msg = best_traj.get_TrajectoryXYZVYaw(); + if (is_success) { + airstack_msgs::msg::TrajectoryXYZVYaw best_traj_msg = + best_traj.get_TrajectoryXYZVYaw_msg(); // set yaw - if (yaw_mode == SMOOTH_YAW && best_traj.waypoint_count() > 0) { - bool found_initial_heading = false; - double initial_heading = 0; - try { - tf2::Stamped transform; - tf_buffer.canTransform( - best_traj.get_frame_id(), look_ahead_odom.header.frame_id, - look_ahead_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto transform_msg = tf_buffer.lookupTransform(best_traj.get_frame_id(), - look_ahead_odom.header.frame_id, - look_ahead_odom.header.stamp); - tf2::fromMsg(transform_msg, transform); - - transform.setOrigin(tf2::Vector3(0, 0, 0)); // only care about rotation - initial_heading = - tf2::getYaw(transform * tflib::to_tf(look_ahead_odom.pose.orientation)); - - found_initial_heading = true; - - } catch (tf2::TransformException& ex) { - RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); - } - - if (found_initial_heading) { - best_traj_msg.waypoints[0].yaw = initial_heading; - double alpha = 0.1; - double sin_yaw_prev = sin(best_traj_msg.waypoints[0].yaw); - double cos_yaw_prev = cos(best_traj_msg.waypoints[0].yaw); - - for (size_t i = 1; i < best_traj_msg.waypoints.size(); i++) { - airstack_msgs::msg::WaypointXYZVYaw wp_prev = - best_traj_msg.waypoints[i - 1]; - airstack_msgs::msg::WaypointXYZVYaw& wp_curr = best_traj_msg.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; - } - } + if (yaw_mode == SMOOTH_YAW && best_traj.get_num_waypoints() > 0) { + apply_smooth_yaw(best_traj_msg); } best_traj_msg.header.stamp = this->now(); traj_pub->publish(best_traj_msg); + RCLCPP_INFO_STREAM(this->get_logger(), "Published local trajectory"); + } else { + RCLCPP_INFO_STREAM(this->get_logger(), + "No valid trajectories, all trajectories either collide or are unseen"); } return true; } - std::vector> get_trajectory_distances_from_map( - std::vector trajectory_candidates) { - // vector of trajectory candidates in PointStamped-vector form - std::vector> traj_cands_as_point_stamped; - for (size_t i = 0; i < trajectory_candidates.size(); i++) { - traj_cands_as_point_stamped.push_back( - trajectory_candidates[i].get_vector_PointStamped()); - } - // TODO: clearly we should refactor map_representation to accept Trajectory objects - std::vector> trajectory_distances_to_closest_obstacle = - this->map_representation->get_values(traj_cands_as_point_stamped); - return trajectory_distances_to_closest_obstacle; - } - /** - * Choose the best trajectory based on the cost function. Minimize the cost + * @brief Get the best trajectory based on the cost function. Minimize the cost + * + * @param trajectory_candidates + * @param global_plan + * @return std::tuple is_success: whether there exists a valid collision-free + * trajectory, best_traj: the best trajectory */ - bool get_best_trajectory(std::vector trajectory_candidates, Trajectory global_plan, - Trajectory& best_traj_ret) { + std::tuple get_best_trajectory( + const std::vector& trajectory_candidates, Trajectory global_plan) { + Trajectory best_traj_ret; double min_cost = std::numeric_limits::max(); - size_t best_traj_index = 0; - bool all_in_collision = true; + bool are_all_traj_in_collision = true; auto now = this->now(); auto trajectory_distances_to_closest_obstacle = - get_trajectory_distances_from_map(trajectory_candidates); + this->get_trajectory_distances_to_closest_obstacle(trajectory_candidates); + + visualization_msgs::msg::MarkerArray traj_lib_marker_arr; - visualization_msgs::msg::MarkerArray traj_lib_markers, look_past_markers; + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Number of trajectories: " << trajectory_candidates.size()); for (size_t i = 0; i < trajectory_candidates.size(); ++i) { Trajectory traj = trajectory_candidates[i]; @@ -330,17 +271,20 @@ class DroanLocalPlanner : public rclcpp::Node { } catch (tf2::TransformException& ex) { RCLCPP_ERROR(this->get_logger(), "Failed to transform global plan to trajectory frame: %s", ex.what()); - return true; + return {false, best_traj_ret}; } - // for each waypoint in the trajectory, calculate the distance to the closest obstacle - for (size_t j = 0; j < traj.waypoint_count(); j++) { + // for each waypoint in the trajectory, fetch its distance to its closest obstacle + for (size_t j = 0; j < traj.get_num_waypoints(); j++) { Waypoint wp = traj.get_waypoint(j); - airstack_msgs::msg::Odometry odom = wp.odometry(now, traj.get_frame_id()); + airstack_msgs::msg::Odometry odom = wp.as_odometry_msg(now, traj.get_frame_id()); geometry_msgs::msg::PoseStamped pose; pose.header = odom.header; pose.pose = odom.pose; + double waypoint_obst_dist = trajectory_distances_to_closest_obstacle.at(i).at(j); + std::cout << "trajectory " << i << " waypoint " << j << " obstacle distance " + << waypoint_obst_dist << std::endl; closest_obstacle_distance = std::min(closest_obstacle_distance, trajectory_distances_to_closest_obstacle.at(i).at(j)); @@ -349,13 +293,13 @@ class DroanLocalPlanner : public rclcpp::Node { Waypoint closest_point_from_global_plan(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); int wp_index; double path_distance; - bool valid = global_plan_in_traj_frame.get_closest_point( + bool is_valid = global_plan_in_traj_frame.get_closest_point( wp.position(), &closest_point_from_global_plan, &wp_index, &path_distance); // reward making progress along the global plan double forward_progress_reward = -forward_progress_reward_weight * path_distance; - if (valid) { + if (is_valid) { if (!std::isfinite(avg_distance_from_global_plan)) { avg_distance_from_global_plan = 0; } @@ -364,136 +308,161 @@ class DroanLocalPlanner : public rclcpp::Node { forward_progress_reward; } } - avg_distance_from_global_plan /= traj.waypoint_count(); - - bool collision = closest_obstacle_distance <= robot_radius; - if (!collision) { - all_in_collision = false; + avg_distance_from_global_plan /= traj.get_num_waypoints(); + + bool is_collision = closest_obstacle_distance <= robot_radius; + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Trajectory " << i << " is_collision: " << is_collision + // << " closest_obstacle_distance: " + // << closest_obstacle_distance); + if (!is_collision) { + are_all_traj_in_collision = false; } + std::string marker_ns = "trajectory_" + std::to_string(i); + visualization_msgs::msg::MarkerArray traj_markers; - if (collision) { + if (is_collision) { // red for collision - traj_markers = traj.get_markers(this->now(), 1, 0, 0, .5); + traj_markers = traj.get_markers(this->now(), marker_ns, 1, 0, 0, .3); } else { // green for no collision - traj_markers = traj.get_markers(this->now(), 0, 1, 0, .5); + traj_markers = traj.get_markers(this->now(), marker_ns, 0, 1, 0, .5); } + traj_lib_marker_arr.markers.insert(traj_lib_marker_arr.markers.end(), + traj_markers.markers.begin(), + traj_markers.markers.end()); - // if look_past_distance is set, then use that to calculate the cost instead - // wtf this doesn't even make sense - 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 direction = curr_wp.position() - prev_wp.position(); - direction.normalize(); - - tf2::Vector3 look_past_position = - curr_wp.position() + look_past_distance * direction; - - Waypoint closest_point_from_global_plan(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - int wp_index; - bool valid = global_plan_in_traj_frame.get_closest_point( - look_past_position, &closest_point_from_global_plan, &wp_index); - - if (!valid) { - collision = true; - } else { - avg_distance_from_global_plan = - look_past_position.distance(closest_point_from_global_plan.position()); - } - - 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 = look_past_position.x(); - marker.pose.position.y = look_past_position.y(); - marker.pose.position.z = look_past_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()); // bigger distance from obstacles makes the cost smaller (more negative). cap by the // obstacle check radius double cost = avg_distance_from_global_plan - obstacle_distance_reward * std::min(closest_obstacle_distance, obstacle_check_radius); - if (!collision && cost < min_cost) { + if (!is_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; + + traj_lib_vis_pub->publish(traj_lib_marker_arr); + map_debug_pub->publish(this->map_representation->get_debug_markerarray()); + + bool is_success = !are_all_traj_in_collision; + return {is_success, best_traj_ret}; + } + + /** + * @brief Get the distance to the closest obstacle for each waypoint in the trajectory + */ + std::vector> get_trajectory_distances_to_closest_obstacle( + const std::vector& trajectory_candidates) { + // TODO: clearly we should refactor map_representation to accept Trajectory objects + std::vector> trajectory_distances_to_closest_obstacle = + this->map_representation->get_values(trajectory_candidates); + return trajectory_distances_to_closest_obstacle; + } + + /** + * @brief Applies a smoothing filter to the yaw of the trajectory + */ + void apply_smooth_yaw(airstack_msgs::msg::TrajectoryXYZVYaw& best_traj_msg) { + bool found_initial_heading = false; + double initial_heading = 0; + try { + tf2::Stamped transform; + tf_buffer_ptr->canTransform( + best_traj_msg.header.frame_id, look_ahead_odom.header.frame_id, + look_ahead_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform(best_traj_msg.header.frame_id, + look_ahead_odom.header.frame_id, + look_ahead_odom.header.stamp); + tf2::fromMsg(transform_msg, transform); + + transform.setOrigin(tf2::Vector3(0, 0, 0)); // only care about rotation + initial_heading = + tf2::getYaw(transform * tflib::to_tf(look_ahead_odom.pose.orientation)); + + found_initial_heading = true; + + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); } - vis_pub->publish(traj_lib_markers); - map_representation->publish_debug(); - look_past_vis_pub->publish(look_past_markers); + if (found_initial_heading) { + best_traj_msg.waypoints[0].yaw = initial_heading; + double alpha = 0.1; + double sin_yaw_prev = sin(best_traj_msg.waypoints[0].yaw); + double cos_yaw_prev = cos(best_traj_msg.waypoints[0].yaw); + + for (size_t i = 1; i < best_traj_msg.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw wp_prev = best_traj_msg.waypoints[i - 1]; + airstack_msgs::msg::WaypointXYZVYaw& wp_curr = best_traj_msg.waypoints[i]; - return all_in_collision; + 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; + } + } } // subscriber callbacks - void global_plan_callback( - const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr global_plan_msg) { - RCLCPP_INFO_STREAM(this->get_logger(), "GOT GLOBAL PLAN, goal_mode: " << goal_mode); - if (goal_mode != TRAJECTORY) return; + + /** + * @brief Saves the global plan if the goal mode is GLOBAL_PLAN + * + * @param global_plan_msg + */ + void global_plan_callback(const nav_msgs::msg::Path::SharedPtr global_plan_msg) { + RCLCPP_INFO_STREAM(this->get_logger(), "GOT GLOBAL PLAN, goal_mode: " << this->goal_mode); + if (this->goal_mode != GLOBAL_PLAN) return; this->global_plan_msg = *global_plan_msg; // copies - is_global_plan_received = true; - global_plan_trajectory_distance = 0; + this->is_global_plan_received = true; + this->global_plan_trajectory_distance = 0; } + /** + * @brief If mode == AUTO_WAYPOINT, then the local planner will automatically interpolate + * waypoints to reach the desired waypoint. This callback happens when mode == AUTO_WAYPOINT and + * it receives a waypoint message. + * + * @param wp the desired waypoint + */ void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr wp) { - if (goal_mode != AUTO_WAYPOINT) { - waypoint_buffer.clear(); - waypoint_buffer.push_back(*wp); + if (this->goal_mode != AUTO_WAYPOINT) { + this->waypoint_buffer.clear(); + this->waypoint_buffer.push_back(*wp); return; } // remove old waypoints if necessary - waypoint_buffer.push_back(*wp); + this->waypoint_buffer.push_back(*wp); if ((rclcpp::Time(wp->header.stamp) - rclcpp::Time(waypoint_buffer.front().header.stamp)) - .seconds() > waypoint_buffer_duration) { + .seconds() > auto_waypoint_buffer_duration) { waypoint_buffer.pop_front(); } // stitch together the history of waypoints - airstack_msgs::msg::TrajectoryXYZVYaw global_plan_msg; + nav_msgs::msg::Path global_plan_msg; global_plan_msg.header.frame_id = wp->header.frame_id; global_plan_msg.header.stamp = wp->header.stamp; - std::vector backwards_global_plan; + 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; + geometry_msgs::msg::PoseStamped waypoint; + waypoint.pose.position.x = curr_wp.point.x; + waypoint.pose.position.y = curr_wp.point.y; + waypoint.pose.position.z = curr_wp.point.z; if (prev_wps.size() > 1) { geometry_msgs::msg::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; @@ -514,8 +483,8 @@ class DroanLocalPlanner : public rclcpp::Node { // 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) { + if (distance >= auto_waypoint_spacing_threshold && + angle_diff < auto_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); @@ -529,7 +498,7 @@ class DroanLocalPlanner : public rclcpp::Node { geometry_msgs::msg::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) { + if (distance >= auto_waypoint_spacing_threshold) { prev_wps.push_back(curr_wp); backwards_global_plan.push_back(waypoint); } @@ -537,15 +506,16 @@ class DroanLocalPlanner : public rclcpp::Node { } } for (size_t i = 0; i < backwards_global_plan.size(); i++) { - global_plan_msg.waypoints.push_back( + global_plan_msg.poses.push_back( backwards_global_plan[backwards_global_plan.size() - 1 - i]); } if (is_tracking_point_received) { try { tf2::Stamped transform; - tf_buffer.canTransform(tracking_point_odom.header.frame_id, wp->header.frame_id, - wp->header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto transform_msg = tf_buffer.lookupTransform( + tf_buffer_ptr->canTransform(tracking_point_odom.header.frame_id, + wp->header.frame_id, wp->header.stamp, + rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform( tracking_point_odom.header.frame_id, wp->header.frame_id, wp->header.stamp); tf2::fromMsg(transform_msg, transform); @@ -555,15 +525,15 @@ class DroanLocalPlanner : public rclcpp::Node { tf2::Vector3 direction = (wp_position - tp_position).normalized() * 3; tf2::Vector3 wp2_position = wp_position + direction; - airstack_msgs::msg::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_msg.waypoints.push_back(wp1); - global_plan_msg.waypoints.push_back(wp2); + geometry_msgs::msg::PoseStamped wp1, wp2; + wp1.pose.position.x = wp_position.x(); + wp1.pose.position.y = wp_position.y(); + wp1.pose.position.z = wp_position.z(); + wp2.pose.position.x = wp2_position.x(); + wp2.pose.position.y = wp2_position.y(); + wp2.pose.position.z = wp2_position.z(); + global_plan_msg.poses.push_back(wp1); + global_plan_msg.poses.push_back(wp2); } catch (tf2::TransformException& ex) { RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); } @@ -574,33 +544,40 @@ class DroanLocalPlanner : public rclcpp::Node { this->is_global_plan_received = true; } + /** + * @brief Clears the current global plan reference and go immediately to a custom waypoint + * (typically set by the user). Sets the global plan to be the line between the current look + * ahead point and the custom waypoint. + * + * @param wp + */ void custom_waypoint_callback(const geometry_msgs::msg::PoseStamped::SharedPtr wp) { if (!is_look_ahead_received) return; try { tf2::Stamped transform; - tf_buffer.canTransform(look_ahead_odom.header.frame_id, wp->header.frame_id, - wp->header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto transform_msg = tf_buffer.lookupTransform(look_ahead_odom.header.frame_id, - wp->header.frame_id, wp->header.stamp); + tf_buffer_ptr->canTransform(look_ahead_odom.header.frame_id, wp->header.frame_id, + wp->header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform( + look_ahead_odom.header.frame_id, wp->header.frame_id, wp->header.stamp); tf2::fromMsg(transform_msg, transform); tf2::Vector3 la_position = tflib::to_tf(look_ahead_odom.pose.position); tf2::Vector3 wp_position = transform * tflib::to_tf(wp->pose.position); - airstack_msgs::msg::TrajectoryXYZVYaw global_plan_msg; + nav_msgs::msg::Path global_plan_msg; global_plan_msg.header.frame_id = look_ahead_odom.header.frame_id; global_plan_msg.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_msg.waypoints.push_back(wp1); - global_plan_msg.waypoints.push_back(wp2); + geometry_msgs::msg::PoseStamped wp1, wp2; + wp1.pose.position.x = la_position.x(); + wp1.pose.position.y = la_position.y(); + wp1.pose.position.z = la_position.z(); + wp2.pose.position.x = wp_position.x(); + wp2.pose.position.y = wp_position.y(); + wp2.pose.position.z = wp_position.z(); + global_plan_msg.poses.push_back(wp1); + global_plan_msg.poses.push_back(wp2); global_plan_trajectory_distance = 0; this->global_plan_msg = global_plan_msg; @@ -617,15 +594,15 @@ class DroanLocalPlanner : public rclcpp::Node { */ void update_waypoint_mode() { if (goal_mode == CUSTOM_WAYPOINT) { - if (global_plan_msg.waypoints.size() < 2) goal_mode = AUTO_WAYPOINT; + if (global_plan_msg.poses.size() < 2) goal_mode = AUTO_WAYPOINT; // check if the time limit for reaching the waypoint has elapsed double elapsed_time = (this->now() - global_plan_msg.header.stamp).seconds(); double distance = 0; - for (size_t i = 1; i < global_plan_msg.waypoints.size(); i++) { - airstack_msgs::msg::WaypointXYZVYaw prev_wp, curr_wp; - prev_wp = global_plan_msg.waypoints[i - 1]; - curr_wp = global_plan_msg.waypoints[i]; + for (size_t i = 1; i < global_plan_msg.poses.size(); i++) { + geometry_msgs::msg::Pose prev_wp, curr_wp; + prev_wp = global_plan_msg.poses[i - 1].pose; + curr_wp = global_plan_msg.poses[i].pose; distance += sqrt(pow(prev_wp.position.x - curr_wp.position.x, 2) + pow(prev_wp.position.y - curr_wp.position.y, 2)); @@ -633,7 +610,7 @@ class DroanLocalPlanner : public rclcpp::Node { // ROS_INFO_STREAM("elapsed: " << elapsed_time << " / " << // distance/custom_waypoint_timeout_factor << " distance: " << distance); - if (elapsed_time >= distance / custom_waypoint_timeout_factor) { + if (elapsed_time >= distance / this->custom_waypoint_timeout_factor) { // ROS_INFO_STREAM("CUSTOM WAYPOINT TIMEOUT REACHED"); goal_mode = AUTO_WAYPOINT; } @@ -642,10 +619,10 @@ class DroanLocalPlanner : public rclcpp::Node { if (is_tracking_point_received) { try { tf2::Stamped transform; - tf_buffer.canTransform( + tf_buffer_ptr->canTransform( tracking_point_odom.header.frame_id, global_plan_msg.header.frame_id, global_plan_msg.header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto transform_msg = tf_buffer.lookupTransform( + auto transform_msg = tf_buffer_ptr->lookupTransform( tracking_point_odom.header.frame_id, global_plan_msg.header.frame_id, global_plan_msg.header.stamp); tf2::fromMsg(transform_msg, transform); @@ -653,7 +630,7 @@ class DroanLocalPlanner : public rclcpp::Node { tf2::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.position); tp_position.setZ(0); tf2::Vector3 wp_position = - transform * tflib::to_tf(global_plan_msg.waypoints.back().position); + transform * tflib::to_tf(global_plan_msg.poses.back().pose.position); wp_position.setZ(0); if (tp_position.distance(wp_position) < custom_waypoint_distance_threshold) { @@ -668,12 +645,21 @@ class DroanLocalPlanner : public rclcpp::Node { } } } + /** + * @brief Receive the look ahead point to plan the next local trajectory from + * + * @param odom + */ void look_ahead_callback(const airstack_msgs::msg::Odometry::SharedPtr odom) { is_look_ahead_received = true; + RCLCPP_INFO_STREAM_ONCE(this->get_logger(), + "look ahead received with frame id: " << odom->header.frame_id); look_ahead_odom = *odom; } void tracking_point_callback(const airstack_msgs::msg::Odometry::SharedPtr odom) { is_tracking_point_received = true; + RCLCPP_INFO_STREAM_ONCE(this->get_logger(), + "tracking point received with frame id: " << odom->header.frame_id); tracking_point_odom = *odom; } }; diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml new file mode 100644 index 00000000..96efd061 --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml deleted file mode 100644 index c1445250..00000000 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.xml +++ /dev/null @@ -1,81 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp index a75ebf02..b1571589 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp @@ -4,7 +4,9 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + node->initialize(); + rclcpp::spin(node); rclcpp::shutdown(); return 0; } diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt index 22eee4e0..4e068f99 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt @@ -34,6 +34,10 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml new file mode 100644 index 00000000..8b80240e --- /dev/null +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + takeoff_height: 1.0 + high_takeoff_height: 2.0 + takeoff_landing_velocity: 0.3 + takeoff_acceptance_distance: 0.3 + takeoff_acceptance_time: 10.0 + landing_stationary_distance: 0.02 + landing_acceptance_time: 5.0 + landing_tracking_point_ahead_time: 5.0 + + takeoff_path_roll: 0. # in degrees + takeoff_path_pitch: 20. # in degrees + takeoff_path_relative_to_orientation: false diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml index 33bc6671..7b4a46a6 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml @@ -1,6 +1,6 @@ - - + + @@ -13,6 +13,6 @@ - + - + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp index 23153f9f..576e9a71 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp @@ -63,7 +63,8 @@ TakeoffLandingPlanner::TakeoffLandingPlanner() : rclcpp::Node("takeoff_landing_p high_takeoff_traj_gen = new TakeoffTrajectory(high_takeoff_height, takeoff_landing_velocity, takeoff_path_roll, takeoff_path_pitch, takeoff_path_relative_to_orientation); - landing_traj_gen = new TakeoffTrajectory(-10000., takeoff_landing_velocity); + // TODO: this landing point is hardcoded. it should be parameterized + landing_traj_gen = new TakeoffTrajectory(0., takeoff_landing_velocity); current_command = airstack_msgs::srv::TakeoffLandingCommand::Request::NONE; completion_percentage = 0.f; diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt index c718964f..b582b711 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(tf2_ros REQUIRED) find_package(airstack_msgs REQUIRED) find_package(airstack_common REQUIRED) -add_library(trajectory_library src/trajectory_library.cpp) +add_library(trajectory_library SHARED src/trajectory_library.cpp) target_include_directories(trajectory_library PUBLIC $ $) diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml index 333e47e3..b63102ac 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml @@ -1,1310 +1,1298 @@ +--- trajectories: - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 0. - dt: $(param dt) + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 22.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 67.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 112.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 157.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -22.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -67.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -112.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -157.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 22.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 67.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 112.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 157.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -22.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -67.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -112.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -157.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 22.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 67.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 112.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 157.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -22.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -67.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -112.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -157.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 22.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 67.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 112.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 157.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -22.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -67.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -112.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -157.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 22.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 67.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 112.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 157.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -22.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -67.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -112.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -157.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht) max_velocity: $(param max_velocity) - - - - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 22.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 67.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 112.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: 157.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -22.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -67.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -112.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: 0. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 0 magnitude_yaw: -157.5 - magnitude_pitch: 0. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 22.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 67.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 112.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: 157.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -22.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -67.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -112.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: 15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 15 magnitude_yaw: -157.5 - magnitude_pitch: 15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 22.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 67.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 112.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: 157.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -22.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -67.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -112.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: -15. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -15 magnitude_yaw: -157.5 - magnitude_pitch: -15. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 22.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 67.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 112.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: 157.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -22.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -67.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -112.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: 30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: 30 magnitude_yaw: -157.5 - magnitude_pitch: 30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 22.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 45. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 67.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 90. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 112.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 135. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: 157.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 180. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -22.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -45. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -67.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -90. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -112.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: -135. - magnitude_pitch: -30. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 max_velocity: $(param max_velocity) - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) magnitude: $(param magnitude) + magnitude_pitch: -30 magnitude_yaw: -157.5 - magnitude_pitch: -30. - dt: $(param dt) - ht: $(param ht_long) max_velocity: $(param max_velocity) - - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: 90. - dt: $(param dt) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) - max_velocity: 0.3 - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized magnitude: $(param magnitude) - magnitude_yaw: 0. - magnitude_pitch: -90. - dt: $(param dt) + magnitude_pitch: 90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht_long) - max_velocity: 0.3 \ No newline at end of file + magnitude: $(param magnitude) + magnitude_pitch: -90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/test.yaml b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/test.yaml index 5299c060..b63102ac 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/test.yaml +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/config/test.yaml @@ -1,9 +1,1298 @@ +--- trajectories: - - type: acceleration - frame: $(param tf_prefix)/look_ahead_point_stabilized - magnitude: 1. - magnitude_yaw: 0. - magnitude_pitch: 0. - dt: $(param dt) + - dt: $(param dt) + frame: look_ahead_point_stabilized ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp index f8ed7b88..7eb8c56f 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp @@ -12,14 +12,13 @@ #include #include #include -// #include -// #include #include #include #include #include #include +#include #include class Trajectory; @@ -32,29 +31,34 @@ class Waypoint { Waypoint(double x, double y, double z, double yaw, double vx, double vy, double vz, double ax, double ay, double az, double jx, double jy, double jz, double time = 0); - double x() const { return x_; } - double y() const { return y_; } - double z() const { return z_; } - double vx() const { return vx_; } - double vy() const { return vy_; } - double vz() const { return vz_; } - double ax() const { return ax_; } - double ay() const { return ay_; } - double az() const { return az_; } - double jx() const { return jx_; } - double jy() const { return jy_; } - double jz() const { return jz_; } - double yaw() const { return yaw_; } - double time() const { return time_; } + double get_x() const { return x_; } + double get_y() const { return y_; } + double get_z() const { return z_; } + double get_vx() const { return vx_; } + double get_vy() const { return vy_; } + double get_vz() const { return vz_; } + double get_ax() const { return ax_; } + double get_ay() const { return ay_; } + double get_az() const { return az_; } + double get_jx() const { return jx_; } + double get_jy() const { return jy_; } + double get_jz() const { return jz_; } + double get_yaw() const { return yaw_; } + double get_time() const { return time_; } void set_time(double time) { time_ = time; } - tf2::Quaternion q() const; + tf2::Quaternion quaternion() const; tf2::Vector3 position() const; tf2::Vector3 velocity() const; tf2::Vector3 acceleration() const; tf2::Vector3 jerk() const; - airstack_msgs::msg::Odometry odometry(rclcpp::Time stamp, std::string frame_id) const; + + /** + * @brief Convert Waypoint to Odometry message + */ + airstack_msgs::msg::Odometry as_odometry_msg(rclcpp::Time stamp, std::string frame_id) const; + Waypoint interpolate(Waypoint wp, double t); friend class Trajectory; @@ -63,6 +67,7 @@ class Waypoint { }; // Create one TransformListener instance to be used by every Trajectory instance +// TODO: change to smart pointer static tf2_ros::Buffer* buffer = NULL; static tf2_ros::TransformListener* listener = NULL; @@ -81,8 +86,9 @@ class Trajectory { public: Trajectory(); - Trajectory(rclcpp::Node* node, std::string frame_id); - Trajectory(rclcpp::Node* node, airstack_msgs::msg::TrajectoryXYZVYaw path); + Trajectory(rclcpp::Node* node_ptr, std::string frame_id); + Trajectory(rclcpp::Node* node_ptr, airstack_msgs::msg::TrajectoryXYZVYaw path); + Trajectory(rclcpp::Node* node_ptr, nav_msgs::msg::Path path); // Trajectory(airstack_msgs::msg::TrajectoryXYZVYaw path); void clear(); @@ -100,26 +106,30 @@ class Trajectory { Waypoint* waypoint, Waypoint* end_waypoint); bool get_waypoint(double time, Waypoint* waypoint); + const std::vector& get_waypoints() const; + double get_duration(); bool get_odom(double time, airstack_msgs::msg::Odometry* odom, rclcpp::Time stamp); Trajectory to_frame(std::string target_frame, rclcpp::Time time); // Trajectory respace(double spacing); // Trajectory shorten(double new_length); // replace shorten with get_subtraj_dist - Trajectory get_subtrajectory_distance(double start, double end); + Trajectory trim_trajectory_between_distances(double start_dist, double end_dist); Trajectory get_reversed_trajectory(); float get_skip_ahead_time(float start_time, float max_velocity, float max_distance); void set_fixed_height(double height); - size_t waypoint_count(); - Waypoint get_waypoint(int index); - std::string get_frame_id(); - airstack_msgs::msg::TrajectoryXYZVYaw get_TrajectoryXYZVYaw(); - std::vector get_vector_PointStamped(); - - visualization_msgs::msg::MarkerArray get_markers(rclcpp::Time stamp, float r = 1, float g = 0, - float b = 0, float a = 1, - bool show_poses = false, + size_t get_num_waypoints() const; + const Waypoint& get_waypoint(int index) const; + const std::string& get_frame_id() const; + const rclcpp::Time& get_stamp() const { return stamp; } + airstack_msgs::msg::TrajectoryXYZVYaw get_TrajectoryXYZVYaw_msg(); + std::vector get_vector_PointStamped_msg(); + + visualization_msgs::msg::MarkerArray get_markers(rclcpp::Time stamp, + const std::string& marker_namespace, + float r = 1, float g = 0, float b = 0, + float a = 1, bool show_poses = false, bool show_velocity = false, float thickness = 0.03f); }; @@ -202,11 +212,18 @@ class TrajectoryLibrary { private: std::vector static_trajectories; std::vector dynamic_trajectories; - // tf2_ros::Buffer* buffer; - rclcpp::Node* node; + rclcpp::Node::SharedPtr node_ptr; - // ros::NodeHandle* pnh; + public: + TrajectoryLibrary(std::string config_filename, + rclcpp::Node::SharedPtr node_ptr); // tf2_ros::Buffer* + + std::vector get_static_trajectories(); + std::vector get_dynamic_trajectories(airstack_msgs::msg::Odometry odom); + visualization_msgs::msg::MarkerArray get_markers( + std::vector trajectories); + private: std::string trim(std::string str) { int trim_start = 0; int trim_end = 0; @@ -234,10 +251,10 @@ class TrajectoryLibrary { std::string get_string(std::string t) { return t; } template - T parse(YAML::Node node) { + T parse(YAML::Node yamlnode) { // std::cout << "trim test |" << trim(" asdjfaksfdj jdfkj jakdsf ") << "|" << std::endl; - std::string str = node.as(); + std::string str = yamlnode.as(); std::size_t start; std::string param_label = "$(param"; @@ -247,26 +264,28 @@ class TrajectoryLibrary { std::size_t end = str.find(")"); // std::cout << start << " " << end << std::endl; if (end == std::string::npos) { - std::cout << "TRAJECTORY LIBRARY PARSING ERROR: No closing parenthesis for $(param"; + RCLCPP_DEBUG_STREAM( + node_ptr->get_logger(), + "TRAJECTORY LIBRARY PARSING ERROR: No closing parenthesis for $(param"); break; } std::string parameter_name = trim(str.substr(start + param_label.size(), end - (start + param_label.size()))); - // std::cout << "parameter_name: |" << parameter_name << "|" << std::endl; + RCLCPP_DEBUG(node_ptr->get_logger(), "parameter_name: %s", parameter_name.c_str()); T parameter_value; - bool set; - airstack::get_param(this->node, parameter_name, T(), &set); - if (!set) { //! pnh->getParam(parameter_name, parameter_value)){ - std::cout << "Couldn't find parameter '" << parameter_name - << "'. This is either because it doesn't exist or the type is incorrect " - "in the launch file."; + if (!this->node_ptr->get_parameter(parameter_name, parameter_value)) { + RCLCPP_INFO(node_ptr->get_logger(), + "Couldn't find parameter '%s'. This is either because it doesn't exist " + "or the type is incorrect in the launch file.", + parameter_name.c_str()); } - - // std::cout << str << " " << start << " " << end << std::endl; + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), "parameter_value: " << parameter_value); + std::cout << str << " " << start << " " << end << std::endl; str = str.substr(0, start) + get_string(parameter_value) + str.substr(end + 1, str.size() - (end + 1)); - // std::cout << str << " " << str.find(param_label) << std::endl; + std::cout << str << " " << str.find(param_label) << std::endl; + RCLCPP_DEBUG(node_ptr->get_logger(), "str: %s", str.c_str()); } // std::cout << "final: " << str << std::endl; @@ -281,15 +300,6 @@ class TrajectoryLibrary { return n["key"].as(); } - - public: - TrajectoryLibrary(std::string config_filename, rclcpp::Node* node); // tf2_ros::Buffer* - // buffer); - - std::vector get_static_trajectories(); - std::vector get_dynamic_trajectories(airstack_msgs::msg::Odometry odom); - visualization_msgs::msg::MarkerArray get_markers( - std::vector trajectories); }; #endif diff --git a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp index 9dfc5042..9e71f5ad 100644 --- a/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp +++ b/ros_ws/src/robot/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp @@ -1,6 +1,4 @@ #include -// #include -// #include Waypoint::Waypoint(double x, double y, double z, double yaw, double vx, double vy, double vz, double ax, double ay, double az, double jx, double jy, double jz, double time) { @@ -20,7 +18,7 @@ Waypoint::Waypoint(double x, double y, double z, double yaw, double vx, double v time_ = time; } -tf2::Quaternion Waypoint::q() const { +tf2::Quaternion Waypoint::quaternion() const { tf2::Quaternion quat; quat.setRPY(0, 0, yaw_); return quat; @@ -34,7 +32,8 @@ tf2::Vector3 Waypoint::acceleration() const { return tf2::Vector3(ax_, ay_, az_) tf2::Vector3 Waypoint::jerk() const { return tf2::Vector3(jx_, jy_, jz_); } -airstack_msgs::msg::Odometry Waypoint::odometry(rclcpp::Time stamp, std::string frame_id) const { +airstack_msgs::msg::Odometry Waypoint::as_odometry_msg(rclcpp::Time stamp, + std::string frame_id) const { airstack_msgs::msg::Odometry odom; odom.header.stamp = stamp; odom.header.frame_id = frame_id; @@ -44,7 +43,7 @@ airstack_msgs::msg::Odometry Waypoint::odometry(rclcpp::Time stamp, std::string odom.pose.position.y = y_; odom.pose.position.z = z_; - tf2::Quaternion quat = q(); + tf2::Quaternion quat = quaternion(); odom.pose.orientation.x = quat.x(); odom.pose.orientation.y = quat.y(); odom.pose.orientation.z = quat.z(); @@ -67,11 +66,11 @@ airstack_msgs::msg::Odometry Waypoint::odometry(rclcpp::Time stamp, std::string Waypoint Waypoint::interpolate(Waypoint wp, double t) { tf2::Vector3 pos_interp = position() + t * (wp.position() - position()); - tf2::Quaternion q_interp = q().slerp(wp.q(), t); + tf2::Quaternion q_interp = quaternion().slerp(wp.quaternion(), t); tf2::Vector3 vel_interp = velocity() + t * (wp.velocity() - velocity()); tf2::Vector3 accel_interp = acceleration() + t * (wp.acceleration() - acceleration()); tf2::Vector3 jerk_interp = jerk() + t * (wp.jerk() - jerk()); - double time_interp = time() + t * (wp.time() - time()); + double time_interp = get_time() + t * (wp.get_time() - get_time()); Waypoint wp_interp(pos_interp.x(), pos_interp.y(), pos_interp.z(), tf2::getYaw(q_interp), vel_interp.x(), vel_interp.y(), vel_interp.z(), accel_interp.x(), @@ -82,58 +81,66 @@ Waypoint Waypoint::interpolate(Waypoint wp, double t) { } std::ostream& operator<<(std::ostream& os, const Waypoint& wp) { - return os << "[pos: " << wp.x() << ", " << wp.y() << ", " << wp.z() << " vel: " << wp.vx() - << ", " << wp.vy() << ", " << wp.vz() << " acc: " << wp.ax() << ", " << wp.ay() - << ", " << wp.az() << " jerk: " << wp.jx() << ", " << wp.jy() << ", " << wp.jz() - << " yaw: " << wp.yaw() << " time: " << wp.time(); + return os << "[pos: " << wp.get_x() << ", " << wp.get_y() << ", " << wp.get_z() + << " vel: " << wp.get_vx() << ", " << wp.get_vy() << ", " << wp.get_vz() + << " acc: " << wp.get_ax() << ", " << wp.get_ay() << ", " << wp.get_az() + << " jerk: " << wp.get_jx() << ", " << wp.get_jy() << ", " << wp.get_jz() + << " yaw: " << wp.get_yaw() << " time: " << wp.get_time(); } -static int trajectory_class_counter = 0; +Trajectory::Trajectory() { generated_waypoint_times = false; } -Trajectory::Trajectory() { - // listener = NULL; - generated_waypoint_times = false; - - // init marker namespace to be unique - std::stringstream ss; - ss << "trajectory_" << trajectory_class_counter; - marker_namespace = ss.str(); - trajectory_class_counter++; -} - -Trajectory::Trajectory(rclcpp::Node* node, std::string frame_id) { +Trajectory::Trajectory(rclcpp::Node* node_ptr, std::string frame_id) : Trajectory() { if (buffer == NULL) { - buffer = new tf2_ros::Buffer(node->get_clock()); + buffer = new tf2_ros::Buffer(node_ptr->get_clock()); listener = new tf2_ros::TransformListener(*buffer); } this->frame_id = frame_id; - // listener = NULL; - generated_waypoint_times = false; - - // init marker namespace to be unique - std::stringstream ss; - ss << "trajectory_" << trajectory_class_counter; - marker_namespace = ss.str(); - trajectory_class_counter++; } -Trajectory::Trajectory(rclcpp::Node* node, airstack_msgs::msg::TrajectoryXYZVYaw path) { - if (buffer == NULL) { - buffer = new tf2_ros::Buffer(node->get_clock()); - listener = new tf2_ros::TransformListener(*buffer); +/** + * @brief Construct a new Trajectory:: Trajectory object + * Converts a nav_msgs::Path to a Trajectory object. + * Only the position of each waypoint is used. + * The velocity of each waypoint is set to 0. + * TODO: add a time component to the trajectory? + * + * @param node_ptr + * @param path + */ +Trajectory::Trajectory(rclcpp::Node* node_ptr, nav_msgs::msg::Path path) + : Trajectory(node_ptr, path.header.frame_id) { + this->stamp = path.header.stamp; + + // remove consecutive waypoints + nav_msgs::msg::Path cleaned_path; + cleaned_path.header = path.header; + for (size_t i = 0; i < path.poses.size(); i++) { + geometry_msgs::msg::PoseStamped pose = path.poses[i]; + if (i == 0) + cleaned_path.poses.push_back(pose); + else if (!(pose.pose.position.x == cleaned_path.poses.back().pose.position.x && + pose.pose.position.y == cleaned_path.poses.back().pose.position.y && + pose.pose.position.z == cleaned_path.poses.back().pose.position.z)) + cleaned_path.poses.push_back(pose); } - frame_id = path.header.frame_id; - stamp = path.header.stamp; - // listener = NULL; - generated_waypoint_times = false; + // translate to Waypoint objects + for (size_t i = 0; i < cleaned_path.poses.size(); i++) { + geometry_msgs::msg::PoseStamped pose = cleaned_path.poses[i]; - // init marker namespace to be unique - std::stringstream ss; - ss << "trajectory_" << trajectory_class_counter; - marker_namespace = ss.str(); - trajectory_class_counter++; + Waypoint waypoint(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0); + this->waypoints.push_back(waypoint); + } +} + +Trajectory::Trajectory(rclcpp::Node* node_ptr, airstack_msgs::msg::TrajectoryXYZVYaw path) + : Trajectory(node_ptr, path.header.frame_id) + +{ + this->stamp = path.header.stamp; // remove any consecutive duplicate waypoints airstack_msgs::msg::TrajectoryXYZVYaw cleaned_path; @@ -148,8 +155,7 @@ Trajectory::Trajectory(rclcpp::Node* node, airstack_msgs::msg::TrajectoryXYZVYaw cleaned_path.waypoints.push_back(wp); } - // ROS_INFO_STREAM("TRAJECTORY CONSTRUCTOR"); - // ROS_INFO_STREAM("cleaned path size: " << cleaned_path.waypoints.size()); + // translate to Waypoint objects for (size_t i = 0; i < cleaned_path.waypoints.size(); i++) { airstack_msgs::msg::WaypointXYZVYaw wp = cleaned_path.waypoints[i]; @@ -157,8 +163,8 @@ Trajectory::Trajectory(rclcpp::Node* node, airstack_msgs::msg::TrajectoryXYZVYaw tf2::Vector3 wp1(wp.position.x, wp.position.y, wp.position.z); tf2::Vector3 wp2(wp1); bool same = false; - if (i == - 0) { // for the first waypoint use the next waypoint to figure out direction of travel + // for the first waypoint use the next waypoint to figure out direction of travel + if (i == 0) { if (cleaned_path.waypoints.size() > 1) wp2 = tf2::Vector3(cleaned_path.waypoints[i + 1].position.x, cleaned_path.waypoints[i + 1].position.y, @@ -187,7 +193,7 @@ Trajectory::Trajectory(rclcpp::Node* node, airstack_msgs::msg::TrajectoryXYZVYaw Waypoint waypoint(wp.position.x, wp.position.y, wp.position.z, wp.yaw, vel.x(), vel.y(), vel.z(), wp.acceleration.x, wp.acceleration.y, wp.acceleration.z, wp.jerk.x, wp.jerk.y, wp.jerk.z); - waypoints.push_back(waypoint); + this->waypoints.push_back(waypoint); } } /* @@ -201,6 +207,11 @@ void Trajectory::clear() { generated_waypoint_times = false; } +/** + * @brief Uses each waypoint's position and velocity to generate the expected time to reach each + * waypoint. + * + */ void Trajectory::generate_waypoint_times() { if (generated_waypoint_times) return; @@ -216,7 +227,7 @@ void Trajectory::generate_waypoint_times() { // ROS_INFO_STREAM(i << " wp vel: " << curr_wp.velocity().x() << ", " << // curr_wp.velocity().y() << ", " << curr_wp.velocity().z() // << " | " << prev_wp.velocity().x() << ", " << prev_wp.velocity().y() << ", " << - //prev_wp.velocity().z()); + // prev_wp.velocity().z()); double distance = curr_wp.position().distance(prev_wp.position()); if (distance == 0) { @@ -234,7 +245,7 @@ void Trajectory::generate_waypoint_times() { // time: " << (prev_wp.time() + distance/velocity) << " prev_time: " << prev_wp.time() << " // inc: " << (distance/velocity)); - curr_wp.set_time(prev_wp.time() + distance / velocity); + curr_wp.set_time(prev_wp.get_time() + distance / velocity); } generated_waypoint_times = true; @@ -344,7 +355,7 @@ bool Trajectory::merge(Trajectory traj, double min_time) { Trajectory transformed_traj; try { - transformed_traj = traj.to_frame(frame_id, stamp); + transformed_traj = traj.to_frame(frame_id, this->stamp); } catch (tf2::TransformException& ex) { std::cout << "Transform exception while merging trajectories: " << ex.what(); } @@ -361,9 +372,10 @@ bool Trajectory::merge(Trajectory traj, double min_time) { // tf2::Vector3 closest_point; Waypoint closest_waypoint(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); int wp_index; - bool valid = get_closest_point(transformed_traj.waypoints[0].position(), &closest_waypoint, &wp_index); + bool valid = + get_closest_point(transformed_traj.waypoints[0].position(), &closest_waypoint, &wp_index); - if (closest_waypoint.time() >= min_time) { + if (closest_waypoint.get_time() >= min_time) { waypoints.erase(waypoints.begin() + wp_index + 1, waypoints.end()); waypoints.insert(waypoints.end(), transformed_traj.waypoints.begin(), transformed_traj.waypoints.end()); @@ -374,7 +386,7 @@ bool Trajectory::merge(Trajectory traj, double min_time) { return true; } - std::cout << "COULDN'T MERGE BECAUSE OF min_time: " << closest_waypoint.time() << " " + std::cout << "COULDN'T MERGE BECAUSE OF min_time: " << closest_waypoint.get_time() << " " << min_time; return false; } @@ -385,20 +397,20 @@ bool Trajectory::get_closest_waypoint(tf2::Vector3 point, double initial_time, d bool found = false; for (size_t i = 1; i < waypoints.size(); i++) { - if (waypoints[i].time() < initial_time) continue; - if (waypoints[i - 1].time() > end_time) break; + if (waypoints[i].get_time() < initial_time) continue; + if (waypoints[i - 1].get_time() > end_time) break; Waypoint wp_start = waypoints[i - 1]; Waypoint wp_end = waypoints[i]; // handle the case that the initial_time is between waypoint i-1 and waypoint i - if (wp_start.time() < initial_time) - wp_start = wp_start.interpolate( - wp_end, (initial_time - wp_start.time()) / (wp_end.time() - wp_start.time())); + if (wp_start.get_time() < initial_time) + wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); // handle the case that the end_time is between waypoint i-1 and waypoint i - if (wp_end.time() > end_time) - wp_end = wp_start.interpolate( - wp_end, (end_time - wp_start.time()) / (wp_end.time() - wp_start.time())); + if (wp_end.get_time() > end_time) + wp_end = wp_start.interpolate(wp_end, (end_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); // parameteric representation of segment between waypoints: segment_start + t*segment_vec tf2::Vector3 segment_start = wp_start.position(); @@ -427,15 +439,15 @@ bool Trajectory::get_waypoint_distance_ahead(double initial_time, double distanc double current_distance = 0; for (size_t i = 1; i < waypoints.size(); i++) { - if (waypoints[i].time() < initial_time) continue; + if (waypoints[i].get_time() < initial_time) continue; Waypoint wp_start = waypoints[i - 1]; Waypoint wp_end = waypoints[i]; // handle the case that the initial_time is between waypoint i-1 and waypoint i - if (wp_start.time() < initial_time) - wp_start = wp_start.interpolate( - wp_end, (initial_time - wp_start.time()) / (wp_end.time() - wp_start.time())); + if (wp_start.get_time() < initial_time) + wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); double segment_distance = wp_start.position().distance(wp_end.position()); if (current_distance + segment_distance >= distance) { @@ -445,7 +457,7 @@ bool Trajectory::get_waypoint_distance_ahead(double initial_time, double distanc } else current_distance += segment_distance; - if (i == waypoints.size() - 1 && wp_end.time() >= initial_time) { + if (i == waypoints.size() - 1 && wp_end.get_time() >= initial_time) { *waypoint = wp_end; return true; } @@ -463,15 +475,15 @@ bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ah // ROS_INFO("\n\n"); for (size_t i = 1; i < waypoints.size(); i++) { - if (waypoints[i].time() < initial_time) continue; + if (waypoints[i].get_time() < initial_time) continue; Waypoint wp_start = waypoints[i - 1]; Waypoint wp_end = waypoints[i]; // handle the case that the initial_time is between waypoint i-1 and waypoint i - if (wp_start.time() < initial_time) - wp_start = wp_start.interpolate( - wp_end, (initial_time - wp_start.time()) / (wp_end.time() - wp_start.time())); + if (wp_start.get_time() < initial_time) + wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); double segment_distance = wp_start.position().distance(wp_end.position()); bool should_break = false; @@ -480,22 +492,22 @@ bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ah wp_end = wp_start.interpolate( wp_end, (ahead_distance - current_path_distance) / segment_distance); if (end_waypoint != NULL) *end_waypoint = wp_end; - } else if (wp_end.time() > time_end) { + } else if (wp_end.get_time() > time_end) { should_break = true; - wp_end = wp_start.interpolate( - wp_end, (time_end - wp_start.time()) / (wp_end.time() - wp_start.time())); + wp_end = wp_start.interpolate(wp_end, (time_end - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); if (end_waypoint != NULL) *end_waypoint = wp_end; } else current_path_distance += segment_distance; // sphere line intersection equations: // http://www.ambrsoft.com/TrigoCalc/Sphere/SpherLineIntersection_.htm - double x1 = wp_start.x(); - double y1 = wp_start.y(); - double z1 = wp_start.z(); - double x2 = wp_end.x(); - double y2 = wp_end.y(); - double z2 = wp_end.z(); + double x1 = wp_start.get_x(); + double y1 = wp_start.get_y(); + double z1 = wp_start.get_z(); + double x2 = wp_end.get_x(); + double y2 = wp_end.get_y(); + double z2 = wp_end.get_z(); double xc = sphere_center.x(); double yc = sphere_center.y(); double zc = sphere_center.z(); @@ -548,13 +560,13 @@ bool Trajectory::get_waypoint(double time, Waypoint* waypoint) { generate_waypoint_times(); for (size_t i = 1; i < waypoints.size(); i++) { - if (waypoints[i].time() < time) continue; + if (waypoints[i].get_time() < time) continue; Waypoint wp_start = waypoints[i - 1]; Waypoint wp_end = waypoints[i]; *waypoint = wp_start.interpolate( - wp_end, (time - wp_start.time()) / (wp_end.time() - wp_start.time())); + wp_end, (time - wp_start.get_time()) / (wp_end.get_time() - wp_start.get_time())); return true; } @@ -571,16 +583,26 @@ double Trajectory::get_duration() { if (waypoints.size() == 0) return 0; - return waypoints.back().time(); + return waypoints.back().get_time(); } +/** + * @brief Get the expected odometry at a given time. Performs interpolation if the time is between + * waypoints. + * + * @param time + * @param odom + * @param stamp + * @return true + * @return false + */ bool Trajectory::get_odom(double time, airstack_msgs::msg::Odometry* odom, rclcpp::Time stamp) { generate_waypoint_times(); if (waypoints.size() == 0) { return false; } else if (waypoints.size() == 1) { - *odom = waypoints[0].odometry(stamp, frame_id); // rclcpp::Time::now(), frame_id); + *odom = waypoints[0].as_odometry_msg(stamp, frame_id); // rclcpp::Time::now(), frame_id); return true; } @@ -588,7 +610,7 @@ bool Trajectory::get_odom(double time, airstack_msgs::msg::Odometry* odom, rclcp Waypoint prev_wp = waypoints.front(); Waypoint curr_wp = waypoints.front(); for (size_t i = waypoints.size() - 1; i >= 1; i--) { - if (time >= waypoints[i - 1].time()) { + if (time >= waypoints[i - 1].get_time()) { prev_wp = waypoints[i - 1]; curr_wp = waypoints[i]; break; @@ -596,16 +618,16 @@ bool Trajectory::get_odom(double time, airstack_msgs::msg::Odometry* odom, rclcp } // figure out where we are in between the current and previous waypoints - double t = (time - prev_wp.time()) / (curr_wp.time() - prev_wp.time()); + double t = (time - prev_wp.get_time()) / (curr_wp.get_time() - prev_wp.get_time()); t = std::max(0.0, std::min(1.0, t)); // TODO check for nan // ROS_INFO_STREAM("t: " << t); *odom = prev_wp.interpolate(curr_wp, t) - .odometry(stamp, frame_id); // rclcpp::Time::now(), frame_id); + .as_odometry_msg(stamp, frame_id); // rclcpp::Time::now(), frame_id); // ROS_INFO_STREAM("odom: " << odom->pose.position.x << ", " << odom->pose.position.y << ", " << // odom->pose.position.z << " | " // << odom->twist.linear.x << ", " << odom->twist.linear.y << ", " << - //odom->twist.linear.z); + // odom->twist.linear.z); return true; } @@ -633,12 +655,12 @@ Trajectory Trajectory::to_frame(std::string target_frame, rclcpp::Time time) { tf2::Vector3 transformed_velocity = rot * wp.velocity(); tf2::Vector3 transformed_acceleration = rot * wp.acceleration(); tf2::Vector3 transformed_jerk = rot * wp.jerk(); - tf2::Quaternion transformed_q = transform * wp.q(); + tf2::Quaternion transformed_q = transform * wp.quaternion(); // ROS_INFO_STREAM("jerk: " << wp.jerk().x() << ", " << wp.jerk().y() << ", " << // wp.jerk().z() << " | " // << transformed_jerk.x() << ", " << transformed_jerk.y() << ", " << - //transformed_jerk.z()); + // transformed_jerk.z()); Waypoint transformed_wp(transformed_position.x(), transformed_position.y(), transformed_position.z(), tf2::getYaw(transformed_q), @@ -718,34 +740,42 @@ Trajectory Trajectory::shorten(double new_length){ return traj; }*/ -Trajectory Trajectory::get_subtrajectory_distance(double start, double end) { +/** + * @brief Trim a subtrajectory between a start distance and end distance + * + * @param start + * @param end + * @return Trajectory + */ +Trajectory Trajectory::trim_trajectory_between_distances(double start_dist, double end_dist) { Trajectory traj; - traj.stamp = stamp; - traj.frame_id = frame_id; + traj.stamp = this->stamp; + traj.frame_id = this->frame_id; - if (waypoints.size() > 0) { + if (this->waypoints.size() > 0) { double distance = 0; - if (start == 0. && waypoints.size() == 1) traj.waypoints.push_back(waypoints[0]); + if (start_dist == 0. && this->waypoints.size() == 1) + traj.waypoints.push_back(this->waypoints[0]); - for (size_t i = 1; i < waypoints.size(); i++) { - Waypoint prev_wp = waypoints[i - 1]; - Waypoint curr_wp = waypoints[i]; + for (size_t i = 1; i < this->waypoints.size(); i++) { + Waypoint prev_wp = this->waypoints[i - 1]; + Waypoint curr_wp = this->waypoints[i]; double segment_length = prev_wp.position().distance(curr_wp.position()); - if (start >= distance && start <= distance + segment_length) { + if (start_dist >= distance && start_dist <= distance + segment_length) { Waypoint interp_start_wp = - prev_wp.interpolate(curr_wp, (start - distance) / segment_length); + prev_wp.interpolate(curr_wp, (start_dist - distance) / segment_length); traj.waypoints.push_back(interp_start_wp); } - if (distance + segment_length > start && distance + segment_length < end) { + if (distance + segment_length > start_dist && distance + segment_length < end_dist) { traj.waypoints.push_back(curr_wp); } - if (end >= distance && end <= distance + segment_length) { + if (end_dist >= distance && end_dist <= distance + segment_length) { Waypoint interp_end_wp = - prev_wp.interpolate(curr_wp, (end - distance) / segment_length); + prev_wp.interpolate(curr_wp, (end_dist - distance) / segment_length); traj.waypoints.push_back(interp_end_wp); } @@ -760,8 +790,8 @@ Trajectory Trajectory::get_reversed_trajectory() { generated_waypoint_times = false; generate_waypoint_times(); Trajectory traj; - traj.frame_id = frame_id; - traj.stamp = stamp; + traj.frame_id = this->frame_id; + traj.stamp = this->stamp; traj.waypoints.assign(waypoints.begin(), waypoints.end()); std::reverse(traj.waypoints.begin(), traj.waypoints.end()); for (size_t i = 0; i < traj.waypoints.size(); i++) { @@ -788,7 +818,7 @@ float Trajectory::get_skip_ahead_time(float start_time, float max_velocity, floa float skip_time = start_time; for (size_t i = 1; i < waypoints.size(); i++) { - if (waypoints[i].time() < start_time) continue; + if (waypoints[i].get_time() < start_time) continue; Waypoint wp_start = waypoints[i - 1]; float wp_start_velocity = wp_start.velocity().length(); @@ -796,9 +826,9 @@ float Trajectory::get_skip_ahead_time(float start_time, float max_velocity, floa Waypoint wp_end = waypoints[i]; float wp_end_velocity = wp_end.velocity().length(); - if (wp_start.time() < start_time) - wp_start = wp_start.interpolate( - wp_end, (start_time - wp_start.time()) / (wp_end.time() - wp_start.time())); + if (wp_start.get_time() < start_time) + wp_start = wp_start.interpolate(wp_end, (start_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); double segment_distance = wp_start.position().distance(wp_end.position()); bool should_break = false; @@ -815,7 +845,7 @@ float Trajectory::get_skip_ahead_time(float start_time, float max_velocity, floa (wp_end_velocity - wp_start_velocity)))); } - skip_time = wp_end.time(); + skip_time = wp_end.get_time(); if (should_break) break; @@ -829,25 +859,27 @@ void Trajectory::set_fixed_height(double height) { for (size_t i = 0; i < waypoints.size(); i++) waypoints[i].z_ = height; } -size_t Trajectory::waypoint_count() { return waypoints.size(); } +size_t Trajectory::get_num_waypoints() const { return waypoints.size(); } + +const Waypoint& Trajectory::get_waypoint(int index) const { return waypoints[index]; } -Waypoint Trajectory::get_waypoint(int index) { return waypoints[index]; } +const std::vector& Trajectory::get_waypoints() const { return waypoints; } -std::string Trajectory::get_frame_id() { return frame_id; } +const std::string& Trajectory::get_frame_id() const { return frame_id; } -airstack_msgs::msg::TrajectoryXYZVYaw Trajectory::get_TrajectoryXYZVYaw() { +airstack_msgs::msg::TrajectoryXYZVYaw Trajectory::get_TrajectoryXYZVYaw_msg() { airstack_msgs::msg::TrajectoryXYZVYaw path; - path.header.stamp = stamp; - path.header.frame_id = frame_id; + path.header.stamp = this->stamp; + path.header.frame_id = this->frame_id; for (size_t i = 0; i < waypoints.size(); i++) { Waypoint wp = waypoints[i]; airstack_msgs::msg::WaypointXYZVYaw w; - w.position.x = wp.x(); - w.position.y = wp.y(); - w.position.z = wp.z(); - w.yaw = tf2::getYaw(wp.q()); + w.position.x = wp.get_x(); + w.position.y = wp.get_y(); + w.position.z = wp.get_z(); + w.yaw = tf2::getYaw(wp.quaternion()); w.velocity = wp.velocity().length(); path.waypoints.push_back(w); @@ -856,18 +888,18 @@ airstack_msgs::msg::TrajectoryXYZVYaw Trajectory::get_TrajectoryXYZVYaw() { return path; } -std::vector Trajectory::get_vector_PointStamped() { +std::vector Trajectory::get_vector_PointStamped_msg() { std::vector points; for (size_t i = 0; i < waypoints.size(); i++) { Waypoint wp = waypoints[i]; geometry_msgs::msg::PointStamped point; - point.header.stamp = stamp; - point.header.frame_id = frame_id; - point.point.x = wp.x(); - point.point.y = wp.y(); - point.point.z = wp.z(); + point.header.stamp = this->stamp; + point.header.frame_id = this->frame_id; + point.point.x = wp.get_x(); + point.point.y = wp.get_y(); + point.point.z = wp.get_z(); points.push_back(point); } @@ -875,9 +907,11 @@ std::vector Trajectory::get_vector_PointStampe return points; } -visualization_msgs::msg::MarkerArray Trajectory::get_markers(rclcpp::Time stamp, float r, float g, - float b, float a, bool show_poses, - bool show_velocity, float thickness) { +visualization_msgs::msg::MarkerArray Trajectory::get_markers(rclcpp::Time stamp, + const std::string& marker_namespace, + float r, float g, float b, float a, + bool show_poses, bool show_velocity, + float thickness) { visualization_msgs::msg::MarkerArray marker_array; // rclcpp::Time now = rclcpp::Time::now(); @@ -910,13 +944,13 @@ visualization_msgs::msg::MarkerArray Trajectory::get_markers(rclcpp::Time stamp, arrow.pose.position.x = wp.position().x(); arrow.pose.position.y = wp.position().y(); arrow.pose.position.z = wp.position().z(); - arrow.pose.orientation.x = wp.q().x(); - arrow.pose.orientation.y = wp.q().y(); - arrow.pose.orientation.z = wp.q().z(); - arrow.pose.orientation.w = wp.q().w(); - arrow.scale.x = 0.5; // length - arrow.scale.y = 0.1; // width - arrow.scale.z = 0.1; // height + arrow.pose.orientation.x = wp.quaternion().x(); + arrow.pose.orientation.y = wp.quaternion().y(); + arrow.pose.orientation.z = wp.quaternion().z(); + arrow.pose.orientation.w = wp.quaternion().w(); + arrow.scale.x = 0.2; // length + arrow.scale.y = 0.05; // width + arrow.scale.z = 0.05; // height arrow.color.r = r; arrow.color.g = g; arrow.color.b = b; @@ -1156,23 +1190,30 @@ airstack_msgs::msg::TrajectoryXYZVYaw CurveTrajectory::get_trajectory() { return //=================================================================================== TrajectoryLibrary::TrajectoryLibrary(std::string config_filename, - rclcpp::Node* node) { // tf2_ros::Buffer* b){ - // if(buffer == NULL) - // buffer = b; - // this->buffer = buffer; - // pnh = new ros::NodeHandle("~"); + rclcpp::Node::SharedPtr node_ptr) { // tf2_ros::Buffer* b){ + + RCLCPP_DEBUG(node_ptr->get_logger(), "TrajectoryLibrary constructor"); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "Traj Library node_ptr name is: " << node_ptr->get_name()); + + // accelertion trajectory parameters + node_ptr->declare_parameter("dt", 69.); + node_ptr->declare_parameter("ht", 69.); + node_ptr->declare_parameter("ht_long", 69.); + node_ptr->declare_parameter("max_velocity", 69.); + node_ptr->declare_parameter("magnitude", 69.); - this->node = node; + this->node_ptr = node_ptr; if (buffer == NULL) { - buffer = new tf2_ros::Buffer(node->get_clock()); + buffer = new tf2_ros::Buffer(node_ptr->get_clock()); listener = new tf2_ros::TransformListener(*buffer); } - YAML::Node config = YAML::LoadFile(config_filename); + YAML::Node yaml_config = YAML::LoadFile(config_filename); - YAML::Node trajectories = config["trajectories"]; - for (size_t i = 0; i < trajectories.size(); i++) { - YAML::Node traj_node = trajectories[i]; + YAML::Node trajectories_yaml = yaml_config["trajectories"]; + for (size_t i = 0; i < trajectories_yaml.size(); i++) { + YAML::Node traj_node = trajectories_yaml[i]; std::string type = parse(traj_node["type"]); if (type == "curve") { @@ -1231,13 +1272,24 @@ TrajectoryLibrary::TrajectoryLibrary(std::string config_filename, dynamic_trajectories.push_back(traj); } } + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "dt: " << node_ptr->get_parameter("dt").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "ht: " << node_ptr->get_parameter("ht").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "ht_long: " << node_ptr->get_parameter("ht_long").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "max_velocity: " << node_ptr->get_parameter("max_velocity").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "magnitude: " << node_ptr->get_parameter("magnitude").as_double()); } std::vector TrajectoryLibrary::get_static_trajectories() { std::vector trajectories; for (size_t i = 0; i < static_trajectories.size(); i++) { - trajectories.push_back(Trajectory(node, static_trajectories[i]->get_trajectory())); + trajectories.push_back( + Trajectory(node_ptr.get(), static_trajectories[i]->get_trajectory())); } return trajectories; @@ -1249,7 +1301,7 @@ std::vector TrajectoryLibrary::get_dynamic_trajectories( for (size_t i = 0; i < dynamic_trajectories.size(); i++) { airstack_msgs::msg::TrajectoryXYZVYaw path = dynamic_trajectories[i]->get_trajectory(odom); - if (path.waypoints.size() > 0) trajectories.push_back(Trajectory(node, path)); + if (path.waypoints.size() > 0) trajectories.push_back(Trajectory(node_ptr.get(), path)); } return trajectories; diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml index 7863acdc..099ee5b8 100644 --- a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml @@ -1,8 +1,8 @@ - - + + @@ -20,5 +20,5 @@ - - + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp index 01747788..6ddd83c1 100644 --- a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp @@ -19,8 +19,6 @@ class TrajectoryControlNode : public rclcpp::Node { private: - const std::string ROBOT_NAME = - std::getenv("ROBOT_NAME") == NULL ? "" : std::getenv("ROBOT_NAME"); rclcpp::Subscription::SharedPtr traj_seg_to_add_sub; rclcpp::Subscription::SharedPtr traj_override_sub; rclcpp::Subscription::SharedPtr odom_sub; @@ -170,7 +168,7 @@ TrajectoryControlNode::TrajectoryControlNode() : rclcpp::Node("trajectory_contro drone_point.header.frame_id = target_frame; drone_point.child_frame_id = target_frame; current_velocity = 0.f; - time_multiplier = 1.f; + time_multiplier = 1.f; // for forwards or backwards. Forward uses 1.0, Rewind uses -1.0 transition_dt = target_dt; new_rewind = false; @@ -205,11 +203,12 @@ void TrajectoryControlNode::timer_callback() { // visualization markers.overwrite(); markers - .add_sphere(target_frame, now- rclcpp::Duration::from_seconds(0.3), robot_point.x(), robot_point.y(), robot_point.z(), - sphere_radius) + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), robot_point.x(), + robot_point.y(), robot_point.z(), sphere_radius) .set_color(0.f, 0.f, 1.f, 0.7f); trajectory_vis_pub->publish( - trajectory->get_markers(now - rclcpp::Duration::from_seconds(0.3), 1, 1, 0, 1, false, false, traj_vis_thickness)); + trajectory->get_markers(now - rclcpp::Duration::from_seconds(0.3), "traj_controller", 1, + 1, 0, 1, false, false, traj_vis_thickness)); // find closest point on entire trajectory Waypoint closest_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); @@ -217,7 +216,7 @@ void TrajectoryControlNode::timer_callback() { bool closest_valid = false; // trajectory->get_closest_waypoint(robot_point, 0, // trajectory->get_duration(), &closest_wp); if (closest_valid) - closest_valid = trajectory->get_odom(closest_wp.time(), &closest_point_odom, now); + closest_valid = trajectory->get_odom(closest_wp.get_time(), &closest_point_odom, now); // else // ROS_INFO("CLOSEST NOT VALID"); if (closest_valid) closest_point_pub->publish(closest_point_odom); @@ -234,12 +233,13 @@ void TrajectoryControlNode::timer_callback() { bool closest_ahead_valid = trajectory->get_closest_waypoint( robot_point, virtual_time, prev_vtp_time + look_ahead_time, &closest_ahead_wp); if (closest_ahead_valid) { - virtual_time = closest_ahead_wp.time(); + virtual_time = closest_ahead_wp.get_time(); // visualization markers - .add_sphere(target_frame, now- rclcpp::Duration::from_seconds(0.3), closest_ahead_wp.x(), closest_ahead_wp.y(), - closest_ahead_wp.z(), 0.025f) + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + closest_ahead_wp.get_x(), closest_ahead_wp.get_y(), + closest_ahead_wp.get_z(), 0.025f) .set_color(0.f, 1.f, 0.f); Waypoint vtp_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); @@ -248,22 +248,23 @@ void TrajectoryControlNode::timer_callback() { virtual_time, search_ahead_factor * sphere_radius, prev_vtp_time + look_ahead_time, robot_point, sphere_radius, min_virtual_tracking_velocity, &vtp_wp, &end_wp); - if (vtp_valid) current_virtual_ahead_time = vtp_wp.time() - virtual_time; + if (vtp_valid) current_virtual_ahead_time = vtp_wp.get_time() - virtual_time; // visualization if (vtp_valid) markers - .add_sphere(target_frame, now- rclcpp::Duration::from_seconds(0.3), vtp_wp.x(), vtp_wp.y(), vtp_wp.z(), - 0.025f) + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + vtp_wp.get_x(), vtp_wp.get_y(), vtp_wp.get_z(), 0.025f) .set_color(0.f, 1.f, 0.f); else markers - .add_sphere(target_frame, now- rclcpp::Duration::from_seconds(0.3), closest_ahead_wp.x(), - closest_ahead_wp.y(), closest_ahead_wp.z() + sphere_radius, - 0.025f) + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + closest_ahead_wp.get_x(), closest_ahead_wp.get_y(), + closest_ahead_wp.get_z() + sphere_radius, 0.025f) .set_color(1.f, 0.f, 0.f); markers - .add_sphere(target_frame, now- rclcpp::Duration::from_seconds(0.3), end_wp.x(), end_wp.y(), end_wp.z(), 0.025f) + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + end_wp.get_x(), end_wp.get_y(), end_wp.get_z(), 0.025f) .set_color(0.f, 0.f, 1.f); } else RCLCPP_INFO(this->get_logger(), "AHEAD NOT VALID"); @@ -288,13 +289,13 @@ void TrajectoryControlNode::timer_callback() { } //*/ - // get virtual tracking point + // get virtual waypoint Waypoint virtual_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); bool virtual_valid = trajectory->get_waypoint(virtual_time + current_virtual_ahead_time, &virtual_wp); if (virtual_valid) { virtual_valid = - trajectory->get_odom(virtual_wp.time(), &virtual_tracking_point_odom, now); + trajectory->get_odom(virtual_wp.get_time(), &virtual_tracking_point_odom, now); // prevent oscillating between slow and normal mode if (tflib::to_tf(virtual_tracking_point_odom.twist.linear).length() <= @@ -304,18 +305,19 @@ void TrajectoryControlNode::timer_callback() { float look_ahead_multiplier = 1.f; if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) look_ahead_multiplier = -1.f; - trajectory->get_odom(virtual_wp.time() + look_ahead_multiplier * look_ahead_time, + trajectory->get_odom(virtual_wp.get_time() + look_ahead_multiplier * look_ahead_time, &look_ahead_point, now); - trajectory->get_odom(virtual_wp.time(), &drone_point, now); - actual_time = virtual_wp.time(); - prev_vtp_time = virtual_wp.time(); + trajectory->get_odom(virtual_wp.get_time(), &drone_point, now); + actual_time = virtual_wp.get_time(); + prev_vtp_time = virtual_wp.get_time(); } else { - RCLCPP_INFO_STREAM_ONCE(this->get_logger(), - "Virtual waypoint does not correspond to a waypoint along the " - "reference trajectory. If no trajectory is available yet, this is okay."); + RCLCPP_INFO_STREAM_ONCE( + this->get_logger(), + "Virtual waypoint does not correspond to a waypoint along the " + "reference trajectory. If no trajectory is available yet, this is okay."); } - if (trajectory->waypoint_count() <= 3) { + if (trajectory->get_num_waypoints() <= 3) { virtual_tracking_point_odom.twist.linear.x = 0; virtual_tracking_point_odom.twist.linear.y = 0; virtual_tracking_point_odom.twist.linear.z = 0; @@ -369,7 +371,7 @@ void TrajectoryControlNode::timer_callback() { } // When the tracking point reaches the end of the trajectory, the velocity gets set to zero - if (virtual_wp.time() >= trajectory->get_duration() || + if (virtual_wp.get_time() >= trajectory->get_duration() || trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE) { virtual_tracking_point_odom.twist.linear.x = 0; virtual_tracking_point_odom.twist.linear.y = 0; @@ -466,10 +468,10 @@ void TrajectoryControlNode::set_trajectory_mode( trajectory->clear(); } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::TRACK) { trajectory->clear(); - } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::SEGMENT) { + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ADD_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) { + prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT) { trajectory->clear(); virtual_time = 0; actual_time = 0; @@ -507,7 +509,7 @@ void TrajectoryControlNode::set_trajectory_mode( void TrajectoryControlNode::traj_seg_to_add_callback( const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr traj) { - if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::SEGMENT) + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT) trajectory->merge(Trajectory(this, *traj), virtual_time); } @@ -520,13 +522,15 @@ void TrajectoryControlNode::traj_override_callback( void TrajectoryControlNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom) { // this->odom = odom; - // An odometry's child frame is the frame of the robot, and the Twist (velocities) are put in that frame. - // This line converts the odometry's position AND velocities to the target frame, typically "world" or "map". - if (tflib::transform_odometry(tf_buffer, *odom, target_frame, target_frame, &(this->odom))){ + // An odometry's child frame is the frame of the robot, and the Twist (velocities) are put in + // that frame. This line converts the odometry's position AND velocities to the target frame, + // typically "world" or "map". + if (tflib::transform_odometry(tf_buffer, *odom, target_frame, target_frame, &(this->odom))) { got_odom = true; - } - else { - RCLCPP_INFO_STREAM(this->get_logger(), "Failed to transform odometry to target frame " << target_frame); + } else { + RCLCPP_INFO_STREAM(this->get_logger(), "Failed to transform odometry in frame " + << odom->header.frame_id << " to target frame " + << target_frame); } } diff --git a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv index c67d38fb..9b354e2c 100644 --- a/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv +++ b/ros_ws/src/robot/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv @@ -1,7 +1,7 @@ int32 PAUSE=0 int32 ROBOT_POSE=1 int32 TRACK=2 -int32 SEGMENT=3 +int32 ADD_SEGMENT=3 int32 REWIND=4 int32 mode diff --git a/ros_ws/src/robot/autonomy/3_local/local_bringup/CMakeLists.txt b/ros_ws/src/robot/autonomy/3_local/local_bringup/CMakeLists.txt index 8e1d0c32..19c86fbb 100644 --- a/ros_ws/src/robot/autonomy/3_local/local_bringup/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/3_local/local_bringup/CMakeLists.txt @@ -25,7 +25,7 @@ endif() # Install files. install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) # install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) # install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) diff --git a/ros_ws/src/robot/autonomy/3_local/local_bringup/launch/local.launch.xml b/ros_ws/src/robot/autonomy/3_local/local_bringup/launch/local.launch.xml index 59b12da9..b3c15b82 100644 --- a/ros_ws/src/robot/autonomy/3_local/local_bringup/launch/local.launch.xml +++ b/ros_ws/src/robot/autonomy/3_local/local_bringup/launch/local.launch.xml @@ -1,40 +1,18 @@ - - - - - - + - - - - - - - - - - - - - + - + + + + + + + + + + + + + + + + + + + + + - @@ -76,5 +76,10 @@ + + ?> \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz b/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz index f0537cce..e8754fbc 100644 --- a/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz +++ b/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz @@ -1,17 +1,16 @@ ---- Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Grid1 - /TF1/Frames1 - - /Odometry1/Topic1 - - /ExpansionPoly1 - - /Left Depth1 - Splitter Ratio: 0.35953420400619507 + - /Sensors1 + - /Local1 + - /Local1/DROAN1 + - /Local1/Trajectory Controller1 + - /Global1 + Splitter Ratio: 0.590062141418457 Tree Height: 1085 - Class: rviz_common/Selection Name: Selection @@ -32,7 +31,7 @@ Panels: SyncMode: 0 SyncSource: Expansion Cloud Visualization Manager: - Class: '' + Class: "" Displays: - Alpha: 0.5 Cell Size: 1 @@ -46,7 +45,7 @@ Visualization Manager: Normal Cell Count: 0 Offset: X: 0 - 'Y': 0 + Y: 0 Z: 0 Plane: XY Plane Cell Count: 100 @@ -57,6 +56,10 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false + American_Beech: + Value: true + Plane: + Value: true base_link: Value: true base_link_frd: @@ -98,265 +101,490 @@ Visualization Manager: Show Names: true Tree: world: + American_Beech: + {} + Plane: + {} map_FLU: map: base_link: - base_link_frd: {} + base_link_frd: + {} front_stereo: - left_camera: {} - right_camera: {} - ouster: {} - base_link_stabilized: {} - look_ahead_point: {} - look_ahead_point_stabilized: {} - map_ned: {} - tracking_point: {} - tracking_point_stabilized: {} + left_camera: + {} + right_camera: + {} + ouster: + {} + base_link_stabilized: + {} + look_ahead_point: + {} + look_ahead_point_stabilized: + {} + map_ned: + {} + tracking_point: + {} + tracking_point_stabilized: + {} Update Interval: 0 Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Traj Vis - Namespaces: - trajectory_0: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: trajectory_controller/trajectory_vis - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Traj Debug - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: trajectory_controller/trajectory_controller_debug_markers - Value: false - - Angle Tolerance: 0 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Left RGB + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/image_rect Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Front Left Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/depth Value: true - Value: true - Enabled: false - Keep: 1 - Name: Odometry - Position Tolerance: 0 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: odometry_conversion/odometry - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: VDB Mapping Marker - Namespaces: - '': true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: vdb_mapping/vdb_map_visualization - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 6.571824073791504 - Min Value: -0.5682187080383301 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 170; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 1 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/ouster/point_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 100 - Median window: 5 - Min Value: 0 - Name: Front Left Depth - Normalize Range: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/depth - Value: false - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Front Left RGB - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/image_rect - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Traj Library - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/trajectory_library_vis - 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/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 220 - Min Color: 0; 0; 0 - Min Intensity: 120 - Name: Expansion Cloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/expansion_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Trimmed Global Plan for DROAN - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/local_planner_global_plan_vis - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ExpansionPoly - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/expansion_poly - Value: false - - Class: rviz_default_plugins/MarkerArray + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.571824073791504 + Min Value: -0.5682187080383301 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 170; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Lidar + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/ouster/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: odometry_conversion/odometry + Value: false Enabled: true - Name: DisparityMarker - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/disparity_marker - Value: true - - Class: rviz_default_plugins/Image + Name: Sensors + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Disparity Frustum + Namespaces: + frustum: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/frustum + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Map Collision Checking + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_map_debug + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Graph Poses + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_graph + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Trimmed Global Plan for DROAN + Namespaces: + global_plan: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/local_planner_global_plan_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ExpansionPoly + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_poly + 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/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 220 + Min Color: 0; 0; 0 + Min Intensity: 120 + Name: Expansion Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Library + Namespaces: + trajectory_0: true + trajectory_1: true + trajectory_10: true + trajectory_100: true + trajectory_101: true + trajectory_102: true + trajectory_103: true + trajectory_104: true + trajectory_105: true + trajectory_106: true + trajectory_107: true + trajectory_108: true + trajectory_109: true + trajectory_11: true + trajectory_110: true + trajectory_111: true + trajectory_112: true + trajectory_113: true + trajectory_114: true + trajectory_115: true + trajectory_116: true + trajectory_117: true + trajectory_118: true + trajectory_119: true + trajectory_12: true + trajectory_120: true + trajectory_121: true + trajectory_122: true + trajectory_123: true + trajectory_124: true + trajectory_125: true + trajectory_126: true + trajectory_127: true + trajectory_128: true + trajectory_129: true + trajectory_13: true + trajectory_130: true + trajectory_131: true + trajectory_132: true + trajectory_133: true + trajectory_134: true + trajectory_135: true + trajectory_136: true + trajectory_137: true + trajectory_138: true + trajectory_139: true + trajectory_14: true + trajectory_140: true + trajectory_141: true + trajectory_142: true + trajectory_143: true + trajectory_144: true + trajectory_145: true + trajectory_146: true + trajectory_147: true + trajectory_148: true + trajectory_149: true + trajectory_15: true + trajectory_150: true + trajectory_151: true + trajectory_152: true + trajectory_153: true + trajectory_154: true + trajectory_155: true + trajectory_156: true + trajectory_157: true + trajectory_158: true + trajectory_159: true + trajectory_16: true + trajectory_160: true + trajectory_161: true + trajectory_17: true + trajectory_18: true + trajectory_19: true + trajectory_2: true + trajectory_20: true + trajectory_21: true + trajectory_22: true + trajectory_23: true + trajectory_24: true + trajectory_25: true + trajectory_26: true + trajectory_27: true + trajectory_28: true + trajectory_29: true + trajectory_3: true + trajectory_30: true + trajectory_31: true + trajectory_32: true + trajectory_33: true + trajectory_34: true + trajectory_35: true + trajectory_36: true + trajectory_37: true + trajectory_38: true + trajectory_39: true + trajectory_4: true + trajectory_40: true + trajectory_41: true + trajectory_42: true + trajectory_43: true + trajectory_44: true + trajectory_45: true + trajectory_46: true + trajectory_47: true + trajectory_48: true + trajectory_49: true + trajectory_5: true + trajectory_50: true + trajectory_51: true + trajectory_52: true + trajectory_53: true + trajectory_54: true + trajectory_55: true + trajectory_56: true + trajectory_57: true + trajectory_58: true + trajectory_59: true + trajectory_6: true + trajectory_60: true + trajectory_61: true + trajectory_62: true + trajectory_63: true + trajectory_64: true + trajectory_65: true + trajectory_66: true + trajectory_67: true + trajectory_68: true + trajectory_69: true + trajectory_7: true + trajectory_70: true + trajectory_71: true + trajectory_72: true + trajectory_73: true + trajectory_74: true + trajectory_75: true + trajectory_76: true + trajectory_77: true + trajectory_78: true + trajectory_79: true + trajectory_8: true + trajectory_80: true + trajectory_81: true + trajectory_82: true + trajectory_83: true + trajectory_84: true + trajectory_85: true + trajectory_86: true + trajectory_87: true + trajectory_88: true + trajectory_89: true + trajectory_9: true + trajectory_90: true + trajectory_91: true + trajectory_92: true + trajectory_93: true + trajectory_94: true + trajectory_95: true + trajectory_96: true + trajectory_97: true + trajectory_98: true + trajectory_99: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/trajectory_library_vis + Value: true + Enabled: true + Name: DROAN + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Vis + Namespaces: + traj_controller: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Debug + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_controller_debug_markers + Value: false + Enabled: true + Name: Trajectory Controller Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Left Camera - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/image_rect - Value: true - - Class: rviz_default_plugins/Image + Name: Local + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: false + Name: VDB Mapping Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: vdb_mapping/vdb_map_visualization + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/global_plan + Value: true Enabled: true - Max Value: 100 - Median window: 5 - Min Value: 0 - Name: Left Depth - Normalize Range: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/depth - Value: true + Name: Global Enabled: true Global Options: Background Color: 48; 48; 48 @@ -403,26 +631,26 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 43.695953369140625 + Distance: 8.18502426147461 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -1.6335676908493042 - 'Y': -2.40643310546875 - Z: -2.289210796356201 + X: 3.3486995697021484 + Y: -0.9512473344802856 + Z: 1.4642823934555054 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6753983497619629 + Pitch: 0.560396134853363 Target Frame: Value: Orbit (rviz) - Yaw: 0.6135711669921875 - Saved: null + Yaw: 2.143571615219116 + Saved: ~ Window Geometry: Displays: collapsed: false @@ -433,12 +661,7 @@ Window Geometry: Height: 1376 Hide Left Dock: false Hide Right Dock: false - Left Camera: - collapsed: false - Left Depth: - collapsed: false - QMainWindow State: >- - 000000ff00000000fd0000000400000000000001f5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c900000000000000000000000100000201000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000002ee0000002800fffffffb00000014004c006500660074002000440065007000740068010000032f000001d20000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042000000003b000001360000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068000000003b0000026b0000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b8000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001e5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c9000000000000000000000001000001f6000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042010000003b0000020e0000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068010000024f000002b20000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005d3000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -449,324 +672,4 @@ Window Geometry: collapsed: false Width: 2490 X: 1990 - 'Y': 27 trajectory_164305: true - trajectory_164794: true - trajectory_165283: true - trajectory_165772: true - trajectory_166261: true - trajectory_16627: true - trajectory_166750: true - trajectory_167239: true - trajectory_167728: true - trajectory_168217: true - trajectory_168706: true - trajectory_169195: true - trajectory_169684: true - trajectory_17116: true - trajectory_17605: true - trajectory_18094: true - trajectory_18583: true - trajectory_19072: true - trajectory_19561: true - trajectory_1957: true - trajectory_20050: true - trajectory_20539: true - trajectory_21028: true - trajectory_21517: true - trajectory_22006: true - trajectory_22495: true - trajectory_22984: true - trajectory_23473: true - trajectory_23962: true - trajectory_24451: true - trajectory_2446: true - trajectory_24940: true - trajectory_25429: true - trajectory_25918: true - trajectory_26407: true - trajectory_26896: true - trajectory_27385: true - trajectory_27874: true - trajectory_28363: true - trajectory_28852: true - trajectory_29341: true - trajectory_2935: true - trajectory_29830: true - trajectory_30319: true - trajectory_30808: true - trajectory_31297: true - trajectory_31786: true - trajectory_32275: true - trajectory_32764: true - trajectory_33253: true - trajectory_33742: true - trajectory_34231: true - trajectory_3424: true - trajectory_34720: true - trajectory_35209: true - trajectory_35698: true - trajectory_36187: true - trajectory_36676: true - trajectory_37165: true - trajectory_37654: true - trajectory_38143: true - trajectory_38632: true - trajectory_39121: true - trajectory_3913: true - trajectory_39610: true - trajectory_40099: true - trajectory_40588: true - trajectory_41077: true - trajectory_41566: true - trajectory_42055: true - trajectory_42544: true - trajectory_43033: true - trajectory_43522: true - trajectory_44011: true - trajectory_4402: true - trajectory_44500: true - trajectory_44989: true - trajectory_45478: true - trajectory_45967: true - trajectory_46456: true - trajectory_46945: true - trajectory_47434: true - trajectory_47923: true - trajectory_48412: true - trajectory_48901: true - trajectory_4891: true - trajectory_490: true - trajectory_49390: true - trajectory_49879: true - trajectory_50368: true - trajectory_50857: true - trajectory_51346: true - trajectory_51835: true - trajectory_52324: true - trajectory_52813: true - trajectory_53302: true - trajectory_53791: true - trajectory_5380: true - trajectory_54280: true - trajectory_54769: true - trajectory_55258: true - trajectory_55747: true - trajectory_56236: true - trajectory_56725: true - trajectory_57214: true - trajectory_57703: true - trajectory_58192: true - trajectory_58681: true - trajectory_5869: true - trajectory_59170: true - trajectory_59659: true - trajectory_60148: true - trajectory_60637: true - trajectory_61126: true - trajectory_61615: true - trajectory_62104: true - trajectory_62593: true - trajectory_63082: true - trajectory_63571: true - trajectory_6358: true - trajectory_64060: true - trajectory_64549: true - trajectory_65038: true - trajectory_65527: true - trajectory_66016: true - trajectory_66505: true - trajectory_66994: true - trajectory_67483: true - trajectory_67972: true - trajectory_68461: true - trajectory_6847: true - trajectory_68950: true - trajectory_69439: true - trajectory_69928: true - trajectory_70417: true - trajectory_70906: true - trajectory_71395: true - trajectory_71884: true - trajectory_72373: true - trajectory_72862: true - trajectory_73351: true - trajectory_7336: true - trajectory_73840: true - trajectory_74329: true - trajectory_74818: true - trajectory_75307: true - trajectory_75796: true - trajectory_76285: true - trajectory_76774: true - trajectory_77263: true - trajectory_77752: true - trajectory_78241: true - trajectory_7825: true - trajectory_78730: true - trajectory_79219: true - trajectory_79708: true - trajectory_80197: true - trajectory_80686: true - trajectory_81175: true - trajectory_81664: true - trajectory_82153: true - trajectory_82642: true - trajectory_83131: true - trajectory_8314: true - trajectory_83620: true - trajectory_84109: true - trajectory_84598: true - trajectory_85087: true - trajectory_85576: true - trajectory_86065: true - trajectory_86554: true - trajectory_87043: true - trajectory_87532: true - trajectory_88021: true - trajectory_8803: true - trajectory_88510: true - trajectory_88999: true - trajectory_89488: true - trajectory_89977: true - trajectory_90466: true - trajectory_90955: true - trajectory_91444: true - trajectory_91933: true - trajectory_92422: true - trajectory_92911: true - trajectory_9292: true - trajectory_93400: true - trajectory_93889: true - trajectory_94378: true - trajectory_94867: true - trajectory_95356: true - trajectory_95845: true - trajectory_96334: true - trajectory_96823: true - trajectory_97312: true - trajectory_97801: true - trajectory_9781: true - trajectory_979: true - trajectory_98290: true - trajectory_98779: true - trajectory_99268: true - trajectory_99757: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/local_planner_global_plan_vis - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ExpansionPoly - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/expansion_poly - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DisparityMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/disparity_marker - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 18.647010803222656 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 5.373880863189697 - Y: -3.36837100982666 - Z: -1.463265299797058 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.32039859890937805 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.983567476272583 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Front Left Depth: - collapsed: false - Front Left RGB: - collapsed: false - Height: 773 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002b1000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c9000000000000000000000001000002010000026bfc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042000000003b000001360000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068000000003b0000026b0000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005a00000026b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1440 - X: 2431 - Y: 309 + Y: 27 diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt new file mode 100644 index 00000000..9de81c1b --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.5) +project(ensemble_global_planner) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +################################### +## ament specific configuration ## +################################### + +ament_package() + +########### +## Build ## +########### + +# Specify additional locations of header files + +# Declare a C++ executable +add_executable(ensemble_global_planner src/ensemble_global_planner_node.cpp) + +# Link libraries +ament_target_dependencies(ensemble_global_planner + rclcpp + rclcpp_action + std_msgs + std_srvs + geometry_msgs + nav_msgs + visualization_msgs + tf2 + tf2_ros +) + +# Install executable +install(TARGETS + ensemble_global_planner + DESTINATION lib/${PROJECT_NAME} +) +# install(DIRECTORY +# launch +# DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + src + DESTINATION share/${PROJECT_NAME}) + + +############# +## Testing ## +############# + +# Add gtest based cpp test target and link libraries +# ament_add_gtest(${PROJECT_NAME}-test test/test_ensemble_global_planner.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node) +# endif() \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml new file mode 100644 index 00000000..f38ff465 --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle" + way_point_planners: + - name: "random_walk" + config: + frequency: 1.0 #hz + diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp new file mode 100644 index 00000000..80ff9b2e --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp @@ -0,0 +1,58 @@ + +#ifndef RANDOM_WALK_NODE_H +#define RANDOM_WALK_NODE_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +class EnsembleGlobalPlannerNode : public rclcpp::Node { + private: + + // String constants + std::string srv_global_plan_toggle_topic_; + + void globalPlannnerToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + // Other functions + void readParameters(); + + bool enable_global_planner = false; + + public: + // explicit RandomWalkNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + EnsembleGlobalPlannerNode(); + ~EnsembleGlobalPlannerNode() = default; + + // ROS subscribers + rclcpp::Subscription::SharedPtr sub_map; + rclcpp::Subscription::SharedPtr sub_robot_tf; + + // ROS publishers + // rclcpp::Publisher::SharedPtr pub_global_path; + rclcpp::Publisher::SharedPtr pub_goal_point; + rclcpp::Publisher::SharedPtr pub_trajectory_lines; + + // ROS services + rclcpp::Service::SharedPtr srv_global_planner_toggle; + + // ROS timers + rclcpp::TimerBase::SharedPtr timer; +}; + +#endif // RANDOM_WALK_NODE_H diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/package.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/package.xml new file mode 100644 index 00000000..fafdacbc --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/package.xml @@ -0,0 +1,46 @@ + + + ensemble_global_planner + 0.0.0 + Ensemble planner to coordinate different groups of planners + + + + + todo + + + + + + TODO + + + + + + + + + + + + + + ament_cmake + rclcpp + rclcpp_action + std_msgs + std_srvs + base + geometry_msgs + nav_msgs + visualization_msgs + message_generation + tf2 + tf2_ros + + + ament_cmake + + diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp new file mode 100644 index 00000000..bf8b10f8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp @@ -0,0 +1,41 @@ + +#include "../include/ensemble_global_planner_node.hpp" + +void EnsembleGlobalPlannerNode::readParameters() { + this->declare_parameter("srv_global_plan_toggle_topic"); + if (!this->get_parameter("srv_global_plan_toggle_topic", this->srv_global_plan_toggle_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_global_plan_toggle_topic"); + } +} + +EnsembleGlobalPlannerNode::EnsembleGlobalPlannerNode() : Node("ensemble_global_planner_node") { + // Initialize the Global Planner planner + EnsembleGlobalPlannerNode::readParameters(); + this->srv_global_planner_toggle = this->create_service( + srv_global_plan_toggle_topic_, + std::bind(&EnsembleGlobalPlannerNode::globalPlannnerToggleCallback, this, std::placeholders::_1, + std::placeholders::_2)); +} + +void EnsembleGlobalPlannerNode::globalPlannnerToggleCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) { + if (this->enable_global_planner == false) { + this->enable_global_planner = true; + response->success = true; + response->message = "Global Planner enabled"; + RCLCPP_INFO(this->get_logger(), "Global Planner enabled"); + } else { + this->enable_global_planner = false; + response->success = true; + response->message = "Global Planer disabled"; + RCLCPP_INFO(this->get_logger(), "Global Planner disabled"); + } +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt b/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt similarity index 67% rename from ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt rename to ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt index a6f2b551..50e46d65 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(trivial_planners) +project(global_planner_bringup) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -11,15 +11,6 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) -add_executable(random_walk_planner_node src/random_walk_planner_node.cpp) -target_include_directories(random_walk_planner_node PUBLIC - $ - $) -target_compile_features(random_walk_planner_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -install(TARGETS random_walk_planner_node - DESTINATION lib/${PROJECT_NAME}) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -32,4 +23,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml new file mode 100644 index 00000000..939903f8 --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/package.xml similarity index 87% rename from ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml rename to ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/package.xml index 41317518..1aab4c36 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/package.xml +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/global_planner_bringup/package.xml @@ -1,11 +1,11 @@ - trivial_planners + global_planner_bringup 0.0.0 TODO: Package description andrew - TODO: License declaration + Apache-2.0 ament_cmake diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/CMakeLists.txt b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/CMakeLists.txt index 6cb27eed..6ba75c20 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/CMakeLists.txt @@ -9,13 +9,14 @@ endif() # Find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) -find_package(airstack_msgs REQUIRED) ################################### ## ament specific configuration ## @@ -35,13 +36,14 @@ add_executable(random_walk_planner src/random_walk_node.cpp src/random_walk_logi # Link libraries ament_target_dependencies(random_walk_planner rclcpp + rclcpp_action std_msgs + std_srvs geometry_msgs nav_msgs visualization_msgs tf2 tf2_ros - airstack_msgs ) # Install executable diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml index 8155ca0b..5386249c 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml @@ -1,20 +1,22 @@ /**: ros__parameters: - world_frame_id: "/sim_lidar" - pub_global_path_topic: "/robot1/global/planning/trajectory" - pub_goal_point_topic: "/robot1/global/planning/goal_point" - pub_trajectory_lines_topic: "/robot1/global/planning/traj_viz" - sub_vdb_map_topic: "/vdb_mapping/vdb_map_visualization" - sub_odometry_topic: "/odom" + world_frame_id: "map" + robot_frame_id: "base_link" + pub_global_plan_topic: "~/global_plan" + pub_goal_point_viz_topic: "~/goal_point_viz" + pub_trajectory_viz_topic: "~/traj_viz" + sub_map_topic: "vdb_map_visualization" + sub_robot_tf_topic: "/tf" + srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle" - publish_visualizations: true + publish_visualizations: false # should trajectory visualizations be published + num_paths_to_generate: 5 # how many paths to generate # Constants - max_start_to_goal_dist_m: 3.5 # how far away the goal waypoint could be - max_angle_change_deg: 30.0 # degrees, + and - from horizontal - checking_point_cnt: 1000 #cnt, how many points to check between each waypoint - waypoint_dist_m: 1.0 # waypoint is this far away - max_z_m: 0.1 # max z distance that the goal position can be - collision_padding_m: 0.1 # how far away from the obstacle to plan - path_end_threshold_m: 0.1 # how close to the goal to stop + max_start_to_goal_dist_m: 10.0 # how max distance the goal can be from the current robot position + checking_point_cnt: 1000 # how many points to collision check between the start and goal points + max_z_m: 0.1 # max height that the goal can deviate from the current height + collision_padding_m: 0.1 # how much space should the path have to any obstacles + path_end_threshold_m: 0.1 # how close to the goal will the path be considered complete + max_z_angle_change_rad: 1.5708 # how much the goal can deviate from the current orientation diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp index b188aabe..9b69babe 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp @@ -15,12 +15,11 @@ typedef std::vector> Path; // x, y, z, y struct init_params { float max_start_to_goal_dist_m; - float max_angle_change_deg; int checking_point_cnt; - float waypoint_dist_m; float max_z_m; float collision_padding_m; float path_end_threshold_m; + float max_z_angle_change_rad; std::tuple voxel_size_m; }; @@ -31,25 +30,24 @@ class RandomWalkPlanner { ~RandomWalkPlanner() = default; std::optional generate_straight_rand_path( - std::tuple start_point, - float timeout_duration = 1.0); // x, y, z, yaw - - std::vector> voxel_points; + std::tuple start_point, float timeout_duration); // x, y, z, yaw float path_end_threshold_m; + + std::vector> voxel_points; private: // Numerical constants float max_start_to_goal_dist_m_; - float max_angle_change_deg_; int checking_point_cnt; - float waypoint_dist_m_; float max_z_m_; float collision_padding_m; + float max_z_angle_change_rad; // Variables std::tuple voxel_size_m; + // Functions bool check_if_collided_single_voxel(std::tuple point, std::tuple voxel_center); @@ -57,7 +55,7 @@ class RandomWalkPlanner { bool check_if_collided(std::tuple point); std::tuple generate_goal_point( - std::tuple start_point); + std::tuple start_point); }; double get_point_distance(std::tuple point1, @@ -65,4 +63,6 @@ double get_point_distance(std::tuple point1, double deg2rad(double deg); +double rad2deg(double rad); + #endif // RANDOM_WALK_LOGIC_H diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp index 880b0e8f..896e5c21 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp @@ -2,50 +2,69 @@ #ifndef RANDOM_WALK_NODE_H #define RANDOM_WALK_NODE_H -#include +#include +#include + +#include #include -#include +#include +#include +#include +#include #include +#include #include +#include #include #include #include -#include "geometry_msgs/msg/point.hpp" #include "random_walk_logic.hpp" #include "rclcpp/rclcpp.hpp" class RandomWalkNode : public rclcpp::Node { private: - // Planner RandomWalkPlanner random_walk_planner; // String constants std::string world_frame_id_; - std::string pub_global_path_topic_; - std::string pub_goal_point_topic_; - std::string pub_trajectory_lines_topic_; - std::string sub_vdb_map_topic_; - std::string sub_odometry_topic_; + std::string robot_frame_id_; + std::string pub_global_plan_topic_; + std::string pub_goal_point_viz_topic_; + std::string pub_trajectory_viz_topic_; + std::string sub_map_topic_; + std::string sub_robot_tf_topic_; + std::string srv_random_walk_toggle_topic_; // Variables init_params params; - airstack_msgs::msg::TrajectoryXYZVYaw generated_path; - std::vector> voxel_points; + // nav_msgs::msg::Path generated_path; + int num_paths_to_generate_; + std::vector generated_paths; bool publish_visualizations = false; - bool is_path_executing = false; bool received_first_map = false; - bool received_first_odometry = false; - std::tuple current_location; // x, y, z, yaw - std::tuple current_goal_location; // x, y, z, yaw + bool received_first_robot_tf = false; + bool enable_random_walk = false; + bool is_path_executing = false; + + geometry_msgs::msg::Transform current_location; // x, y, z, yaw + geometry_msgs::msg::Transform current_goal_location; // x, y, z, yaw // Callbacks - void vdbmapCallback(const visualization_msgs::msg::Marker::SharedPtr msg); + void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg); + + void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg); + + void randomWalkToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); - void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg); void timerCallback(); + void generate_plan(); + + void publish_plan(); + // Other functions std::optional readParameters(); @@ -53,18 +72,22 @@ class RandomWalkNode : public rclcpp::Node { visualization_msgs::msg::Marker createTrajectoryLineMarker(); public: + // explicit RandomWalkNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); RandomWalkNode(); ~RandomWalkNode() = default; // ROS subscribers - rclcpp::Subscription::SharedPtr sub_vdb_map; - rclcpp::Subscription::SharedPtr sub_odometry; + rclcpp::Subscription::SharedPtr sub_map; + rclcpp::Subscription::SharedPtr sub_robot_tf; // ROS publishers - rclcpp::Publisher::SharedPtr pub_global_path; + rclcpp::Publisher::SharedPtr pub_global_plan; rclcpp::Publisher::SharedPtr pub_goal_point; rclcpp::Publisher::SharedPtr pub_trajectory_lines; + // ROS services + rclcpp::Service::SharedPtr srv_random_walk_toggle; + // ROS timers rclcpp::TimerBase::SharedPtr timer; }; diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml new file mode 100644 index 00000000..174bc53f --- /dev/null +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml @@ -0,0 +1,11 @@ + + + + + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launcher.py b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launcher.py deleted file mode 100644 index 6fe4e55c..00000000 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/launch/random_walk_launcher.py +++ /dev/null @@ -1,23 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - ld = LaunchDescription() - - config = os.path.join( - get_package_share_directory('random_walk_planner'), - 'config', - 'random_walk_config.yaml' - ) - random_walk_planner = Node( - package='random_walk_planner', - executable='random_walk_planner', - name='random_walk_planner', - parameters = [config] - ) - - ld.add_action(random_walk_planner) - - return ld \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/package.xml b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/package.xml index a30c3b4a..019be21c 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/package.xml +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/package.xml @@ -28,37 +28,18 @@ ament_cmake - rclcpp - std_msgs - base - geometry_msgs - nav_msgs - visualization_msgs - message_generation - tf2 - tf2_ros - airstack_msgs - + rclcpp + rclcpp_action + std_msgs + std_srvs + base + geometry_msgs + nav_msgs + visualization_msgs + message_generation + tf2 + tf2_ros - rclcpp - std_msgs - base - geometry_msgs - nav_msgs - visualization_msgs - message_runtime - - rclcpp - std_msgs - base - geometry_msgs - nav_msgs - visualization_msgs - message_runtime - tf2 - tf2_ros - airstack_msgs - ament_cmake diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp index 17fe54e5..812430c5 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp @@ -5,36 +5,37 @@ RandomWalkPlanner::RandomWalkPlanner(init_params params) { this->max_start_to_goal_dist_m_ = params.max_start_to_goal_dist_m; - this->max_angle_change_deg_ = params.max_angle_change_deg; this->checking_point_cnt = params.checking_point_cnt; - this->waypoint_dist_m_ = params.waypoint_dist_m; this->max_z_m_ = params.max_z_m; this->voxel_size_m = params.voxel_size_m; this->collision_padding_m = params.collision_padding_m; this->path_end_threshold_m = params.path_end_threshold_m; + this->max_z_angle_change_rad = params.max_z_angle_change_rad; } std::optional RandomWalkPlanner::generate_straight_rand_path( std::tuple start_point, float timeout_duration) { - std::tuple start_point_wo_yaw( - std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + // std::tuple start_point_wo_yaw( + // std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); std::optional path; bool is_goal_point_valid = false; - // get start time + // get start ti const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Starting Path Search..."); while (!is_goal_point_valid && std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time) .count() < timeout_duration) { - std::tuple goal_point = generate_goal_point(start_point_wo_yaw); - RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Point Generated..."); + std::tuple goal_point = generate_goal_point(start_point); + // RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Point Generated..."); if (std::get<2>(goal_point) == -1) { - RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "No valid goal point found"); + RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), + "No valid goal point found in timeout duration"); break; } - float x_diff = std::get<0>(goal_point) - std::get<0>(start_point_wo_yaw); - float y_diff = std::get<1>(goal_point) - std::get<1>(start_point_wo_yaw); - float z_diff = std::get<2>(goal_point) - std::get<2>(start_point_wo_yaw); + + float x_diff = std::get<0>(goal_point) - std::get<0>(start_point); + float y_diff = std::get<1>(goal_point) - std::get<1>(start_point); + float z_diff = std::get<2>(goal_point) - std::get<2>(start_point); float yaw = std::atan2(y_diff, x_diff); path = Path(); @@ -43,10 +44,11 @@ std::optional RandomWalkPlanner::generate_straight_rand_path( path.value().push_back(first_point); // start moving in the direction of the goal point - std::tuple current_point = start_point_wo_yaw; - RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), - "Checking intermediate points at tolerance %f, with point count %d", - this->path_end_threshold_m, this->checking_point_cnt); + std::tuple current_point = std::make_tuple( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + // RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), + // "Checking intermediate points at tolerance %f, with point count %d", + // this->path_end_threshold_m, this->checking_point_cnt); while (get_point_distance(current_point, goal_point) > 0.001) { if (!check_if_collided(current_point)) { float new_x = std::get<0>(current_point) + x_diff / this->checking_point_cnt; @@ -59,7 +61,7 @@ std::optional RandomWalkPlanner::generate_straight_rand_path( path.value().push_back(new_point_with_yaw); } else { RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Collision detected"); - is_goal_point_valid = false; + is_goal_point_valid = false; break; } is_goal_point_valid = true; @@ -75,15 +77,11 @@ std::optional RandomWalkPlanner::generate_straight_rand_path( bool RandomWalkPlanner::check_if_collided_single_voxel( std::tuple point, std::tuple voxel_center) { // Check if the point is within the voxel - float x = std::get<0>(point); - float y = std::get<1>(point); - float z = std::get<2>(point); - float x_voxel = std::get<0>(voxel_center); - float y_voxel = std::get<1>(voxel_center); - float z_voxel = std::get<2>(voxel_center); - float dist = std::sqrt((x - x_voxel) * (x - x_voxel) + (y - y_voxel) * (y - y_voxel) + - (z - z_voxel) * (z - z_voxel)); - if (dist < std::get<0>(this->voxel_size_m) + this->collision_padding_m) { + float dist = get_point_distance(point, voxel_center); + float max_size = + std::max(std::get<0>(this->voxel_size_m), + std::max(std::get<1>(this->voxel_size_m), std::get<2>(this->voxel_size_m))); + if (dist < max_size + this->collision_padding_m) { return true; } return false; @@ -100,7 +98,7 @@ bool RandomWalkPlanner::check_if_collided(std::tuple point) } std::tuple RandomWalkPlanner::generate_goal_point( - std::tuple start_point) { + std::tuple start_point) { float time_out_duration = 1.0; const clock_t start_time = clock(); @@ -113,17 +111,25 @@ std::tuple RandomWalkPlanner::generate_goal_point( this->max_start_to_goal_dist_m_) - this->max_start_to_goal_dist_m_; float rand_z = static_cast(rand()) / static_cast(RAND_MAX) * this->max_z_m_; - float x_diff = rand_x - std::get<0>(start_point); - float y_diff = rand_y - std::get<1>(start_point); - float z_diff = rand_z - std::get<2>(start_point); + std::tuple rand_point(rand_x, rand_y, rand_z); + std::tuple start_point_wo_yaw( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + float dist = get_point_distance(start_point_wo_yaw, rand_point); + float new_angle = std::atan2(std::get<1>(rand_point) - std::get<1>(start_point_wo_yaw), + std::get<0>(rand_point) - std::get<0>(start_point_wo_yaw)); + float angle_diff = std::abs(std::get<3>(start_point) - new_angle); // if the z value of the point is low enough if (rand_z < max_z_m_) { - // if the point is close enough to the start point - if (std::sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff) < - this->max_start_to_goal_dist_m_) { - // if the point doesnt collide with any of the voxels - if (!(check_if_collided(std::tuple(rand_x, rand_y, rand_z)))) { - return std::tuple(rand_x, rand_y, rand_z); + // if the angle change is low enough + if (angle_diff < this->max_z_angle_change_rad) { + // if the point is close enough to the start point + if (dist < this->max_start_to_goal_dist_m_) { + // if the point doesnt collide with any of the voxels + if (!(check_if_collided(rand_point))) { + RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), + "Angle Difference: %f", rad2deg(angle_diff)); + return rand_point; + } } } } @@ -140,3 +146,5 @@ double get_point_distance(std::tuple point1, } double deg2rad(double deg) { return deg * M_PI / 180.0; } + +double rad2deg(double rad) { return rad * 180.0 / M_PI; } diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp index cd2b281a..25a98f5d 100644 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp +++ b/ros_ws/src/robot/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp @@ -3,122 +3,6 @@ #include "../include/random_walk_logic.hpp" -RandomWalkNode::RandomWalkNode() : Node("random_walk_node") { - std::optional params_opt = RandomWalkNode::readParameters(); - // Initialize the random walk planner - if (params_opt.has_value()) { - this->params = params_opt.value(); - } else - RCLCPP_ERROR(this->get_logger(), "Failed to initialize random walk planner"); - - this->sub_vdb_map = this->create_subscription( - sub_vdb_map_topic_, 10, - std::bind(&RandomWalkNode::vdbmapCallback, this, std::placeholders::_1)); - this->sub_odometry = this->create_subscription( - sub_odometry_topic_, 10, - std::bind(&RandomWalkNode::odometryCallback, this, std::placeholders::_1)); - - this->pub_global_path = - this->create_publisher(pub_global_path_topic_, 10); - this->pub_goal_point = - this->create_publisher(pub_goal_point_topic_, 10); - this->pub_trajectory_lines = - this->create_publisher(pub_trajectory_lines_topic_, 10); - this->timer = this->create_wall_timer(std::chrono::seconds(1), - std::bind(&RandomWalkNode::timerCallback, this)); -} - -void RandomWalkNode::vdbmapCallback(const visualization_msgs::msg::Marker::SharedPtr msg) { - // updating the local voxel points and generating a path only if the path is not executing - if (!this->received_first_map) { - this->received_first_map = true; - RCLCPP_INFO(this->get_logger(), "Received first map"); - this->params.voxel_size_m = - std::tuple(msg->scale.x, msg->scale.y, msg->scale.z); - this->random_walk_planner = RandomWalkPlanner(this->params); - RCLCPP_INFO(this->get_logger(), "Initialized random walk planner"); - } - // if path is not executing and the odometry has been received - if (!this->is_path_executing && this->received_first_odometry) { - RCLCPP_INFO(this->get_logger(), "Setting Voxel Points from message"); - this->voxel_points.clear(); - for (int i = 0; i < msg->points.size(); i++) { - this->random_walk_planner.voxel_points.push_back(std::tuple( - msg->points[i].x, msg->points[i].y, msg->points[i].z)); - } - RCLCPP_INFO(this->get_logger(), "Generating Path..."); - std::optional gen_path_opt = - this->random_walk_planner.generate_straight_rand_path(this->current_location); - if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) { - RCLCPP_INFO(this->get_logger(), "Generated path with size %d", - gen_path_opt.value().size()); - - this->current_goal_location = std::tuple( - std::get<0>(gen_path_opt.value().back()), std::get<1>(gen_path_opt.value().back()), - std::get<2>(gen_path_opt.value().back()), std::get<3>(gen_path_opt.value().back())); - // publish the path - this->generated_path = airstack_msgs::msg::TrajectoryXYZVYaw(); - this->generated_path.header.stamp = this->now(); - this->generated_path.header.frame_id = world_frame_id_; - for (auto point : gen_path_opt.value()) { - airstack_msgs::msg::WaypointXYZVYaw point_msg; - point_msg.position.x = std::get<0>(point); - point_msg.position.y = std::get<1>(point); - point_msg.position.z = std::get<2>(point); - point_msg.yaw = std::get<3>(point); - this->generated_path.waypoints.push_back(point_msg); - } - if (this->publish_visualizations) { - this->pub_global_path->publish(this->generated_path); - this->pub_goal_point->publish(createGoalPointMarker()); - this->pub_trajectory_lines->publish(createTrajectoryLineMarker()); - } - RCLCPP_INFO(this->get_logger(), "Published path and goal point at %f, %f, %f", - this->generated_path.waypoints.back().position.x, - this->generated_path.waypoints.back().position.y, - this->generated_path.waypoints.back().position.z); - this->is_path_executing = true; - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); - } - } - // check if the path is executing - else if (this->is_path_executing) { - std::tuple curr_loc_wo_yaw(std::get<0>(this->current_location), - std::get<1>(this->current_location), - std::get<2>(this->current_location)); - std::tuple goal_loc_wo_yaw(std::get<0>(this->current_goal_location), - std::get<1>(this->current_goal_location), - std::get<2>(this->current_goal_location)); - if (get_point_distance(curr_loc_wo_yaw, goal_loc_wo_yaw) < - this->random_walk_planner.path_end_threshold_m) { - this->is_path_executing = false; - } - } -} - -void RandomWalkNode::odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { - // get the current location - this->current_location = std::tuple( - msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, - msg->pose.pose.orientation.z); - if (!this->received_first_odometry) { - this->received_first_odometry = true; - RCLCPP_INFO(this->get_logger(), "Received first odometry"); - } -} - -void RandomWalkNode::timerCallback() { - if (this->is_path_executing && this->publish_visualizations) { - RCLCPP_INFO(this->get_logger(), "Publishing visualizations..."); - visualization_msgs::msg::Marker goal_point_msg = createGoalPointMarker(); - this->pub_goal_point->publish(goal_point_msg); - - visualization_msgs::msg::Marker trajectory_line_msg = createTrajectoryLineMarker(); - this->pub_trajectory_lines->publish(trajectory_line_msg); - } -} - std::optional RandomWalkNode::readParameters() { // Read in parameters based off the default yaml file init_params params; @@ -127,29 +11,39 @@ std::optional RandomWalkNode::readParameters() { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: world_frame_id"); return std::optional{}; } - this->declare_parameter("pub_global_path_topic"); - if (!this->get_parameter("pub_global_path_topic", this->pub_global_path_topic_)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_global_path_topic"); + this->declare_parameter("robot_frame_id"); + if (!this->get_parameter("robot_frame_id", this->robot_frame_id_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: robot_frame_id"); return std::optional{}; } - this->declare_parameter("pub_goal_point_topic"); - if (!this->get_parameter("pub_goal_point_topic", this->pub_goal_point_topic_)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_goal_point_topic"); + this->declare_parameter("pub_global_plan_topic"); + if (!this->get_parameter("pub_global_plan_topic", this->pub_global_plan_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_global_plan_topic"); return std::optional{}; } - this->declare_parameter("pub_trajectory_lines_topic"); - if (!this->get_parameter("pub_trajectory_lines_topic", this->pub_trajectory_lines_topic_)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_trajectory_lines_topic"); + this->declare_parameter("pub_goal_point_viz_topic"); + if (!this->get_parameter("pub_goal_point_viz_topic", this->pub_goal_point_viz_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_goal_point_viz_topic"); return std::optional{}; } - this->declare_parameter("sub_vdb_map_topic"); - if (!this->get_parameter("sub_vdb_map_topic", this->sub_vdb_map_topic_)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_vdb_map_topic"); + this->declare_parameter("pub_trajectory_viz_topic"); + if (!this->get_parameter("pub_trajectory_viz_topic", this->pub_trajectory_viz_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_trajectory_viz_topic"); return std::optional{}; } - this->declare_parameter("sub_odometry_topic"); - if (!this->get_parameter("sub_odometry_topic", this->sub_odometry_topic_)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_odometry_topic"); + this->declare_parameter("sub_map_topic"); + if (!this->get_parameter("sub_map_topic", this->sub_map_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_map_topic"); + return std::optional{}; + } + this->declare_parameter("sub_robot_tf_topic"); + if (!this->get_parameter("sub_robot_tf_topic", this->sub_robot_tf_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_robot_tf_topic"); + return std::optional{}; + } + this->declare_parameter("srv_random_walk_toggle_topic"); + if (!this->get_parameter("srv_random_walk_toggle_topic", this->srv_random_walk_toggle_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_random_walk_toggle_topic"); return std::optional{}; } this->declare_parameter("publish_visualizations"); @@ -157,26 +51,21 @@ std::optional RandomWalkNode::readParameters() { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: publish_visualizations"); return std::optional{}; } + this->declare_parameter("num_paths_to_generate"); + if (!this->get_parameter("num_paths_to_generate", this->num_paths_to_generate_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: num_paths_to_generate"); + return std::optional{}; + } this->declare_parameter("max_start_to_goal_dist_m"); if (!this->get_parameter("max_start_to_goal_dist_m", params.max_start_to_goal_dist_m)) { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_start_to_goal_dist_m"); return std::optional{}; } - this->declare_parameter("max_angle_change_deg"); - if (!this->get_parameter("max_angle_change_deg", params.max_angle_change_deg)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_angle_change_deg"); - return std::optional{}; - } this->declare_parameter("checking_point_cnt"); if (!this->get_parameter("checking_point_cnt", params.checking_point_cnt)) { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: checking_point_cnt"); return std::optional{}; } - this->declare_parameter("waypoint_dist_m"); - if (!this->get_parameter("waypoint_dist_m", params.waypoint_dist_m)) { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: waypoint_dist_m"); - return std::optional{}; - } this->declare_parameter("max_z_m"); if (!this->get_parameter("max_z_m", params.max_z_m)) { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_m"); @@ -192,9 +81,226 @@ std::optional RandomWalkNode::readParameters() { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: path_end_threshold_m"); return std::optional{}; } + this->declare_parameter("max_z_angle_change_rad"); + if (!this->get_parameter("max_z_angle_change_rad", params.max_z_angle_change_rad)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_angle_change_rad"); + return std::optional{}; + } return params; } +RandomWalkNode::RandomWalkNode() : Node("random_walk_node") { + // Initialize the random walk planner + std::optional params_opt = RandomWalkNode::readParameters(); + if (params_opt.has_value()) { + this->params = params_opt.value(); + } else + RCLCPP_ERROR(this->get_logger(), "Failed to initialize random walk planner"); + + this->sub_map = this->create_subscription( + sub_map_topic_, 10, std::bind(&RandomWalkNode::mapCallback, this, std::placeholders::_1)); + this->sub_robot_tf = this->create_subscription( + sub_robot_tf_topic_, 10, + std::bind(&RandomWalkNode::tfCallback, this, std::placeholders::_1)); + + this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); + this->pub_goal_point = + this->create_publisher(pub_goal_point_viz_topic_, 10); + this->pub_trajectory_lines = + this->create_publisher(pub_trajectory_viz_topic_, 10); + + // Set up the timer + this->timer = this->create_wall_timer(std::chrono::seconds(1), + std::bind(&RandomWalkNode::timerCallback, this)); + // Set up the service + this->srv_random_walk_toggle = this->create_service( + srv_random_walk_toggle_topic_, std::bind(&RandomWalkNode::randomWalkToggleCallback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Random walk node initialized"); +} + +void RandomWalkNode::randomWalkToggleCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) { + if (this->enable_random_walk == false) { + this->enable_random_walk = true; + response->success = true; + response->message = "Random walk enabled"; + RCLCPP_INFO(this->get_logger(), "Random walk enabled"); + } else { + this->enable_random_walk = false; + response->success = true; + response->message = "Random walk disabled"; + RCLCPP_INFO(this->get_logger(), "Random walk disabled"); + } +} + +void RandomWalkNode::mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg) { + // updating the local voxel points and generating a path only if the path is not executing + if (!this->received_first_map) { + this->received_first_map = true; + RCLCPP_INFO(this->get_logger(), "Received first map"); + this->params.voxel_size_m = + std::tuple(msg->scale.x, msg->scale.y, msg->scale.z); + this->random_walk_planner = RandomWalkPlanner(this->params); + RCLCPP_INFO(this->get_logger(), "Initialized random walk planner logic"); + } + this->random_walk_planner.voxel_points.clear(); + for (int i = 0; i < msg->points.size(); i++) { + this->random_walk_planner.voxel_points.push_back( + std::tuple(msg->points[i].x, msg->points[i].y, msg->points[i].z)); + } +} + +void RandomWalkNode::tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { + // get the current location + for (int i = 0; i < msg->transforms.size(); i++) { + if (msg->transforms[i].child_frame_id.c_str() == this->robot_frame_id_) { + this->current_location = msg->transforms[i].transform; + if (!this->received_first_robot_tf) { + this->received_first_robot_tf = true; + RCLCPP_INFO(this->get_logger(), "Received first robot_tf"); + } + } + } +} + +void RandomWalkNode::generate_plan() { + RCLCPP_INFO(this->get_logger(), "Starting to generate plan..."); + + std::tuple start_loc; + if (this->generated_paths.size() == 0) { + geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + q.normalize(); + double roll, pitch, yaw; + tf2::Matrix3x3 m(q); + m.getRPY(roll, pitch, yaw); + start_loc = std::make_tuple(this->current_location.translation.x, + this->current_location.translation.y, + this->current_location.translation.z, yaw); + } else { + geometry_msgs::msg::Quaternion orientation = + this->generated_paths.back().poses.back().pose.orientation; + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + q.normalize(); + double roll, pitch, yaw; + tf2::Matrix3x3 m(q); + m.getRPY(roll, pitch, yaw); + start_loc = std::make_tuple(this->generated_paths.back().poses.back().pose.position.x, + this->generated_paths.back().poses.back().pose.position.y, + this->generated_paths.back().poses.back().pose.position.z, yaw); + } + + float timeout_duration = 5.0; + std::optional gen_path_opt = + this->random_walk_planner.generate_straight_rand_path(start_loc, timeout_duration); + if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) { + RCLCPP_INFO(this->get_logger(), "Generated path with %ld points", + gen_path_opt.value().size()); + + // set the current goal location + this->current_goal_location = geometry_msgs::msg::Transform(); + this->current_goal_location.translation.x = std::get<0>(gen_path_opt.value().back()); + this->current_goal_location.translation.y = std::get<1>(gen_path_opt.value().back()); + this->current_goal_location.translation.z = std::get<2>(gen_path_opt.value().back()); + float z_rot = std::get<3>(gen_path_opt.value().back()); + tf2::Quaternion q; + q.setRPY(0, 0, z_rot); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + this->current_goal_location.rotation.x = q.x(); + this->current_goal_location.rotation.y = q.y(); + this->current_goal_location.rotation.z = q.z(); + this->current_goal_location.rotation.w = q.w(); + + // publish the path + nav_msgs::msg::Path generated_single_path; + generated_single_path = nav_msgs::msg::Path(); + generated_single_path.header.stamp = this->now(); + generated_single_path.header.frame_id = world_frame_id_; + for (auto point : gen_path_opt.value()) { + geometry_msgs::msg::PoseStamped point_msg; + point_msg.pose.position.x = std::get<0>(point); + point_msg.pose.position.y = std::get<1>(point); + point_msg.pose.position.z = std::get<2>(point); + // convert yaw rotation to quaternion + tf2::Quaternion q; + q.setRPY(0, 0, std::get<3>(point)); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + point_msg.pose.orientation.x = q.x(); + point_msg.pose.orientation.y = q.y(); + point_msg.pose.orientation.z = q.z(); + point_msg.pose.orientation.w = q.w(); + point_msg.header.stamp = this->now(); + generated_single_path.poses.push_back(point_msg); + } + // this->pub_global_path->publish(this->generated_path); + if (this->publish_visualizations) { + this->pub_goal_point->publish(createGoalPointMarker()); + this->pub_trajectory_lines->publish(createTrajectoryLineMarker()); + } + // RCLCPP_INFO(this->get_logger(), "Published path and goal point at %f, %f, %f", + // this->generated_path.poses.back().pose.position.x, + // this->generated_path.poses.back().pose.position.y, + // this->generated_path.poses.back().pose.position.z); + geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); + this->current_goal_location.translation.x = last_goal_loc.pose.position.x; + this->current_goal_location.translation.y = last_goal_loc.pose.position.y; + this->current_goal_location.translation.z = last_goal_loc.pose.position.z; + this->current_goal_location.rotation.z = last_goal_loc.pose.orientation.z; + this->generated_paths.push_back(generated_single_path); + + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); + } +} + +void RandomWalkNode::publish_plan() { + nav_msgs::msg::Path full_path; + for (auto path : this->generated_paths) { + for (auto point : path.poses) { + full_path.poses.push_back(point); + } + } + full_path.header.stamp = this->now(); + full_path.header.frame_id = this->world_frame_id_; + this->pub_global_plan->publish(full_path); + RCLCPP_INFO(this->get_logger(), "Published full path"); +} +void RandomWalkNode::timerCallback() { + if (this->enable_random_walk && !this->is_path_executing) { + if (this->received_first_map && this->received_first_robot_tf) { + for (int i = 0; i < this->num_paths_to_generate_; i++) { + this->generate_plan(); + } + this->publish_plan(); + this->is_path_executing = true; + } else { + RCLCPP_INFO(this->get_logger(), "Waiting for map and robot tf to be received..."); + } + } else if (this->enable_random_walk && this->is_path_executing) { + std::tuple current_point = std::make_tuple( + this->current_location.translation.x, this->current_location.translation.y, + this->current_location.translation.z); + std::tuple goal_point = std::make_tuple( + this->current_goal_location.translation.x, this->current_goal_location.translation.y, + this->current_goal_location.translation.z); + if (get_point_distance(current_point, goal_point) < + this->random_walk_planner.path_end_threshold_m) { + this->is_path_executing = false; + this->generated_paths.clear(); + RCLCPP_INFO(this->get_logger(), "Reached goal point"); + } + } + // if (this->publish_visualizations && this->generated_path.poses.size() > 0) { + // visualization_msgs::msg::Marker goal_point_msg = createGoalPointMarker(); + // this->pub_goal_point->publish(goal_point_msg); + // visualization_msgs::msg::Marker trajectory_line_msg = createTrajectoryLineMarker(); + // this->pub_trajectory_lines->publish(trajectory_line_msg); + // RCLCPP_INFO(this->get_logger(), "Published goal point and trajectory line"); + // } +} + visualization_msgs::msg::Marker RandomWalkNode::createTrajectoryLineMarker() { visualization_msgs::msg::Marker trajectory_line_msg; trajectory_line_msg.header.frame_id = this->world_frame_id_; @@ -209,18 +315,15 @@ visualization_msgs::msg::Marker RandomWalkNode::createTrajectoryLineMarker() { trajectory_line_msg.color.r = 1.0; trajectory_line_msg.color.g = 0.0; trajectory_line_msg.color.b = 0.0; - if (this->generated_path.waypoints.size() > 0) { - // RCLCPP_INFO(this->get_logger(), "Creating trajectory line with %d points...", - // this->generated_path.waypoints.size()); - for (auto point : this->generated_path.waypoints) { + + for (auto path : this->generated_paths) { + for (auto point : path.poses) { geometry_msgs::msg::Point p; - p.x = point.position.x; - p.y = point.position.y; - p.z = point.position.z; + p.x = point.pose.position.x; + p.y = point.pose.position.y; + p.z = point.pose.position.z; trajectory_line_msg.points.push_back(p); } - } else { - RCLCPP_ERROR(this->get_logger(), "No points in the path"); } return trajectory_line_msg; } @@ -234,9 +337,9 @@ visualization_msgs::msg::Marker RandomWalkNode::createGoalPointMarker() { goal_point_msg.type = visualization_msgs::msg::Marker::SPHERE; goal_point_msg.action = visualization_msgs::msg::Marker::ADD; geometry_msgs::msg::Point goal_point = geometry_msgs::msg::Point(); - goal_point.x = std::get<0>(this->current_goal_location); - goal_point.y = std::get<1>(this->current_goal_location); - goal_point.z = std::get<2>(this->current_goal_location); + goal_point.x = this->current_goal_location.translation.x; + goal_point.y = this->current_goal_location.translation.y; + goal_point.z = this->current_goal_location.translation.z; goal_point_msg.pose.position = goal_point; goal_point_msg.scale.x = 0.1; goal_point_msg.scale.y = 0.1; diff --git a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp b/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp deleted file mode 100644 index 620e7e64..00000000 --- a/ros_ws/src/robot/autonomy/4_global/b_planners/trivial_planners/src/random_walk_planner_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -int main(int argc, char ** argv) -{ - (void) argc; - (void) argv; - - printf("hello world trivial_planners package\n"); - return 0; -} diff --git a/ros_ws/src/robot/autonomy/4_global/global_bringup/launch/global.launch.xml b/ros_ws/src/robot/autonomy/4_global/global_bringup/launch/global.launch.xml index 6ab58c42..ffae4718 100644 --- a/ros_ws/src/robot/autonomy/4_global/global_bringup/launch/global.launch.xml +++ b/ros_ws/src/robot/autonomy/4_global/global_bringup/launch/global.launch.xml @@ -4,9 +4,15 @@ + - - + + + + + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml b/ros_ws/src/robot/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml index f9d192fa..5ef5a0b4 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml @@ -2,28 +2,31 @@ - - + - + - - - - + + + + + diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/CMakeLists.txt b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/CMakeLists.txt index c67cae6f..c3bba336 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/CMakeLists.txt +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/CMakeLists.txt @@ -8,7 +8,10 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(behavior_tree REQUIRED) find_package(behavior_tree_msgs REQUIRED) find_package(airstack_msgs REQUIRED) @@ -20,9 +23,12 @@ target_include_directories(behavior_executive PUBLIC $ $) target_compile_features(behavior_executive PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - behavior_executive +ament_target_dependencies(behavior_executive + rclcpp + rclcpp_action std_msgs + std_srvs + nav_msgs behavior_tree behavior_tree_msgs airstack_msgs diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp index 6da18b57..99ef14c2 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp @@ -3,9 +3,13 @@ #include #include #include +#include #include +#include #include +#include "rclcpp_action/rclcpp_action.hpp" + class BehaviorExecutive : public rclcpp::Node { private: // parameters @@ -21,7 +25,7 @@ class BehaviorExecutive : public rclcpp::Node { bt::Condition* pause_commanded_condition; bt::Condition* rewind_commanded_condition; bt::Condition* fixed_trajectory_condition; - bt::Condition* explore_condition; + bt::Condition* global_plan_condition; bt::Condition* offboard_commanded_condition; bt::Condition* arm_commanded_condition; bt::Condition* disarm_commanded_condition; @@ -34,7 +38,7 @@ class BehaviorExecutive : public rclcpp::Node { bt::Action* pause_action; bt::Action* rewind_action; bt::Action* follow_fixed_trajectory_action; - bt::Action* explore_action; + bt::Action* global_plan_action; bt::Action* request_control_action; bt::Action* disarm_action; std::vector actions; @@ -44,7 +48,6 @@ class BehaviorExecutive : public rclcpp::Node { rclcpp::Subscription::SharedPtr is_armed_sub; rclcpp::Subscription::SharedPtr has_control_sub; - // publishers // services @@ -52,6 +55,7 @@ class BehaviorExecutive : public rclcpp::Node { rclcpp::Client::SharedPtr robot_command_client; rclcpp::Client::SharedPtr trajectory_mode_client; rclcpp::Client::SharedPtr takeoff_landing_command_client; + rclcpp::Client::SharedPtr global_planner_toggle_client; // timers rclcpp::TimerBase::SharedPtr timer; @@ -62,7 +66,8 @@ class BehaviorExecutive : public rclcpp::Node { void is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg); void has_control_callback(const std_msgs::msg::Bool::SharedPtr msg); -public: + + public: BehaviorExecutive(); }; diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/package.xml b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/package.xml index e66e7f81..8633fd1c 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/package.xml +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/package.xml @@ -10,7 +10,10 @@ ament_cmake rclcpp + rclcpp_action std_msgs + std_srvs + nav_msgs behavior_tree behavior_tree_msgs airstack_msgs diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp index 1f3b7b87..9d6bdfd9 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp @@ -11,7 +11,7 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { pause_commanded_condition = new bt::Condition("Pause Commanded", this); rewind_commanded_condition = new bt::Condition("Rewind Commanded", this); fixed_trajectory_condition = new bt::Condition("Fixed Trajectory Commanded", this); - explore_condition = new bt::Condition("Explore Commanded", this); + global_plan_condition = new bt::Condition("Global Plan Commanded", this); offboard_commanded_condition = new bt::Condition("Offboard Commanded", this); arm_commanded_condition = new bt::Condition("Arm Commanded", this); disarm_commanded_condition = new bt::Condition("Disarm Commanded", this); @@ -24,7 +24,7 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { conditions.push_back(pause_commanded_condition); conditions.push_back(rewind_commanded_condition); conditions.push_back(fixed_trajectory_condition); - conditions.push_back(explore_condition); + conditions.push_back(global_plan_condition); conditions.push_back(offboard_commanded_condition); conditions.push_back(arm_commanded_condition); conditions.push_back(disarm_commanded_condition); @@ -36,7 +36,7 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { pause_action = new bt::Action("Pause", this); rewind_action = new bt::Action("Rewind", this); follow_fixed_trajectory_action = new bt::Action("Follow Fixed Trajectory", this); - explore_action = new bt::Action("Explore", this); + global_plan_action = new bt::Action("Follow Global Plan", this); request_control_action = new bt::Action("Request Control", this); disarm_action = new bt::Action("Disarm", this); actions.push_back(arm_action); @@ -45,7 +45,7 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { actions.push_back(pause_action); actions.push_back(rewind_action); actions.push_back(follow_fixed_trajectory_action); - actions.push_back(explore_action); + actions.push_back(global_plan_action); actions.push_back(request_control_action); actions.push_back(disarm_action); @@ -72,6 +72,8 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { "set_trajectory_mode", rmw_qos_profile_services_default, service_callback_group); takeoff_landing_command_client = this->create_client( "set_takeoff_landing_command", rmw_qos_profile_services_default, service_callback_group); + global_planner_toggle_client = this->create_client( + "global_plan_toggle", rmw_qos_profile_services_default, service_callback_group); // timers timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), @@ -247,6 +249,29 @@ void BehaviorExecutive::timer_callback() { } } + if (global_plan_action->is_active()) { + global_plan_action->set_running(); + if (global_plan_action->active_has_changed()) { + // put trajectory controller in ADD_SEGMENT mode + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + std::cout << "mode 1" << std::endl; + mode_result.wait(); + std::cout << "mode 2" << std::endl; + + std_srvs::srv::Trigger::Request::SharedPtr request = + std::make_shared(); + auto result = global_planner_toggle_client->async_send_request(request); + result.wait(); + if (result.get()->success) + global_plan_action->set_success(); + else + global_plan_action->set_failure(); + }; + } + for (bt::Condition* condition : conditions) condition->publish(); for (bt::Action* action : actions) action->publish(); } diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_tree/config/drone.tree b/ros_ws/src/robot/autonomy/5_behavior/behavior_tree/config/drone.tree index 3069add3..9e461748 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_tree/config/drone.tree +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_tree/config/drone.tree @@ -34,9 +34,9 @@ ? [Follow Fixed Trajectory] -> - (Explore Commanded) + (Global Plan Commanded) ? - [Explore] + [Follow Global Plan] -> (Offboard Commanded) ? diff --git a/ros_ws/src/robot/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml b/ros_ws/src/robot/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml index ab71657b..fb5ba46a 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml +++ b/ros_ws/src/robot/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml @@ -1,21 +1,23 @@ - + - - + + - + - + - - + + \ No newline at end of file diff --git a/ros_ws/src/robot/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml b/ros_ws/src/robot/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml index 3c8ab707..456dfaa5 100644 --- a/ros_ws/src/robot/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml +++ b/ros_ws/src/robot/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml @@ -1,14 +1,16 @@ groups: - Commands: - - Takeoff: + - Arm and Takeoff: condition_name: Auto Takeoff Commanded - - Land: - condition_name: Land Commanded + - Fixed Trajectory: + condition_name: Fixed Trajectory Commanded + - Global Plan: + condition_name: Global Plan Commanded - Pause: condition_name: Pause Commanded - Rewind: condition_name: Rewind Commanded - - Fixed Trajectory: - condition_name: Fixed Trajectory Commanded - Disarm: - condition_name: Disarm Commanded \ No newline at end of file + condition_name: Disarm Commanded + - Land: + condition_name: Land Commanded \ No newline at end of file diff --git a/ros_ws/src/robot/robot_bringup/config/core.rviz b/ros_ws/src/robot/robot_bringup/config/core.rviz deleted file mode 100644 index b3620767..00000000 --- a/ros_ws/src/robot/robot_bringup/config/core.rviz +++ /dev/null @@ -1,452 +0,0 @@ ---- -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Grid1 - - /TF1/Frames1 - - /Odometry1/Topic1 - - /ExpansionPoly1 - - /Left Depth1 - Splitter Ratio: 0.35953420400619507 - Tree Height: 1085 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Expansion Cloud -Visualization Manager: - Class: '' - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - 'Y': 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - base_link_frd: - Value: false - base_link_stabilized: - Value: false - front_stereo: - Value: false - left_camera: - Value: true - look_ahead_point: - Value: false - look_ahead_point_stabilized: - Value: false - map: - Value: true - map_FLU: - Value: false - map_ned: - Value: false - odom: - Value: false - odom_ned: - Value: false - ouster: - Value: false - right_camera: - Value: true - tracking_point: - Value: true - tracking_point_stabilized: - Value: false - world: - Value: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - map_FLU: - map: - base_link: - base_link_frd: {} - front_stereo: - left_camera: {} - right_camera: {} - ouster: {} - base_link_stabilized: {} - look_ahead_point: {} - look_ahead_point_stabilized: {} - map_ned: {} - tracking_point: {} - tracking_point_stabilized: {} - Update Interval: 0 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Traj Vis - Namespaces: - trajectory_0: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: trajectory_controller/trajectory_vis - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Traj Debug - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: trajectory_controller/trajectory_controller_debug_markers - Value: false - - Angle Tolerance: 0 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Keep: 1 - Name: Odometry - Position Tolerance: 0 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: odometry_conversion/odometry - Value: false - - Class: rviz_default_plugins/Marker - Enabled: true - Name: VDB Mapping Marker - Namespaces: - '': true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: vdb_mapping/vdb_map_visualization - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 6.571824073791504 - Min Value: -0.5682187080383301 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 170; 170; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Lidar - Position Transformer: XYZ - Selectable: true - Size (Pixels): 1 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/ouster/point_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 100 - Median window: 5 - Min Value: 0 - Name: Front Left Depth - Normalize Range: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/depth - Value: false - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Front Left RGB - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/image_rect - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Traj Library - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/trajectory_library_vis - 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/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 220 - Min Color: 0; 0; 0 - Min Intensity: 120 - Name: Expansion Cloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/expansion_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Trimmed Global Plan for DROAN - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/local_planner_global_plan_vis - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ExpansionPoly - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/expansion_poly - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DisparityMarker - Namespaces: {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: droan/disparity_marker - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Left Camera - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/image_rect - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 100 - Median window: 5 - Min Value: 0 - Name: Left Depth - Normalize Range: false - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/depth - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 43.695953369140625 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -1.6335676908493042 - 'Y': -2.40643310546875 - Z: -2.289210796356201 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6753983497619629 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.6135711669921875 - Saved: null -Window Geometry: - Displays: - collapsed: false - Front Left Depth: - collapsed: false - Front Left RGB: - collapsed: false - Height: 1376 - Hide Left Dock: false - Hide Right Dock: false - Left Camera: - collapsed: false - Left Depth: - collapsed: false - QMainWindow State: >- - 000000ff00000000fd0000000400000000000001f5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c900000000000000000000000100000201000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000002ee0000002800fffffffb00000014004c006500660074002000440065007000740068010000032f000001d20000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042000000003b000001360000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068000000003b0000026b0000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b8000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2490 - X: 1990 - 'Y': 27 \ No newline at end of file diff --git a/ros_ws/src/robot/robot_bringup/launch/robot.launch.xml b/ros_ws/src/robot/robot_bringup/launch/robot.launch.xml index 650b01b5..76d16918 100644 --- a/ros_ws/src/robot/robot_bringup/launch/robot.launch.xml +++ b/ros_ws/src/robot/robot_bringup/launch/robot.launch.xml @@ -16,9 +16,9 @@ - + \ No newline at end of file diff --git a/ros_ws/src/robot/robot_bringup/rviz/robot.rviz b/ros_ws/src/robot/robot_bringup/rviz/robot.rviz index 2241045f..93da615d 100644 --- a/ros_ws/src/robot/robot_bringup/rviz/robot.rviz +++ b/ros_ws/src/robot/robot_bringup/rviz/robot.rviz @@ -4,11 +4,14 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 - - /DisparityExpansion1 - Splitter Ratio: 0.5 - Tree Height: 385 + - /TF1/Frames1 + - /Sensors1 + - /Local1 + - /Local1/DROAN1 + - /Local1/Trajectory Controller1 + - /Global1 + Splitter Ratio: 0.590062141418457 + Tree Height: 1085 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -26,7 +29,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: LaserScan + SyncSource: Expansion Cloud Visualization Manager: Class: "" Displays: @@ -45,215 +48,382 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 100 Reference Frame: Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Marker - Namespaces: - "": true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot1/vdb_mapping/vdb_map_visualization - Value: true - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: - All Enabled: true - Velodyne_VLS_128: + All Enabled: false + American_Beech: + Value: true + Plane: Value: true - camera1: + base_link: Value: true + base_link_frd: + Value: false + base_link_stabilized: + Value: false + front_stereo: + Value: false + left_camera: + Value: true + look_ahead_point: + Value: false + look_ahead_point_stabilized: + Value: false map: Value: true - robot_1_base_link: + map_FLU: + Value: false + map_ned: + Value: false + odom: + Value: false + odom_ned: + Value: false + ouster: + Value: false + right_camera: Value: true - robot_1_origin: + tracking_point: Value: true + tracking_point_stabilized: + Value: false world: - Value: true + Value: false Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: world: - map: + American_Beech: {} - robot_1_base_link: - Velodyne_VLS_128: - {} - camera1: - {} - robot_1_origin: + Plane: {} + map_FLU: + map: + base_link: + base_link_frd: + {} + front_stereo: + left_camera: + {} + right_camera: + {} + ouster: + {} + base_link_stabilized: + {} + look_ahead_point: + {} + look_ahead_point_stabilized: + {} + map_ned: + {} + tracking_point: + {} + tracking_point_stabilized: + {} 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 - Median window: 5 - Min Value: 0 - Name: Depth - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot1/perception/depth - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot1/perception/rgb - Value: true - - 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/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Left RGB + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/image_rect + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Front Left Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/depth + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.571824073791504 + Min Value: -0.5682187080383301 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 170; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Lidar + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/ouster/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: odometry_conversion/odometry + Value: false Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 220 - Min Color: 0; 0; 0 - Min Intensity: 120 - Name: DisparityExpansion - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /expansion/cloud_fg - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/Image + Name: Sensors + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Disparity Frustum + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/frustum + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Map Collision Checking + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_map_debug + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Graph Poses + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_graph + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Trimmed Global Plan for DROAN + Namespaces: + global_plan: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/local_planner_global_plan_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ExpansionPoly + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_poly + 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/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 220 + Min Color: 0; 0; 0 + Min Intensity: 120 + Name: Expansion Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Library + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/trajectory_library_vis + Value: false + Enabled: true + Name: DROAN + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Vis + Namespaces: + traj_controller: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Debug + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_controller_debug_markers + Value: false + Enabled: true + Name: Trajectory Controller 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 + Name: Local + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: VDB Mapping Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: vdb_mapping/vdb_map_visualization + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/global_plan + Value: true 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 + Name: Global Enabled: true Global Options: Background Color: 48; 48; 48 @@ -300,37 +470,37 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 18.20482635498047 + Distance: 14.309243202209473 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.965320885181427 - Y: 1.274168610572815 - Z: 2.3382456302642822 - Focal Shape Fixed Size: true + X: 3.3486995697021484 + Y: -0.9512473344802856 + Z: 1.4642823934555054 + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.40979740023612976 + Pitch: 0.47539615631103516 Target Frame: Value: Orbit (rviz) - Yaw: 0.9285785555839539 + Yaw: 2.9035723209381104 Saved: ~ Window Geometry: - Depth: - collapsed: false Displays: collapsed: false - Height: 1043 + Front Left Depth: + collapsed: false + Front Left RGB: + collapsed: false + Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000018a0000050afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000018c000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006801000001cd000000910000002800fffffffb0000000600520047004201000002640000014b0000002800fffffffb000000260065007800700061006e0064005f006400690073007000610072006900740079005f0066006701000003b5000000b20000002800fffffffb0000002a0065007800700061006e006400650064005f006400690073007000610072006900740079005f00620067010000046d000000d80000002800ffffff000000010000015f00000506fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000506000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000082a0000050a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c9000000000000000000000001000001f6000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042010000003b0000020e0000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068010000024f000002b20000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005d3000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -340,9 +510,5 @@ Window Geometry: Views: collapsed: false Width: 2490 - X: 1150 + X: 1990 Y: 27 - expand_disparity_fg: - collapsed: false - expanded_disparity_bg: - collapsed: false diff --git a/ros_ws/src/simulation/simulation_bringup/launch/simulation.launch.xml b/ros_ws/src/simulation/simulation_bringup/launch/simulation.launch.xml index 8f35d374..33cd020f 100644 --- a/ros_ws/src/simulation/simulation_bringup/launch/simulation.launch.xml +++ b/ros_ws/src/simulation/simulation_bringup/launch/simulation.launch.xml @@ -5,7 +5,7 @@ - +